Added Box2D

external/Box2D-2.3.1/Contributions/Enhancements/Controllers/b2BuoyancyController.cpp

@@ -0,0 +1,119 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2BuoyancyController.h"
+#include "../b2Fixture.h"
+
+b2BuoyancyController::b2BuoyancyController(const b2BuoyancyControllerDef* def) : b2Controller(def)
+{
+	normal = def->normal;
+	offset = def->offset;
+	density = def->density;
+	velocity = def->velocity;
+	linearDrag = def->linearDrag;
+	angularDrag = def->angularDrag;
+	useDensity = def->useDensity;
+	useWorldGravity = def->useWorldGravity;
+	gravity = def->gravity;
+}
+
+void b2BuoyancyController::Step(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+	if(!m_bodyList)
+		return;
+	if(useWorldGravity)
+	{
+		gravity = m_world->GetGravity();
+	}
+	for(b2ControllerEdge *i=m_bodyList;i;i=i->nextBody)
+	{
+		b2Body* body = i->body;
+		if(body->IsSleeping())
+		{
+			//Buoyancy force is just a function of position,
+			//so unlike most forces, it is safe to ignore sleeping bodes
+			continue;
+		}
+		b2Vec2 areac(0,0);
+		b2Vec2 massc(0,0);
+		float32 area = 0;
+		float32 mass = 0;
+		for(b2Fixture* shape=body->GetFixtureList();shape;shape=shape->GetNext())
+		{
+			b2Vec2 sc(0,0);
+			float32 sarea = shape->ComputeSubmergedArea(normal, offset, &sc);
+			area += sarea;
+			areac.x += sarea * sc.x;
+			areac.y += sarea * sc.y;
+			float shapeDensity = 0;
+			if(useDensity)
+			{
+				//TODO: Expose density publicly
+				shapeDensity=shape->GetDensity();
+			}
+			else
+			{
+				shapeDensity = 1;
+			}
+			mass += sarea*shapeDensity;
+			massc.x += sarea * sc.x * shapeDensity;
+			massc.y += sarea * sc.y * shapeDensity;
+		}
+		areac.x/=area;
+		areac.y/=area;
+		b2Vec2 localCentroid = b2MulT(body->GetXForm(),areac);
+		massc.x/=mass;
+		massc.y/=mass;
+		if(area<B2_FLT_EPSILON)
+			continue;
+		//Buoyancy
+		b2Vec2 buoyancyForce = -density*area*gravity;
+		body->ApplyForce(buoyancyForce,massc);
+		//Linear drag
+		b2Vec2 dragForce = body->GetLinearVelocityFromWorldPoint(areac) - velocity;
+		dragForce *= -linearDrag*area;
+		body->ApplyForce(dragForce,areac);
+		//Angular drag
+		//TODO: Something that makes more physical sense?
+		body->ApplyTorque(-body->GetInertia()/body->GetMass()*area*body->GetAngularVelocity()*angularDrag);
+		
+	}
+}
+
+void b2BuoyancyController::Draw(b2DebugDraw *debugDraw)
+{
+	float32 r = 1000;
+	b2Vec2 p1 = offset * normal + b2Cross(normal, r);
+	b2Vec2 p2 = offset * normal - b2Cross(normal, r);
+
+	b2Color color(0,0,0.8f);
+
+	debugDraw->DrawSegment(p1, p2, color);
+}
+
+void b2BuoyancyController::Destroy(b2BlockAllocator* allocator)
+{
+	allocator->Free(this, sizeof(b2BuoyancyController));
+}
+
+b2BuoyancyController* b2BuoyancyControllerDef::Create(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2BuoyancyController));
+	return new (mem) b2BuoyancyController(this);
+}