static
inline
float calcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
{
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
}
void resolveSingleConstraintRowGeneric2(b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3GpuSolverConstraint* c)
{
float deltaImpulse = c->m_rhs - b3Scalar(c->m_appliedImpulse) * c->m_cfm;
float deltaVel1Dotn = b3Dot(c->m_contactNormal, body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal, body1->m_deltaAngularVelocity);
float deltaVel2Dotn = -b3Dot(c->m_contactNormal, body2->m_deltaLinearVelocity) + b3Dot(c->m_relpos2CrossNormal, body2->m_deltaAngularVelocity);
deltaImpulse -= deltaVel1Dotn * c->m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c->m_jacDiagABInv;
float sum = b3Scalar(c->m_appliedImpulse) + deltaImpulse;
if (sum < c->m_lowerLimit)
{
deltaImpulse = c->m_lowerLimit - b3Scalar(c->m_appliedImpulse);
c->m_appliedImpulse = c->m_lowerLimit;
}
else if (sum > c->m_upperLimit)
{
deltaImpulse = c->m_upperLimit - b3Scalar(c->m_appliedImpulse);
c->m_appliedImpulse = c->m_upperLimit;
}
else
{
c->m_appliedImpulse = sum;
}
internalApplyImpulse(body1, c->m_contactNormal * body1->m_invMass, c->m_angularComponentA, deltaImpulse);
internalApplyImpulse(body2, -c->m_contactNormal * body2->m_invMass, c->m_angularComponentB, deltaImpulse);
}
float calcJacCoeff(const b3Vector3& linear0, const b3Vector3& linear1, const b3Vector3& angular0, const b3Vector3& angular1,
float invMass0, const b3Matrix3x3* invInertia0, float invMass1, const b3Matrix3x3* invInertia1, float countA, float countB)
{
// linear0,1 are normlized
float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;
float jmj1 = b3Dot(mtMul3(angular0,*invInertia0), angular0);
float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;
float jmj3 = b3Dot(mtMul3(angular1,*invInertia1), angular1);
return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB);
// return -1.f/((jmj0+jmj1)+(jmj2+jmj3));
}
bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronData& poly,
const b3AlignedObjectArray<b3GpuFace>& faces, float& hitFraction, b3Vector3& hitNormal)
{
float exitFraction = hitFraction;
float enterFraction = -0.1f;
b3Vector3 curHitNormal=b3MakeVector3(0,0,0);
for (int i=0;i<poly.m_numFaces;i++)
{
const b3GpuFace& face = faces[poly.m_faceOffset+i];
float fromPlaneDist = b3Dot(rayFromLocal,face.m_plane)+face.m_plane.w;
float toPlaneDist = b3Dot(rayToLocal,face.m_plane)+face.m_plane.w;
if (fromPlaneDist<0.f)
{
if (toPlaneDist >= 0.f)
{
float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
if (exitFraction>fraction)
{
exitFraction = fraction;
}
}
} else
{
if (toPlaneDist<0.f)
{
float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
if (enterFraction <= fraction)
{
enterFraction = fraction;
curHitNormal = face.m_plane;
curHitNormal.w = 0.f;
}
} else
{
return false;
}
}
if (exitFraction <= enterFraction)
return false;
}
if (enterFraction < 0.f)
return false;
hitFraction = enterFraction;
hitNormal = curHitNormal;
return true;
}
// Christer Ericson, Real-Time Collision Detection (2005), p. 198.
bool b3Polyhedron::RayCast(const b3RayCastInput& input, b3RayCastOutput& output, const b3Transform& transform) const {
b3Assert(m_hull);
b3Assert(m_hull->faceCount >= 3);
// Perform computations in the local space of the hull.
b3Vec3 a = b3MulT(transform.rotation, input.p1 - transform.translation);
b3Vec3 b = b3MulT(transform.rotation, input.p2 - transform.translation);
b3Vec3 direction = b - a;
r32 tfirst = B3_ZERO;
r32 tlast = input.maxFraction;
i32 index = -1;
// Test ray against each plane.
for (u32 i = 0; i < m_hull->faceCount; i++) {
const b3Plane& plane = m_hull->facesPlanes[i];
r32 numerator = -b3Distance(plane, a);
r32 denominator = b3Dot(plane.normal, direction);
if (denominator == B3_ZERO) {
// The ray is parallel to the plane.
if (numerator > B3_ZERO) {
// Then return false if ray segment lies in front (outside) of the plane.
return false;
}
}
else {
// Compute parameterized t value for intersection with current plane.
r32 t = numerator / denominator;
if (denominator < B3_ZERO) {
// When entering halfspace, update tfirst if t is larger.
if (t > tfirst) {
tfirst = t;
index = i;
}
}
else {
// When exiting halfspace, update tlast if t is smaller.
if (t < tlast) {
tlast = t;
}
}
// Exit with �no intersection� if intersection becomes empty.
if (tfirst > tlast) {
return false;
}
}
}
output.fraction = tfirst;
if (index != -1) {
output.normal = transform.rotation * m_hull->GetPlane(index).normal;
}
// If the ray is contained in all planes then it intersects.
return true;
}
bool sphere_intersect(const b3Vector3& spherePos, b3Scalar radius, const b3Vector3& rayFrom, const b3Vector3& rayTo)
{
// rs = ray.org - sphere.center
const b3Vector3& rs = rayFrom - spherePos;
b3Vector3 rayDir = rayTo-rayFrom;//rayFrom-rayTo;
rayDir.normalize();
float B = b3Dot(rs, rayDir);
float C = b3Dot(rs, rs) - (radius * radius);
float D = B * B - C;
if (D > 0.0)
{
float t = -B - sqrt(D);
if ( (t > 0.0))// && (t < isect.t) )
{
return true;//isect.t = t;
}
}
return false;
}
bool sphere_intersect(const b3Vector3& spherePos, b3Scalar radius, const b3Vector3& rayFrom, const b3Vector3& rayTo, float& hitFraction)
{
b3Vector3 rs = rayFrom - spherePos;
b3Vector3 rayDir = rayTo-rayFrom;
float A = b3Dot(rayDir,rayDir);
float B = b3Dot(rs, rayDir);
float C = b3Dot(rs, rs) - (radius * radius);
float D = B * B - A*C;
if (D > 0.0)
{
float t = (-B - sqrt(D))/A;
if ( (t >= 0.0f) && (t < hitFraction) )
{
hitFraction = t;
return true;
}
}
return false;
}
void ExplicitEuler::computeSpringForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt)
{
//add spring forces
for(int i=0;i<clothData->m_springs.size();i++)
{
int indexA = clothData->m_springs[i].m_particleIndexA;
int indexB = clothData->m_springs[i].m_particleIndexB;
float restLength = clothData->m_springs[i].m_restLength;
const ClothMaterial& mat = clothData->m_materials[clothData->m_springs[i].m_material];
const b3Vector3& posA = (const b3Vector3&)vertexPositions[indexA*vertexStride];
const b3Vector3& posB = (const b3Vector3&)vertexPositions[indexB*vertexStride];
const b3Vector3& velA = clothData->m_velocities[indexA];
const b3Vector3& velB = clothData->m_velocities[indexB];
b3Vector3 deltaP = posA-posB;
b3Vector3 deltaV = velA-velB;
float dist = deltaP.length();
b3Vector3 deltaPNormalized = deltaP/dist;
float spring = -mat.m_stiffness * (dist-restLength)*100000;
float damper = mat.m_damping * b3Dot(deltaV,deltaPNormalized)*100;
b3Vector3 springForce = (spring+damper)*deltaPNormalized;
float particleMassA = clothData->m_particleMasses[indexA];
float particleMassB = clothData->m_particleMasses[indexB];
//if (springForce.length())
{
if (particleMassA)
{
clothData->m_forces[indexA] += springForce*particleMassA;
}
if (particleMassB)
{
clothData->m_forces[indexB] -= springForce*particleMassB;
}
}
}
}
static
__inline
void solveFriction(b3GpuConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
b3Vector3 tangent[2];
#if 1
b3PlaneSpace1 (n, tangent[0],tangent[1]);
#else
b3Vector3 r = cs.m_worldPos[0]-center;
tangent[0] = cross3( n, r );
tangent[1] = cross3( tangent[0], n );
tangent[0] = normalize3( tangent[0] );
tangent[1] = normalize3( tangent[1] );
#endif
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = center - posA;
b3Vector3 r1 = center - posB;
for(int i=0; i<2; i++)
{
setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[i] );
updated = b3Min( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
{ // angular damping for point constraint
b3Vector3 ab = ( posB - posA ).normalized();
b3Vector3 ac = ( center - posA ).normalized();
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = b3Dot( n, angVelA );
float angNB = b3Dot( n, angVelB );
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
}
}
}
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 b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, u32 positionIterations, u32 flags)
{
float32 h = dt;
// 1. Integrate velocities
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b3Vec3 v = b->m_linearVelocity;
b3Vec3 w = b->m_angularVelocity;
b3Vec3 x = b->m_sweep.worldCenter;
b3Quat q = b->m_sweep.orientation;
// Remember the positions for CCD
b->m_sweep.worldCenter0 = b->m_sweep.worldCenter;
b->m_sweep.orientation0 = b->m_sweep.orientation;
if (b->m_type == e_dynamicBody)
{
// Integrate forces
v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
// Clear forces
b->m_force.SetZero();
// Integrate torques
// Superposition Principle
// w2 - w1 = dw1 + dw2
// w2 - w1 = h * I^1 * bt + h * I^1 * -gt
// w2 = w1 + dw1 + dw2
// Explicit Euler on current inertia and applied torque
// w2 = w1 + h * I1^1 * bt1
b3Vec3 dw1 = h * b->m_worldInvI * b->m_torque;
// Implicit Euler on next inertia and angular velocity
// w2 = w1 - h * I2^1 * cross(w2, I2 * w2)
// w2 - w1 = -I2^1 * h * cross(w2, I2 * w2)
// I2 * (w2 - w1) = -h * cross(w2, I2 * w2)
// I2 * (w2 - w1) + h * cross(w2, I2 * w2) = 0
// Toss out I2 from f using local I2 (constant) and local w1
// to remove its time dependency.
b3Vec3 w2 = b3SolveGyro(q, b->m_I, w, h);
b3Vec3 dw2 = w2 - w;
w += dw1 + dw2;
// Clear torques
b->m_torque.SetZero();
// Apply local damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Padé approximation:
// 1 / (1 + c * dt)
v *= 1.0f / (1.0f + h * b->m_linearDamping);
w *= 1.0f / (1.0f + h * b->m_angularDamping);
}
m_velocities[i].v = v;
m_velocities[i].w = w;
m_positions[i].x = x;
m_positions[i].q = q;
m_invInertias[i] = b->m_worldInvI;
}
b3JointSolverDef jointSolverDef;
jointSolverDef.joints = m_joints;
jointSolverDef.count = m_jointCount;
jointSolverDef.positions = m_positions;
jointSolverDef.velocities = m_velocities;
jointSolverDef.invInertias = m_invInertias;
jointSolverDef.dt = h;
b3JointSolver jointSolver(&jointSolverDef);
b3ContactSolverDef contactSolverDef;
contactSolverDef.allocator = m_allocator;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
contactSolverDef.invInertias = m_invInertias;
contactSolverDef.dt = h;
b3ContactSolver contactSolver(&contactSolverDef);
// 2. Initialize constraints
{
B3_PROFILE("Initialize Constraints");
contactSolver.InitializeConstraints();
if (flags & e_warmStartBit)
{
contactSolver.WarmStart();
}
jointSolver.InitializeConstraints();
if (flags & e_warmStartBit)
{
jointSolver.WarmStart();
}
}
// 3. Solve velocity constraints
{
B3_PROFILE("Solve Velocity Constraints");
for (u32 i = 0; i < velocityIterations; ++i)
{
jointSolver.SolveVelocityConstraints();
contactSolver.SolveVelocityConstraints();
}
if (flags & e_warmStartBit)
{
contactSolver.StoreImpulses();
}
}
// 4. Integrate positions
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b3Vec3 x = m_positions[i].x;
b3Quat q = m_positions[i].q;
b3Vec3 v = m_velocities[i].v;
b3Vec3 w = m_velocities[i].w;
b3Mat33 invI = m_invInertias[i];
// Prevent numerical instability due to large velocity changes.
b3Vec3 translation = h * v;
if (b3Dot(translation, translation) > B3_MAX_TRANSLATION_SQUARED)
{
float32 ratio = B3_MAX_TRANSLATION / b3Length(translation);
v *= ratio;
}
b3Vec3 rotation = h * w;
if (b3Dot(rotation, rotation) > B3_MAX_ROTATION_SQUARED)
{
float32 ratio = B3_MAX_ROTATION / b3Length(rotation);
w *= ratio;
}
// Integrate
x += h * v;
q = b3Integrate(q, w, h);
invI = b3RotateToFrame(b->m_invI, q);
m_positions[i].x = x;
m_positions[i].q = q;
m_velocities[i].v = v;
m_velocities[i].w = w;
m_invInertias[i] = invI;
}
// 5. Solve position constraints
{
B3_PROFILE("Solve Position Constraints");
bool positionsSolved = false;
for (u32 i = 0; i < positionIterations; ++i)
{
bool contactsSolved = contactSolver.SolvePositionConstraints();
bool jointsSolved = jointSolver.SolvePositionConstraints();
if (contactsSolved && jointsSolved)
{
// Early out if the position errors are small.
positionsSolved = true;
break;
}
}
}
// 6. Copy state buffers back to the bodies
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b->m_sweep.worldCenter = m_positions[i].x;
b->m_sweep.orientation = m_positions[i].q;
b->m_sweep.orientation.Normalize();
b->m_linearVelocity = m_velocities[i].v;
b->m_angularVelocity = m_velocities[i].w;
b->m_worldInvI = m_invInertias[i];
b->SynchronizeTransform();
}
// 7. Put bodies under unconsiderable motion to sleep
if (flags & e_sleepBit)
{
float32 minSleepTime = B3_MAX_FLOAT;
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
if (b->m_type == e_staticBody)
{
continue;
}
// Compute the linear and angular speed of the body.
float32 sqrLinVel = b3Dot(b->m_linearVelocity, b->m_linearVelocity);
float32 sqrAngVel = b3Dot(b->m_angularVelocity, b->m_angularVelocity);
if (sqrLinVel > B3_SLEEP_LINEAR_TOL || sqrAngVel > B3_SLEEP_ANGULAR_TOL)
{
minSleepTime = 0.0f;
b->m_sleepTime = 0.0f;
}
else
{
b->m_sleepTime += h;
minSleepTime = b3Min(minSleepTime, b->m_sleepTime);
}
}
// Put the island to sleep so long as the minimum found sleep time
// is below the threshold.
if (minSleepTime >= B3_TIME_TO_SLEEP)
{
for (u32 i = 0; i < m_bodyCount; ++i)
{
m_bodies[i]->SetAwake(false);
}
}
}
}
bool b3HullShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const
{
u32 planeCount = m_hull->faceCount;
const b3Plane* planes = m_hull->planes;
// Put the segment into the poly's frame of reference.
b3Vec3 p1 = b3MulT(xf.rotation, input.p1 - xf.position);
b3Vec3 p2 = b3MulT(xf.rotation, input.p2 - xf.position);
b3Vec3 d = p2 - p1;
float32 lower = 0.0f;
float32 upper = input.maxFraction;
u32 index = B3_MAX_U32;
// s(lower) = p1 + lower * d, 0 <= lower <= kupper
// The segment intersects the plane if a 'lower' exists
// for which s(lower) is inside all half-spaces.
// Solve line segment to plane:
// dot(n, s(lower)) = offset
// dot(n, p1 + lower * d) = offset
// dot(n, p1) + dot(n, lower * d) = offset
// dot(n, p1) + lower * dot(n, d) = offset
// lower * dot(n, d) = offset - dot(n, p1)
// lower = (offset - dot(n, p1)) / dot(n, d)
for (u32 i = 0; i < planeCount; ++i)
{
float32 numerator = planes[i].offset - b3Dot(planes[i].normal, p1);
float32 denominator = b3Dot(planes[i].normal, d);
if (denominator == 0.0f)
{
// s is parallel to this half-space.
if (numerator < 0.0f)
{
// s is outside of this half-space.
// dot(n, p1) and dot(n, p2) < 0.
return false;
}
}
else
{
// Original predicates:
// lower < numerator / denominator, for denominator < 0
// upper < numerator / denominator, for denominator < 0
// Optimized predicates:
// lower * denominator > numerator
// upper * denominator > numerator
if (denominator < 0.0f)
{
// s enters this half-space.
if (numerator < lower * denominator)
{
// Increase lower.
lower = numerator / denominator;
index = i;
}
}
else
{
// s exits the half-space.
if (numerator < upper * denominator)
{
// Decrease upper.
upper = numerator / denominator;
}
}
// Exit if intersection becomes empty.
if (upper < lower)
{
return false;
}
}
}
B3_ASSERT(lower >= 0.0f && lower <= input.maxFraction);
if (index != B3_MAX_U32)
{
output->fraction = lower;
output->normal = b3Mul(xf.rotation, planes[index].normal);
return true;
}
return false;
}