static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
{
linear = -n;
angular0 = -b3Cross(r0, n);
angular1 = b3Cross(r1, n);
}
void b3SphericalJoint::SolveVelocityConstraint(const b3SolverData* data) {
u32 indexA = m_indexA;
u32 indexB = m_indexB;
r32 mA = m_mA;
r32 mB = m_mB;
b3Mat33 iA = m_iA;
b3Mat33 iB = m_iB;
b3Vec3 rA = m_rA;
b3Vec3 rB = m_rB;
b3Mat33 invMass = m_invMass;
b3Vec3 velocityBias = m_velocityBias;
b3Vec3 lastImpulse = m_accImpulse;
b3Vec3 vA = data->velocities[indexA].v;
b3Vec3 wA = data->velocities[indexA].w;
b3Vec3 vB = data->velocities[indexB].v;
b3Vec3 wB = data->velocities[indexB].w;
// Compute J * u.
b3Vec3 dCdt = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
// Compute the new, total, and delta impulses.
b3Vec3 impulse = invMass * (-dCdt + velocityBias);
b3Vec3 newImpulse = lastImpulse + impulse;
// b3Vec3 deltaImpulse = newImpulse - lastImpulse <=> impulse;
// Keep track of the accumulated impulse for the next iteration.
lastImpulse = newImpulse;
// Apply new impulse.
vA -= mA * impulse;
wA -= iA * b3Cross(rA, impulse);
vB += mB * impulse;
wB += iB * b3Cross(rB, impulse);
// Copy the new old impulse for the next iteration.
m_accImpulse = lastImpulse;
// Update velocity vector.
data->velocities[indexA].v = vA;
data->velocities[indexA].w = wA;
data->velocities[indexB].v = vB;
data->velocities[indexB].w = wB;
}
void b3SphericalJoint::WarmStart(const b3SolverData* data) {
u32 indexA = m_indexA;
u32 indexB = m_indexB;
r32 mA = m_mA;
r32 mB = m_mB;
b3Mat33 iA = m_iA;
b3Mat33 iB = m_iB;
b3Vec3 rA = m_rA;
b3Vec3 rB = m_rB;
b3Vec3 impulse = m_accImpulse;
data->velocities[indexA].v -= mA * impulse;
data->velocities[indexA].w -= iA * b3Cross(rA, impulse);
data->velocities[indexB].v += mB * impulse;
data->velocities[indexB].w += iB * b3Cross(rB, impulse);
}
// Box2D
static B3_FORCE_INLINE b3Vec3 b3SolveGyro(const b3Quat& q, const b3Mat33& Ib, const b3Vec3& w1, float32 h)
{
// Convert angular velocity to body coordinates
b3Vec3 w1b = b3MulT(q, w1);
// Jacobian of f
b3Mat33 J = Ib + h * (b3Skew(w1b) * Ib - b3Skew(Ib * w1b));
// One iteration of Newton-Raphson
// Residual vector
b3Vec3 f;
{
f = h * b3Cross(w1b, Ib * w1b);
w1b -= J.Solve(f);
}
// Convert angular velocity back to world coordinates
b3Vec3 w2 = b3Mul(q, w1b);
return w2;
}
void b3BuildEdgeContact(b3Manifold& manifold,
const b3Transform& xf1, u32 index1, const b3HullShape* s1,
const b3Transform& xf2, u32 index2, const b3HullShape* s2)
{
const b3Hull* hull1 = s1->m_hull;
const b3HalfEdge* edge1 = hull1->GetEdge(index1);
const b3HalfEdge* twin1 = hull1->GetEdge(index1 + 1);
b3Vec3 C1 = xf1 * hull1->centroid;
b3Vec3 P1 = xf1 * hull1->GetVertex(edge1->origin);
b3Vec3 Q1 = xf1 * hull1->GetVertex(twin1->origin);
b3Vec3 E1 = Q1 - P1;
b3Vec3 N1 = E1;
float32 L1 = N1.Normalize();
B3_ASSERT(L1 > B3_LINEAR_SLOP);
const b3Hull* hull2 = s2->m_hull;
const b3HalfEdge* edge2 = hull2->GetEdge(index2);
const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1);
b3Vec3 C2 = xf2 * hull2->centroid;
b3Vec3 P2 = xf2 * hull2->GetVertex(edge2->origin);
b3Vec3 Q2 = xf2 * hull2->GetVertex(twin2->origin);
b3Vec3 E2 = Q2 - P2;
b3Vec3 N2 = E2;
float32 L2 = N2.Normalize();
B3_ASSERT(L2 > B3_LINEAR_SLOP);
// Compute the closest points on the two lines.
float32 b = b3Dot(N1, N2);
float32 den = 1.0f - b * b;
if (den <= 0.0f)
{
return;
}
float32 inv_den = 1.0f / den;
b3Vec3 E3 = P1 - P2;
float32 d = b3Dot(N1, E3);
float32 e = b3Dot(N2, E3);
float32 s = inv_den * (b * e - d);
float32 t = inv_den * (e - b * d);
b3Vec3 c1 = P1 + s * N1;
b3Vec3 c2 = P2 + t * N2;
// Ensure normal orientation to hull 2.
b3Vec3 N = b3Cross(E1, E2);
float32 LN = N.Normalize();
B3_ASSERT(LN > 0.0f);
if (b3Dot(N, P1 - C1) < 0.0f)
{
N = -N;
}
b3FeaturePair pair = b3MakePair(index1, index1 + 1, index2, index2 + 1);
manifold.pointCount = 1;
manifold.points[0].localNormal1 = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint1 = b3MulT(xf1, c1);
manifold.points[0].localPoint2 = b3MulT(xf2, c2);
manifold.points[0].key = b3MakeKey(pair);
}
void b3ContactGraph::UpdateContacts() {
// Update all contact constraints and its states.
b3Contact* c = m_contactList;
while (c) {
const b3Shape* shapeA = c->m_shapeA;
const b3Shape* shapeB = c->m_shapeB;
b3Body* bodyA = c->m_shapeA->m_body;
b3Body* bodyB = c->m_shapeB->m_body;
if (bodyA == bodyB) {
b3Contact* quack = c;
c = c->m_next;
DestroyContact(quack);
continue;
}
bool activeA = bodyA->IsAwake() && (bodyA->m_type != e_staticBody);
bool activeB = bodyB->IsAwake() && (bodyB->m_type != e_staticBody);
if (!activeA && !activeB) {
c = c->m_next;
continue;
}
// Destroy the contact if is definately persistenting.
if (!m_broadPhase.TestOverlap(shapeA->broadPhaseID, shapeB->broadPhaseID)) {
b3Contact* quack = c;
c = c->m_next;
DestroyContact(quack);
continue;
}
bool wasTouching = c->IsTouching();
bool isTouching = false;
bool isSensorContact = shapeA->IsSensor() || shapeB->IsSensor();
if (isSensorContact) {
// Simply, a sensor is active if its bounds is touching with another other shape's bounds.
isTouching = m_broadPhase.TestOverlap(shapeA->broadPhaseID, shapeB->broadPhaseID);
c->m_manifold.pointCount = 0;
}
else {
// For the time being, Bounce can compute the closest distance between convex objects only.
typedef void(*b3DistanceQuery) (b3Manifold&, const b3Transform&, const b3Shape*, const b3Transform&, const b3Shape*);
// Simply, register the collision routines here.
static b3DistanceQuery queryMatrix[e_maxShapes][e_maxShapes] = {
{ &b3HullHullShapeContact },
};
b3ShapeType typeA = shapeA->GetType();
b3ShapeType typeB = shapeB->GetType();
b3Assert(typeA <= typeB);
b3DistanceQuery Query = queryMatrix[typeA][typeB];
b3Assert(Query);
// Copy the old manifold so we can compare the new contact points with it.
b3Manifold oldManifold = c->m_manifold;
// Compute the a new contact manifold.
c->m_manifold.pointCount = 0;
Query(c->m_manifold, bodyA->m_transform * shapeA->m_local, shapeA, bodyB->m_transform * shapeB->m_local, shapeB);
isTouching = c->m_manifold.pointCount > 0;
// Look up the contact cache for identical contact points.
b3Manifold* newManifold = &c->m_manifold;
b3Vec3 normal = newManifold->normal;
b3Vec3 xA = bodyA->m_worldCenter;
b3Vec3 vA = bodyA->m_linearVelocity;
b3Vec3 wA = bodyA->m_angularVelocity;
b3Vec3 xB = bodyB->m_worldCenter;
b3Vec3 vB = bodyB->m_linearVelocity;
b3Vec3 wB = bodyB->m_angularVelocity;
for (u32 i = 0; i < newManifold->pointCount; ++i) {
b3ContactPoint* p2 = newManifold->points + i;
b3Vec3 position = p2->position;
b3Vec3 tangent1;
b3Vec3 tangent2;
p2->normalImpulse = B3_ZERO;
p2->tangentImpulse[0] = B3_ZERO;
p2->tangentImpulse[1] = B3_ZERO;
p2->warmStarted = false;
// Compute the (two) new tangent directions.
b3Vec3 rA = position - xA;
b3Vec3 rB = position - xB;
b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
tangent1 = dv - b3Dot(dv, normal) * normal;
r32 tangentMag = b3Dot(tangent1, tangent1);
if (tangentMag > B3_EPSILON) {
tangent1 *= B3_ONE / b3Sqrt(tangentMag);
tangent2 = b3Cross(tangent1, normal);
}
else {
b3ComputeBasis(normal, &tangent1, &tangent2);
}
p2->tangents[0] = tangent1;
p2->tangents[1] = tangent2;
// Initialize new contact point with the old contact point solution if they're identical.
for (u32 j = 0; j < oldManifold.pointCount; ++j) {
b3ContactPoint* p1 = oldManifold.points + j;
if (p1->id.key == p2->id.key) {
// Copy normal impulse.
p2->warmStarted = true;
p2->normalImpulse = p1->normalImpulse;
// Project old friction solutions into the new tangential directions.
b3Vec3 oldFrictionSolution = p1->tangentImpulse[0] * p1->tangents[0] + p1->tangentImpulse[1] * p1->tangents[1];
p2->tangentImpulse[0] = b3Dot(oldFrictionSolution, p2->tangents[0]);
p2->tangentImpulse[1] = b3Dot(oldFrictionSolution, p2->tangents[1]);
break;
}
}
}
// If the contact has begun then awake the bodies.
if (isTouching != wasTouching) {
bodyA->SetAwake(true);
bodyB->SetAwake(true);
}
}
// Mark the contact as touching.
if (isTouching) {
c->m_flags |= b3Contact::e_touchingFlag;
}
else {
c->m_flags &= ~b3Contact::e_touchingFlag;
}
// Notify the contact listener the contact state.
if (m_contactListener) {
if (!wasTouching && isTouching) {
m_contactListener->BeginContact(c);
}
if (wasTouching && !isTouching) {
m_contactListener->EndContact(c);
}
if (!isSensorContact && isTouching) {
m_contactListener->Persisting(c);
}
}
// Go to the next contact.
c = c->m_next;
}
}
void b3Body::SetMassData(const b3MassData* massData)
{
if (m_type != e_dynamicBody)
{
return;
}
m_invMass = 0.0f;
m_I.SetZero();
m_invI.SetZero();
m_worldInvI.SetZero();
m_mass = massData->mass;
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
m_I = massData->I - m_mass * b3Steiner(massData->center);
B3_ASSERT(m_I.x.x > 0.0f);
B3_ASSERT(m_I.y.y > 0.0f);
B3_ASSERT(m_I.z.z > 0.0f);
m_invI = b3Inverse(m_I);
m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation);
if (m_flags & e_fixedRotationX)
{
m_invI.y.y = 0.0f;
m_invI.z.y = 0.0f;
m_invI.y.z = 0.0f;
m_invI.z.z = 0.0f;
m_worldInvI.y.y = 0.0f;
m_worldInvI.z.y = 0.0f;
m_worldInvI.y.z = 0.0f;
m_worldInvI.z.z = 0.0f;
}
if (m_flags & e_fixedRotationY)
{
m_invI.x.x = 0.0f;
m_invI.x.z = 0.0f;
m_invI.z.x = 0.0f;
m_invI.z.z = 0.0f;
m_worldInvI.x.x = 0.0f;
m_worldInvI.x.z = 0.0f;
m_worldInvI.z.x = 0.0f;
m_worldInvI.z.z = 0.0f;
}
if (m_flags & e_fixedRotationZ)
{
m_invI.x.x = 0.0f;
m_invI.x.y = 0.0f;
m_invI.y.x = 0.0f;
m_invI.y.y = 0.0f;
m_worldInvI.x.x = 0.0f;
m_worldInvI.x.y = 0.0f;
m_worldInvI.y.x = 0.0f;
m_worldInvI.y.y = 0.0f;
}
}
else
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
// Move center of mass.
b3Vec3 oldCenter = m_sweep.worldCenter;
m_sweep.localCenter = massData->center;
m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
m_sweep.worldCenter0 = m_sweep.worldCenter;
// Update center of mass velocity.
m_linearVelocity += b3Cross(m_angularVelocity, m_sweep.worldCenter - oldCenter);
}
void b3Body::ResetMass()
{
m_mass = 0.0f;
m_invMass = 0.0f;
m_I.SetZero();
m_invI.SetZero();
m_worldInvI.SetZero();
m_sweep.localCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == e_staticBody || m_type == e_kinematicBody)
{
m_sweep.worldCenter0 = m_xf.position;
m_sweep.worldCenter = m_xf.position;
m_sweep.orientation0 = m_sweep.orientation;
return;
}
// Accumulate the mass about the body origin of all shapes.
b3Vec3 localCenter;
localCenter.SetZero();
for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
{
if (s->m_density == 0.0f)
{
continue;
}
b3MassData massData;
s->ComputeMass(&massData, s->m_density);
localCenter += massData.mass * massData.center;
m_mass += massData.mass;
m_I += massData.I;
}
if (m_mass > 0.0f)
{
// Compute local center of mass.
m_invMass = 1.0f / m_mass;
localCenter *= m_invMass;
// Shift inertia about the body origin into the body local center of mass.
m_I = m_I - m_mass * b3Steiner(localCenter);
B3_ASSERT(m_I.x.x > 0.0f);
B3_ASSERT(m_I.y.y > 0.0f);
B3_ASSERT(m_I.z.z > 0.0f);
// Compute inverse inertia about the body local center of mass.
m_invI = b3Inverse(m_I);
// Align the inverse inertia with the world frame of the body.
m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation);
// Fix rotation.
if (m_flags & e_fixedRotationX)
{
m_invI.y.y = 0.0f;
m_invI.z.y = 0.0f;
m_invI.y.z = 0.0f;
m_invI.z.z = 0.0f;
m_worldInvI.y.y = 0.0f;
m_worldInvI.z.y = 0.0f;
m_worldInvI.y.z = 0.0f;
m_worldInvI.z.z = 0.0f;
}
if (m_flags & e_fixedRotationY)
{
m_invI.x.x = 0.0f;
m_invI.x.z = 0.0f;
m_invI.z.x = 0.0f;
m_invI.z.z = 0.0f;
m_worldInvI.x.x = 0.0f;
m_worldInvI.x.z = 0.0f;
m_worldInvI.z.x = 0.0f;
m_worldInvI.z.z = 0.0f;
}
if (m_flags & e_fixedRotationZ)
{
m_invI.x.x = 0.0f;
m_invI.x.y = 0.0f;
m_invI.y.x = 0.0f;
m_invI.y.y = 0.0f;
m_worldInvI.x.x = 0.0f;
m_worldInvI.x.y = 0.0f;
m_worldInvI.y.x = 0.0f;
m_worldInvI.y.y = 0.0f;
}
}
else
{
// Force all dynamic bodies to have positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
// Move center of mass.
b3Vec3 oldCenter = m_sweep.worldCenter;
m_sweep.localCenter = localCenter;
m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
m_sweep.worldCenter0 = m_sweep.worldCenter;
// Update center of mass velocity.
m_linearVelocity += b3Cross(m_angularVelocity, m_sweep.worldCenter - oldCenter);
}
