dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const { dgAssert (normal.m_w == 0.0f); dgAssert (normal.DotProduct(normal).GetScalar() > dgFloat32 (0.999f)); contactsOut[0] = normal * normal.DotProduct(point); return 1; }
void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const { if (level) { dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgVector p01 (p0 + p1); dgVector p12 (p1 + p2); dgVector p20 (p2 + p0); //p01 = p01 * p01.InvMagSqrt(); //p12 = p12 * p12.InvMagSqrt(); //p20 = p20 * p20.InvMagSqrt(); p01 = p01.Normalize(); p12 = p12.Normalize(); p20 = p20.Normalize(); dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); TesselateTriangle (level - 1, p0, p01, p20, count, ouput); TesselateTriangle (level - 1, p1, p12, p01, count, ouput); TesselateTriangle (level - 1, p2, p20, p12, count, ouput); TesselateTriangle (level - 1, p01, p12, p20, count, ouput); } else { ouput[count ++] = p0; ouput[count ++] = p1; ouput[count ++] = p2; } }
dgVector dgCollisionSphere::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dir.m_w == dgFloat32 (0.0f)); dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgAssert (dir.m_w == 0.0f); return dir.Scale (m_radius); }
void dgSolver::IntegrateBodiesVelocity(dgInt32 threadID) { dgVector speedFreeze2(m_world->m_freezeSpeed2 * dgFloat32(0.1f)); dgVector freezeOmega2(m_world->m_freezeOmega2 * dgFloat32(0.1f)); dgVector timestep4(m_timestepRK); const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray; dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0]; const dgInt32 step = m_threadCounts;; const dgInt32 bodyCount = m_cluster->m_bodyCount; for (dgInt32 i = threadID; i < bodyCount; i += step) { dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body; dgAssert(body->m_index == i); if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { const dgVector w(bodyProxyArray[i].m_weight); const dgJacobian& forceAndTorque = internalForces[i]; const dgVector force(body->m_externalForce + forceAndTorque.m_linear * w); const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular * w); const dgVector velocStep((force.Scale(body->m_invMass.m_w)) * timestep4); const dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4); if (!body->m_resting) { body->m_veloc += velocStep; body->m_omega += omegaStep; } else { const dgVector velocStep2(velocStep.DotProduct(velocStep)); const dgVector omegaStep2(omegaStep.DotProduct(omegaStep)); const dgVector test(((velocStep2 > speedFreeze2) | (omegaStep2 > speedFreeze2)) & m_negOne); const dgInt32 equilibrium = test.GetSignMask() ? 0 : 1; body->m_resting &= equilibrium; } dgAssert(body->m_veloc.m_w == dgFloat32(0.0f)); dgAssert(body->m_omega.m_w == dgFloat32(0.0f)); } } }
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const { dgAssert (dir.m_w == dgFloat32 (0.0f)); dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbs (x) > dgFloat32 (0.99995f)) { return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); } dgVector sideDir (m_yzMask & dir); dgAssert (sideDir.DotProduct(sideDir).GetScalar() > dgFloat32 (0.0f)); return sideDir.Normalize().Scale(m_radius); }
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dir.m_w == dgFloat32 (0.0f)); dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbs (x) > dgFloat32 (0.9999f)) { //return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); return dgVector (dgSign (x) * m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); } dgVector sideDir (m_yzMask & dir); //sideDir = sideDir * sideDir.InvMagSqrt(); sideDir = sideDir.Normalize(); return sideDir.Scale(m_radius) + dir.Scale (m_height); }
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const { dgAssert (m_p0.m_w == dgFloat32 (0.0f)); dgAssert (m_p1.m_w == dgFloat32 (0.0f)); if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) { dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32)); dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask); dgBigVector p0(m_p0); dgBigVector p0v0(v0 - p0); dgBigVector diff(m_diff); dgBigVector normal(faceNormal); dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar(); if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) { dgBigVector p (p0 + diff.Scale (tOut)); dgBigVector unitDir(m_unitDir); for (dgInt32 i = 0; i < indexCount; i++) { dgInt32 i2 = indexArray[i] * stride; dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask); dgBigVector edge0(p - v0); dgBigVector edge1(v1 - v0); dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar(); if (area < dgFloat32 (0.0f)) { return 1.2f; } v0 = v1; } return dgFloat32(tOut); } } return dgFloat32 (1.2f); }