initial commit. includes PhsyicsBox2dExtension
This commit is contained in:
warren powers
231
AndEngine/jni/Box2D/Dynamics/Contacts/b2TOISolver.cpp
Normal file
231
AndEngine/jni/Box2D/Dynamics/Contacts/b2TOISolver.cpp
Normal file
@@ -0,0 +1,231 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2010 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 "Box2D/Dynamics/Contacts/b2TOISolver.h"
|
||||
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||
#include "Box2D/Dynamics/b2Body.h"
|
||||
#include "Box2D/Dynamics/b2Fixture.h"
|
||||
#include "Box2D/Common/b2StackAllocator.h"
|
||||
|
||||
struct b2TOIConstraint
|
||||
{
|
||||
b2Vec2 localPoints[b2_maxManifoldPoints];
|
||||
b2Vec2 localNormal;
|
||||
b2Vec2 localPoint;
|
||||
b2Manifold::Type type;
|
||||
float32 radius;
|
||||
int32 pointCount;
|
||||
b2Body* bodyA;
|
||||
b2Body* bodyB;
|
||||
};
|
||||
|
||||
b2TOISolver::b2TOISolver(b2StackAllocator* allocator)
|
||||
{
|
||||
m_allocator = allocator;
|
||||
m_constraints = NULL;
|
||||
m_count = NULL;
|
||||
m_toiBody = NULL;
|
||||
}
|
||||
|
||||
b2TOISolver::~b2TOISolver()
|
||||
{
|
||||
Clear();
|
||||
}
|
||||
|
||||
void b2TOISolver::Clear()
|
||||
{
|
||||
if (m_allocator && m_constraints)
|
||||
{
|
||||
m_allocator->Free(m_constraints);
|
||||
m_constraints = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)
|
||||
{
|
||||
Clear();
|
||||
|
||||
m_count = count;
|
||||
m_toiBody = toiBody;
|
||||
|
||||
m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));
|
||||
|
||||
for (int32 i = 0; i < m_count; ++i)
|
||||
{
|
||||
b2Contact* contact = contacts[i];
|
||||
|
||||
b2Fixture* fixtureA = contact->GetFixtureA();
|
||||
b2Fixture* fixtureB = contact->GetFixtureB();
|
||||
b2Shape* shapeA = fixtureA->GetShape();
|
||||
b2Shape* shapeB = fixtureB->GetShape();
|
||||
float32 radiusA = shapeA->m_radius;
|
||||
float32 radiusB = shapeB->m_radius;
|
||||
b2Body* bodyA = fixtureA->GetBody();
|
||||
b2Body* bodyB = fixtureB->GetBody();
|
||||
b2Manifold* manifold = contact->GetManifold();
|
||||
|
||||
b2Assert(manifold->pointCount > 0);
|
||||
|
||||
b2TOIConstraint* constraint = m_constraints + i;
|
||||
constraint->bodyA = bodyA;
|
||||
constraint->bodyB = bodyB;
|
||||
constraint->localNormal = manifold->localNormal;
|
||||
constraint->localPoint = manifold->localPoint;
|
||||
constraint->type = manifold->type;
|
||||
constraint->pointCount = manifold->pointCount;
|
||||
constraint->radius = radiusA + radiusB;
|
||||
|
||||
for (int32 j = 0; j < constraint->pointCount; ++j)
|
||||
{
|
||||
b2ManifoldPoint* cp = manifold->points + j;
|
||||
constraint->localPoints[j] = cp->localPoint;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct b2TOISolverManifold
|
||||
{
|
||||
void Initialize(b2TOIConstraint* cc, int32 index)
|
||||
{
|
||||
b2Assert(cc->pointCount > 0);
|
||||
|
||||
switch (cc->type)
|
||||
{
|
||||
case b2Manifold::e_circles:
|
||||
{
|
||||
b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
|
||||
b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
|
||||
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
|
||||
{
|
||||
normal = pointB - pointA;
|
||||
normal.Normalize();
|
||||
}
|
||||
else
|
||||
{
|
||||
normal.Set(1.0f, 0.0f);
|
||||
}
|
||||
|
||||
point = 0.5f * (pointA + pointB);
|
||||
separation = b2Dot(pointB - pointA, normal) - cc->radius;
|
||||
}
|
||||
break;
|
||||
|
||||
case b2Manifold::e_faceA:
|
||||
{
|
||||
normal = cc->bodyA->GetWorldVector(cc->localNormal);
|
||||
b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
|
||||
|
||||
b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
|
||||
separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
|
||||
point = clipPoint;
|
||||
}
|
||||
break;
|
||||
|
||||
case b2Manifold::e_faceB:
|
||||
{
|
||||
normal = cc->bodyB->GetWorldVector(cc->localNormal);
|
||||
b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
|
||||
|
||||
b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
|
||||
separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
|
||||
point = clipPoint;
|
||||
|
||||
// Ensure normal points from A to B
|
||||
normal = -normal;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
b2Vec2 normal;
|
||||
b2Vec2 point;
|
||||
float32 separation;
|
||||
};
|
||||
|
||||
// Push out the toi body to provide clearance for further simulation.
|
||||
bool b2TOISolver::Solve(float32 baumgarte)
|
||||
{
|
||||
float32 minSeparation = 0.0f;
|
||||
|
||||
for (int32 i = 0; i < m_count; ++i)
|
||||
{
|
||||
b2TOIConstraint* c = m_constraints + i;
|
||||
b2Body* bodyA = c->bodyA;
|
||||
b2Body* bodyB = c->bodyB;
|
||||
|
||||
float32 massA = bodyA->m_mass;
|
||||
float32 massB = bodyB->m_mass;
|
||||
|
||||
// Only the TOI body should move.
|
||||
if (bodyA == m_toiBody)
|
||||
{
|
||||
massB = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
massA = 0.0f;
|
||||
}
|
||||
|
||||
float32 invMassA = massA * bodyA->m_invMass;
|
||||
float32 invIA = massA * bodyA->m_invI;
|
||||
float32 invMassB = massB * bodyB->m_invMass;
|
||||
float32 invIB = massB * bodyB->m_invI;
|
||||
|
||||
// Solve normal constraints
|
||||
for (int32 j = 0; j < c->pointCount; ++j)
|
||||
{
|
||||
b2TOISolverManifold psm;
|
||||
psm.Initialize(c, j);
|
||||
b2Vec2 normal = psm.normal;
|
||||
|
||||
b2Vec2 point = psm.point;
|
||||
float32 separation = psm.separation;
|
||||
|
||||
b2Vec2 rA = point - bodyA->m_sweep.c;
|
||||
b2Vec2 rB = point - bodyB->m_sweep.c;
|
||||
|
||||
// Track max constraint error.
|
||||
minSeparation = b2Min(minSeparation, separation);
|
||||
|
||||
// Prevent large corrections and allow slop.
|
||||
float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
|
||||
|
||||
// Compute the effective mass.
|
||||
float32 rnA = b2Cross(rA, normal);
|
||||
float32 rnB = b2Cross(rB, normal);
|
||||
float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
|
||||
|
||||
// Compute normal impulse
|
||||
float32 impulse = K > 0.0f ? - C / K : 0.0f;
|
||||
|
||||
b2Vec2 P = impulse * normal;
|
||||
|
||||
bodyA->m_sweep.c -= invMassA * P;
|
||||
bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
|
||||
bodyA->SynchronizeTransform();
|
||||
|
||||
bodyB->m_sweep.c += invMassB * P;
|
||||
bodyB->m_sweep.a += invIB * b2Cross(rB, P);
|
||||
bodyB->SynchronizeTransform();
|
||||
}
|
||||
}
|
||||
|
||||
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||
// push the separation above -b2_linearSlop.
|
||||
return minSeparation >= -1.5f * b2_linearSlop;
|
||||
}
|
||||
Reference in New Issue
Block a user