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); }
dgInt32 dgCollisionSphere::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const { dgAssert (normal.m_w == 0.0f); //contactsOut[0] = normal.Scale3(normal % point); contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(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; } }
void dgConvexHull4d::TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const { if (level) { dgAssert (dgAbsf (p0.DotProduct3(p0) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p1.DotProduct3(p1) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p2.DotProduct3(p2) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgVector p01 (p0 + p1); dgVector p12 (p1 + p2); dgVector p20 (p2 + p0); p01 = p01.Scale3 (dgRsqrt(p01.DotProduct3(p01))); p12 = p12.Scale3 (dgRsqrt(p12.DotProduct3(p12))); p20 = p20.Scale3 (dgRsqrt(p20.DotProduct3(p20))); dgAssert (dgAbsf (p01.DotProduct3(p01) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p12.DotProduct3(p12) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p20.DotProduct3(p20) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); TessellateTriangle (level - 1, p0, p01, p20, count, ouput, start); TessellateTriangle (level - 1, p1, p12, p01, count, ouput, start); TessellateTriangle (level - 1, p2, p20, p12, count, ouput, start); TessellateTriangle (level - 1, p01, p12, p20, count, ouput, start); } else { dgBigPlane n (p0, p1, p2); n = n.Scale (dgFloat64(1.0f) / sqrt (n.DotProduct3(n))); n.m_w = dgFloat64(0.0f); ouput[start] = n; start += 8; count ++; } }
void dgCollisionTaperedCapsule::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const { if (level) { dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgVector p01 (p0 + p1); dgVector p12 (p1 + p2); dgVector p20 (p2 + p0); p01 = p01.Scale3 (dgRsqrt(p01 % p01)); p12 = p12.Scale3 (dgRsqrt(p12 % p12)); p20 = p20.Scale3 (dgRsqrt(p20 % p20)); dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f)); dgAssert (dgAbsf (p20 % p20 - 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 + 0] = p0.Scale3 (m_radio0); ouput[count + 1] = p1.Scale3 (m_radio0); ouput[count + 2] = p2.Scale3 (m_radio0); count += 3; } }
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; }
dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut, dgFloat32 normalSign) const { dgAssert (normal.m_w == 0.0f); dgAssert ((normal % normal) > dgFloat32 (0.999f)); //contactsOut[0] = normal.Scale3 (normal % point); contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point)); return 1; }
dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep) { dgJacobian velocStep; if (m_gyroTorqueOn) { dgVector dtHalf(timestep * dgVector::m_half); dgMatrix matrix(m_gyroRotation, dgVector::m_wOne); dgVector localOmega(matrix.UnrotateVector(m_omega)); dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque)); // derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy) dgVector dw(localOmega * dtHalf); dgMatrix jacobianMatrix( dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)), dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)), dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)), dgVector::m_wOne); // and solving for alpha we get the angular acceleration at t + dt // calculate gradient at a full time step //dgVector gradientStep(localTorque * timestep); dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep)); dgVector omega(matrix.RotateVector(localOmega + gradientStep)); dgAssert(omega.m_w == dgFloat32(0.0f)); // integrate rotation here dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f); dgFloat32 invOmegaMag = dgRsqrt(omegaMag2); dgVector omegaAxis(omega.Scale(invOmegaMag)); dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar(); dgQuaternion deltaRotation(omegaAxis, omegaAngle); m_gyroRotation = m_gyroRotation * deltaRotation; dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f)); matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne); localOmega = matrix.UnrotateVector(omega); //dgVector angularMomentum(inertia * localOmega); //body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum)); //body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque); dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega)); m_gyroTorque = matrix.RotateVector(localGyroTorque); m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass); velocStep.m_angular = matrix.RotateVector(gradientStep); } else { velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep; //velocStep.m_angular = velocStep.m_angular * dgVector::m_half; } velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep; return velocStep; }
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbsf (x) > dgFloat32 (0.9999f)) { return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); } dgVector sideDir (m_yzMask & dir); sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt()); return sideDir.Scale4(m_radius) + dir.Scale4 (m_height); }
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); }
dgIntersectStatus dgCollisionBVH::GetPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance) { dgPolygonMeshDesc& data = (*(dgPolygonMeshDesc*) context); if (data.m_faceCount >= DG_MAX_COLLIDING_FACES) { dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n")); return t_StopSearh; } if ((data.m_globalIndexCount + indexCount * 2 + 3) >= DG_MAX_COLLIDING_INDICES) { dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n")); return t_StopSearh; } if (data.m_me->GetDebugCollisionCallback()) { dgTriplex triplex[128]; dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32)); const dgVector scale = data.m_polySoupInstance->GetScale(); dgMatrix matrix (data.m_polySoupInstance->GetLocalMatrix() * data.m_polySoupBody->GetMatrix()); for (dgInt32 i = 0; i < indexCount; i ++ ) { //dgVector p (&polygon[indexArray[i] * stride]); //p = matrix.TransformVector(scale.CompProduct4(p)); dgVector p (matrix.TransformVector(scale.CompProduct4(dgVector(&polygon[indexArray[i] * stride])))); triplex[i].m_x = p.m_x; triplex[i].m_y = p.m_y; triplex[i].m_z = p.m_z; } if (data.m_polySoupBody) { data.m_me->GetDebugCollisionCallback() (data.m_polySoupBody, data.m_objBody, indexArray[indexCount], indexCount, &triplex[0].m_x, sizeof (dgTriplex)); } } dgAssert (data.m_vertex == polygon); dgInt32 count = indexCount * 2 + 3; data.m_faceIndexCount[data.m_faceCount] = indexCount; // data.m_faceIndexStart[data.m_faceCount] = data.m_faceCount ? (data.m_faceIndexStart[data.m_faceCount - 1] + data.m_faceIndexCount[data.m_faceCount - 1]) : 0; data.m_faceIndexStart[data.m_faceCount] = data.m_globalIndexCount; data.m_hitDistance[data.m_faceCount] = hitDistance; data.m_faceCount ++; dgInt32* const dst = &data.m_faceVertexIndex[data.m_globalIndexCount]; //the docks say memcpy is an intrinsic function but as usual this is another Microsoft lied //memcpy (dst, indexArray, sizeof (dgInt32) * count); for (dgInt32 i = 0; i < count; i ++) { dst[i] = indexArray[i]; } data.m_globalIndexCount += count; return t_ContinueSearh; }
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); }
void dgBilateralConstraint::SetPivotAndPinDir (const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1) { dgAssert (m_body0); dgAssert (m_body1); const dgMatrix& body0_Matrix = m_body0->GetMatrix(); dgAssert ((pinDirection0 % pinDirection0) > dgFloat32 (0.0f)); m_localMatrix0.m_front = pinDirection0.Scale3 (dgRsqrt (pinDirection0 % pinDirection0)); m_localMatrix0.m_right = m_localMatrix0.m_front * pinDirection1; m_localMatrix0.m_right = m_localMatrix0.m_right.Scale3 (dgRsqrt (m_localMatrix0.m_right % m_localMatrix0.m_right)); m_localMatrix0.m_up = m_localMatrix0.m_right * m_localMatrix0.m_front; m_localMatrix0.m_posit = pivot; m_localMatrix0.m_front.m_w = dgFloat32 (0.0f); m_localMatrix0.m_up.m_w = dgFloat32 (0.0f); m_localMatrix0.m_right.m_w = dgFloat32 (0.0f); m_localMatrix0.m_posit.m_w = dgFloat32 (1.0f); const dgMatrix& body1_Matrix = m_body1->GetMatrix(); m_localMatrix1 = m_localMatrix0 * body1_Matrix.Inverse(); m_localMatrix0 = m_localMatrix0 * body0_Matrix.Inverse(); }
dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f)); if (dir.m_x < dgFloat32(-0.9999f)) { return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else if (dir.m_x > dgFloat32(0.9999f)) { return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else { dgVector dir_yz (dir); dir_yz.m_x = dgFloat32 (0.0f); dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar(); dgAssert (mag2 > dgFloat32 (0.0f)); dir_yz = dir_yz.Scale4 (dgFloat32 (1.0f) / dgSqrt (mag2)); dgVector p0 (dir_yz.Scale4 (m_radio0)); dgVector p1 (dir_yz.Scale4 (m_radio1)); p0.m_x = -m_height; p1.m_x = m_height; dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar(); dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar(); if (dist1 >= dist0) { p0 = p1; } return p0; } }
void dgBilateralConstraint::SetPivotAndPinDir (const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1) { _ASSERTE (m_body0); _ASSERTE (m_body1); const dgMatrix& body0_Matrix = m_body0->GetMatrix(); _ASSERTE ((pinDirection0 % pinDirection0) > dgFloat32 (0.0f)); m_localMatrix0.m_front = pinDirection0.Scale (dgFloat32 (1.0f) / dgSqrt (pinDirection0 % pinDirection0)); m_localMatrix0.m_right = m_localMatrix0.m_front * pinDirection1; m_localMatrix0.m_right = m_localMatrix0.m_right.Scale (dgFloat32 (1.0f) / dgSqrt (m_localMatrix0.m_right % m_localMatrix0.m_right)); m_localMatrix0.m_up = m_localMatrix0.m_right * m_localMatrix0.m_front; m_localMatrix0.m_posit = pivot; m_localMatrix0.m_front.m_w = dgFloat32 (0.0f); m_localMatrix0.m_up.m_w = dgFloat32 (0.0f); m_localMatrix0.m_right.m_w = dgFloat32 (0.0f); m_localMatrix0.m_posit.m_w = dgFloat32 (1.0f); // dgMatrix body1_Matrix (dgGetIdentityMatrix()); // if (m_body1) { // body1_Matrix = m_body1->GetMatrix(); // } const dgMatrix& body1_Matrix = m_body1->GetMatrix(); m_localMatrix1 = m_localMatrix0 * body1_Matrix.Inverse(); m_localMatrix0 = m_localMatrix0 * body0_Matrix.Inverse(); }
void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1) { dgFloat32 length; _ASSERTE (m_body0); _ASSERTE (m_body1); const dgMatrix& body0_Matrix = m_body0->GetMatrix(); length = dir % dir; length = dgSqrt (length); _ASSERTE (length > dgFloat32 (0.0f)); // matrix0.m_front = body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length)); // Create__Basis (matrix0.m_front, matrix0.m_up, matrix0.m_right); matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length))); matrix0.m_posit = body0_Matrix.UntransformVector (pivot); matrix0.m_front.m_w = dgFloat32 (0.0f); matrix0.m_up.m_w = dgFloat32 (0.0f); matrix0.m_right.m_w = dgFloat32 (0.0f); matrix0.m_posit.m_w = dgFloat32 (1.0f); // dgMatrix body1_Matrix (dgGetIdentityMatrix()); // if (m_body1) { // body1_Matrix = m_body1->GetMatrix(); // } const dgMatrix& body1_Matrix = m_body1->GetMatrix(); matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse(); }
dgInt32 dgCollisionTaperedCapsule::CalculateSphereConicContacts (dgFloat32 posit, dgFloat32 radius, const dgVector& normal, const dgVector& point, dgVector* const contact) const { dgVector r (posit, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgFloat32 t = normal % (r - point); contact[0] = r - normal.Scale3 (t); return 1; }
dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f)); if (dir.m_x < dgFloat32(-0.9999f)) { return dgVector(-(m_height - D_CONE_SKIN_THINCKNESS), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else if (dir.m_x > dgFloat32(0.9999f)) { return dgVector(m_height - D_CONE_SKIN_THINCKNESS, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)); } else { dgVector dir_yz(dir); dir_yz.m_x = dgFloat32(0.0f); dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar(); dgAssert(mag2 > dgFloat32(0.0f)); dir_yz = dir_yz.Scale4(dgFloat32(1.0f) / dgSqrt(mag2)); dgVector p0(dir_yz.Scale4(m_radius - D_CONE_SKIN_THINCKNESS)); dgVector p1(dgVector::m_zero); p0.m_x = -(m_height - D_CONE_SKIN_THINCKNESS); p1.m_x = m_height - D_CONE_SKIN_THINCKNESS; dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar(); dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar(); if (dist1 >= dist0) { p0 = p1; } return p0; } }
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbsf (x) > dgFloat32 (0.9999f)) { dgFloat32 x0 = (x >= dgFloat32 (0.0f)) ? m_height : -m_height; return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f)); } // dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f)); dgVector sideDir (m_yzMask & dir); // sideDir = sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f))); sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt()); // return sideDir + dir.Scale3 (m_height); return sideDir.Scale4(m_radius) + dir.Scale4 (m_height); }
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); dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0]; const dgInt32 step = m_threadCounts;; const dgInt32 bodyCount = m_cluster->m_bodyCount - 1; for (dgInt32 j = threadID; j < bodyCount; j += step) { const dgInt32 i = j + 1; dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body; dgAssert(body->m_index == i); if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { const dgJacobian& forceAndTorque = internalForces[i]; const dgVector force(body->m_externalForce + forceAndTorque.m_linear); const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular); //const dgVector velocStep((force.Scale(body->m_invMass.m_w)) * timestep4); //const dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4); const dgJacobian velocStep(body->IntegrateForceAndToque(force, torque, timestep4)); if (!body->m_resting) { //body->m_veloc += velocStep; //body->m_omega += omegaStep; body->m_veloc += velocStep.m_linear; body->m_omega += velocStep.m_angular; } else { //const dgVector velocStep2(velocStep.DotProduct(velocStep)); //const dgVector omegaStep2(omegaStep.DotProduct(omegaStep)); const dgVector velocStep2(velocStep.m_linear.DotProduct(velocStep.m_linear)); const dgVector omegaStep2(velocStep.m_angular.DotProduct(velocStep.m_angular)); 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 dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& dir) const { dgVector p0(dir.Scale3 (m_radio0)); dgVector p1(dir.Scale3 (m_radio1)); p0.m_x += m_height; p1.m_x -= m_height; dgFloat32 dir0 = p0 % dir; dgFloat32 dir1 = p1 % dir; return dgVector ((dir0 >= dir1) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); }
dgInt32 dgCollisionChamferCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const { dgFloat32 disc2 = point.m_y * point.m_y + point.m_z * point.m_z; if (disc2 < m_radius * m_radius) { dgVector cylinderNormal ((point.m_x >= dgFloat32 (0.0)) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); return CalculateContactsGeneric (point, cylinderNormal, proxy, contactsOut); } else { dgVector r (dgFloat32 (0.0f), point.m_y, point.m_z, dgFloat32 (0.0f)); dgAssert ((r % r) > dgFloat32 (0.0f)); // r = r.Scale3(m_radius * dgRsqrt (r % r)); r = r.CompProduct4(r.InvMagSqrt()).Scale4(m_radius); //dgFloat32 t = normal % (r - point); dgVector t (normal.DotProduct4(r - point)); //contactsOut[0] = r - normal.Scale3 (t); contactsOut[0] = r - normal.CompProduct4(t); return 1; } }
dgInt32 dgCollisionTaperedCapsule::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const { dgInt32 count = 0; dgVector p0 (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector dir0 (p0 - origin); dgFloat32 dist0 = dir0 % normal; if ((dist0 * dist0) < (m_radio0 * m_radio0)) { contactsOut[count] = p0 - normal.Scale3 (dist0); count ++; } dgVector p1 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector dir1 (p1 - origin); dgFloat32 dist1 = dir1 % normal; if ((dist1 * dist1) < (m_radio1 * m_radio1)) { contactsOut[count] = p1 - normal.Scale3 (dist1); count ++; } return count; }
dgVector dgCollisionBox::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf(dir.DotProduct3(dir) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgAssert (dir.m_w == dgFloat32 (0.0f)); dgVector mask (dir < dgVector (dgFloat32 (0.0f))); if (vertexIndex) { dgVector index (m_indexMark.CompProduct4(mask & dgVector::m_one)); index = (index.AddHorizontal()).GetInt(); *vertexIndex = dgInt32 (index.m_ix); } return (m_size[1] & mask) + m_size[0].AndNot(mask); }
dgFloat32 dgCollisionCone::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const { dgFloat32 volume; dgFloat32 inertaxx; dgFloat32 inertayyzz; //dgVector centerOfMass1; //dgVector inertia1; //dgVector crossInertia1; //volume = dgCollisionConvex::CalculateMassProperties (inertia1, crossInertia1, centerOfMass1); volume = dgFloat32 (3.1616f * 2.0f / 3.0f) * m_radius * m_radius * m_height; centerOfMass = GetOffsetMatrix().m_posit - GetOffsetMatrix().m_front.Scale (dgFloat32 (0.5f) * m_height); inertaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius * volume; inertayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (4.0f / 10.0f) * m_height * m_height) * volume; dgMatrix inertiaTensor (dgGetIdentityMatrix()); inertiaTensor[0][0] = inertaxx; inertiaTensor[1][1] = inertayyzz; inertiaTensor[2][2] = inertayyzz; inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix(); crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z; crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x; crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y; dgVector central (centerOfMass.CompProduct(centerOfMass)); inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z); inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x); inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y); centerOfMass = centerOfMass.Scale (volume); return volume; }
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const { *vertexIndex = -1; dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbsf (x) > dgFloat32 (0.99995f)) { return dgVector::m_zero; } dgVector sideDir (m_yzMask & dir); dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f)); return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius); }
dgVector dgCollisionTaperedCapsule::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgVector p0(dir.Scale3 (m_radio0)); dgVector p1(dir.Scale3 (m_radio1)); p0.m_x += m_height; p1.m_x -= m_height; dgFloat32 dir0 = p0 % dir; dgFloat32 dir1 = p1 % dir; if (dir1 > dir0) { p0 = p1; } return p0; }
dgVector dgCollisionChamferCylinder::ConvexConicSupporVertex (const dgVector& dir) const { dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); //if (dgAbsf (dir.m_x) > dgFloat32 (0.99995f)) { if (dgAbsf (x) > dgFloat32 (0.99995f)) { return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); } //dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f)); dgVector sideDir (m_yzMask & dir); dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f)); //return sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir)); return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius); }
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir) const { _ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); if (dgAbsf (dir.m_x) > dgFloat32 (0.9998f)) { dgFloat32 x0; x0 = (dir.m_x >= dgFloat32 (0.0f)) ? m_height : -m_height; return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f)); } _ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f)); sideDir = sideDir.Scale (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f))); return sideDir + dir.Scale (m_height); }
dgVector dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const { dgAssert (dgAbsf (dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgVector p (SupportVertex(dir, NULL)); dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0), dgFloat32 (0.0f)); dgAssert (dgAbsf (n % n - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 project = m_sideNormal % n; if (project > dgFloat32 (0.9998f)) { dgFloat32 t = dgFloat32 (0.5f) * (point.m_x + m_height) / m_height; dgFloat32 r = m_radio1 + (m_radio0 - m_radio1) * t; p = dir.Scale3 (r); p.m_x += point.m_x; } return p; }