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;
}
}
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();
}
dgFloat32 dgCollisionBVH::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
dgBVHRay ray (localP0, localP1);
ray.m_t = dgMin(maxT, dgFloat32 (1.0f));
ray.m_me = this;
ray.m_userData = userData;
if (!m_userRayCastCallback) {
ForAllSectorsRayHit (ray, maxT, RayHit, &ray);
if (ray.m_t <= maxT) {
maxT = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale3 (dgRsqrt ((ray.m_normal % ray.m_normal) + dgFloat32 (1.0e-8f)));
// contactOut.m_userId = ray.m_id;
contactOut.m_shapeId0 = ray.m_id;
contactOut.m_shapeId1 = ray.m_id;
}
} else {
if (body) {
//ray.m_matrix = body->m_collisionWorldMatrix;
ray.m_matrix = body->m_collision->GetGlobalMatrix();
}
ForAllSectorsRayHit (ray, maxT, RayHitUser, &ray);
if (ray.m_t <= dgFloat32 (1.0f)) {
maxT = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale3 (dgRsqrt ((ray.m_normal % ray.m_normal) + dgFloat32 (1.0e-8f)));
// contactOut.m_userId = ray.m_id;
contactOut.m_shapeId0 = ray.m_id;
contactOut.m_shapeId1 = ray.m_id;
}
}
return maxT;
}
dgFloat32 dgCollisionBVH::RayCast (const dgVector& localP0, const dgVector& localP1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
return dgFloat32 (1.2f);
}
dgFloat32 param = dgFloat32 (1.2f);
dgBVHRay ray (localP0, localP1);
ray.m_t = 2.0f;
ray.m_me = this;
ray.m_userData = userData;
if (!m_userRayCastCallback) {
ForAllSectorsRayHit (ray, RayHit, &ray);
if (ray.m_t <= 1.0f) {
param = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale (dgRsqrt ((ray.m_normal % ray.m_normal) + 1.0e-8f));
contactOut.m_userId = ray.m_id;
}
} else {
if (body) {
ray.m_matrix = body->m_collisionWorldMatrix;
}
ForAllSectorsRayHit (ray, RayHitUser, &ray);
if (ray.m_t <= 1.0f) {
param = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale (dgRsqrt ((ray.m_normal % ray.m_normal) + 1.0e-8f));
contactOut.m_userId = ray.m_id;
}
}
return param;
}
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 % 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));
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 % n));
n.m_w = dgFloat64(0.0f);
ouput[start] = n;
start += 8;
count ++;
}
}
void dgBallConstraint::SetLimits (
const dgVector& coneDir,
dgFloat32 minConeAngle,
dgFloat32 maxConeAngle,
dgFloat32 maxTwistAngle,
const dgVector& bilateralDir,
dgFloat32 negativeBilateralConeAngle__,
dgFloat32 positiveBilateralConeAngle__)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
dgVector lateralDir (bilateralDir * coneDir);
if ((lateralDir % lateralDir) < dgFloat32 (1.0e-3f)) {
dgMatrix tmp (coneDir);
lateralDir = tmp.m_up;
}
m_localMatrix0.m_front = body0_Matrix.UnrotateVector (coneDir);
m_localMatrix0.m_up = body0_Matrix.UnrotateVector (lateralDir);
m_localMatrix0.m_posit = body0_Matrix.UntransformVector (matrix1.m_posit);
m_localMatrix0.m_front = m_localMatrix0.m_front.Scale3 (dgRsqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
m_localMatrix0.m_up = m_localMatrix0.m_up.Scale3 (dgRsqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;
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_twistAngle = dgClamp (maxTwistAngle, dgFloat32 (5.0f) * dgDEG2RAD, dgFloat32 (90.0f) * dgDEG2RAD);
m_coneAngle = dgClamp ((maxConeAngle - minConeAngle) * dgFloat32 (0.5f), dgFloat32 (5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
m_coneAngleCos = dgCos (m_coneAngle);
dgMatrix coneMatrix (dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32 (0.5f)));
m_localMatrix0 = coneMatrix * m_localMatrix0;
m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();
}
void dgPolygonSoupDatabaseBuilder::End(bool optimize)
{
Optimize(optimize);
// build the normal array and adjacency array
// calculate all face the normals
hacd::HaI32 indexCount = 0;
m_normalPoints[m_faceCount].m_x = hacd::HaF64 (0.0f);
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
hacd::HaI32 faceIndexCount = m_faceVertexCount[i];
hacd::HaI32* const ptr = &m_vertexIndex[indexCount + 1];
dgBigVector v0 (&m_vertexPoints[ptr[0]].m_x);
dgBigVector v1 (&m_vertexPoints[ptr[1]].m_x);
dgBigVector e0 (v1 - v0);
dgBigVector normal (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
for (hacd::HaI32 j = 2; j < faceIndexCount - 1; j ++) {
dgBigVector v2 (&m_vertexPoints[ptr[j]].m_x);
dgBigVector e1 (v2 - v0);
normal += e0 * e1;
e0 = e1;
}
normal = normal.Scale (dgRsqrt (normal % normal));
m_normalPoints[i].m_x = normal.m_x;
m_normalPoints[i].m_y = normal.m_y;
m_normalPoints[i].m_z = normal.m_z;
indexCount += faceIndexCount;
}
// compress normals array
m_normalIndex[m_faceCount] = 0;
m_normalCount = dgVertexListToIndexList(&m_normalPoints[0].m_x, sizeof (dgBigVector), 3, m_faceCount, &m_normalIndex[0], hacd::HaF32 (1.0e-4f));
}
dgInt32 dgCollisionTaperedCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut, normalSign);
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut, normalSign);
}
return count;
}
dgVector dgCollisionTaperedCylinder::ConvexConicSupporVertex (const dgVector& dir) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 height = (m_height > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_height - DG_TAPED_CYLINDER_SKIN_PADDING : m_height;
dgFloat32 radio0 = (m_radio0 > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_radio0 - DG_TAPED_CYLINDER_SKIN_PADDING : m_radio0;
dgFloat32 radio1 = (m_radio1 > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_radio1 - DG_TAPED_CYLINDER_SKIN_PADDING : m_radio1;
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 y0 = radio0;
dgFloat32 z0 = dgFloat32 (0.0f);
dgFloat32 y1 = radio1;
dgFloat32 z1 = dgFloat32 (0.0f);
dgFloat32 mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * radio0 * mag2;
z0 = dir.m_z * radio0 * mag2;
y1 = dir.m_y * radio1 * mag2;
z1 = dir.m_z * radio1 * mag2;
}
dgVector p0 ( height, y0, z0, dgFloat32 (0.0f));
dgVector p1 (-height, y1, z1, dgFloat32 (0.0f));
dgFloat32 dist0 = dir % p0;
dgFloat32 dist1 = dir % p1;
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
dgVector dgCollisionTaperedCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 y0 = m_radio0;
dgFloat32 z0 = dgFloat32 (0.0f);
dgFloat32 y1 = m_radio1;
dgFloat32 z1 = dgFloat32 (0.0f);
dgFloat32 mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * m_radio0 * mag2;
z0 = dir.m_z * m_radio0 * mag2;
y1 = dir.m_y * m_radio1 * mag2;
z1 = dir.m_z * m_radio1 * mag2;
}
dgVector p0 ( m_height, y0, z0, dgFloat32 (0.0f));
dgVector p1 (-m_height, y1, z1, dgFloat32 (0.0f));
dgFloat32 dist0 = dir % p0;
dgFloat32 dist1 = dir % p1;
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
dgFloat32 dgCollisionSphere::RayCast (const dgVector& p0, const dgVector& p1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
dgFloat32 t = dgRayCastSphere (p0, p1, dgVector (dgFloat32 (0.0f)), m_radius);
if (t < maxT) {
dgVector contact (p0 + (p1 - p0).Scale3 (t));
contactOut.m_normal = contact.Scale3 (dgRsqrt (contact % contact));
//contactOut.m_userId = SetUserDataID();
}
return t;
}
dgVector dgCollisionCone::SupportVertex (const dgVector& dir) const
{
/*
dgFloat32 y0;
dgFloat32 z0;
dgFloat32 y1;
dgFloat32 z1;
dgFloat32 dist0;
dgFloat32 dist1;
dgFloat32 tetha;
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dir.m_x > m_sinAngle) {
return dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
tetha = m_tethaStep * dgFloor (dgAtan2 (dir.m_y, dir.m_z) * m_tethaStepInv);
dgSinCos (tetha, y0, z0);
y0 *= m_radius;
z0 *= m_radius;
y1 = y0 * m_delCosTetha + z0 * m_delSinTetha;
z1 = z0 * m_delCosTetha - y0 * m_delSinTetha;
dist0 = dir.m_y * y0 + dir.m_z * z0;
dist1 = dir.m_y * y1 + dir.m_z * z1;
if (dist1 > dist0) {
y0 = y1;
z0 = z1;
}
return dgVector (-m_height, y0, z0, dgFloat32 (0.0f));
*/
dgFloat32 y0;
dgFloat32 z0;
dgFloat32 mag2;
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dir.m_x > m_sinAngle) {
return dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
y0 = m_radius;
z0 = dgFloat32 (0.0f);
mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * m_radius * mag2;
z0 = dir.m_z * m_radius * mag2;
}
return dgVector (-m_height, y0, z0, dgFloat32 (0.0f));
}
void dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale3 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale3 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
#ifdef _DEBUG
for (dgInt32 i = 0; i < 4; i ++) {
for (dgInt32 j = 0; j < 4; j ++) {
dgAssert (dgCheckFloat(m_matrix[i][j]));
}
}
dgInt32 j0 = 1;
dgInt32 j1 = 2;
for (dgInt32 i = 0; i < 3; i ++) {
dgAssert (m_matrix[i][3] == 0.0f);
dgFloat32 val = m_matrix[i] % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
dgVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
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;
}
void dgCollisionDeformableMesh::UpdateVisualNormals()
{
for (dgInt32 i = 0; i < m_trianglesCount; i ++) {
dgInt32 i0 = m_indexList[i * 3];
dgInt32 i1 = m_indexList[i * 3 + 1];
dgInt32 i2 = m_indexList[i * 3 + 2];
dgVector e0 (m_particles.m_posit[i1] - m_particles.m_posit[i0]);
dgVector e1 (m_particles.m_posit[i2] - m_particles.m_posit[i0]);
dgVector n = e0 * e1;
n = n.Scale3(dgRsqrt (n % n));
m_faceNormals[i] = n;
}
dgAssert (m_visualVertexData);
for (dgInt32 i = 0; i < m_visualVertexCount; i ++) {
m_visualVertexData[i].m_normals[0] = dgFloat32 (0.0f);
m_visualVertexData[i].m_normals[1] = dgFloat32 (0.0f);
m_visualVertexData[i].m_normals[2] = dgFloat32 (0.0f);
}
for (dgList<dgMeshSegment>::dgListNode* node = m_visualSegments.GetFirst(); node; node = node->GetNext() ) {
const dgMeshSegment& segment = node->GetInfo();
for (dgInt32 i = 0; i < segment.m_indexCount; i ++) {
dgInt32 index = segment.m_indexList[i];
dgInt32 faceIndexNormal = i / 3;
m_visualVertexData[index].m_normals[0] += m_faceNormals[faceIndexNormal].m_x;
m_visualVertexData[index].m_normals[1] += m_faceNormals[faceIndexNormal].m_y;
m_visualVertexData[index].m_normals[2] += m_faceNormals[faceIndexNormal].m_z;
}
}
for (dgInt32 i = 0; i < m_visualVertexCount; i ++) {
dgVector n (m_visualVertexData[i].m_normals[0], m_visualVertexData[i].m_normals[1], m_visualVertexData[i].m_normals[2], dgFloat32 (0.0f));
n = n.Scale3(dgRsqrt (n % n));
m_visualVertexData[i].m_normals[0] = n.m_x;
m_visualVertexData[i].m_normals[1] = n.m_y;
m_visualVertexData[i].m_normals[2] = n.m_z;
}
}
void dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale4 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale4 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
dgAssert (m_matrix.TestOrthogonal());
}
dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 i;
dgInt32 count;
dgFloat32 y;
dgFloat32 z;
dgFloat32 cosAng;
dgFloat32 sinAng;
dgFloat32 magInv;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
// magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
// cosAng = normal.m_y * magInv;
// sinAng = normal.m_z * magInv;
// dgMatrix matrix (dgGetIdentityMatrix ());
// matrix[1][1] = cosAng;
// matrix[1][2] = sinAng;
// matrix[2][1] = -sinAng;
// matrix[2][2] = cosAng;
// dgVector normal2 (matrix.UnrotateVector (normal));
// dgVector origin2 (matrix.UnrotateVector (origin));
// count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut);
// matrix.TransformTriplex (contactsOut, sizeof (dgVector), contactsOut, sizeof (dgVector), count);
magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
cosAng = normal.m_y * magInv;
sinAng = normal.m_z * magInv;
_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
// dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng,
// normal.m_z * cosAng - normal.m_y * sinAng, dgFloat32 (0.0f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut);
for (i = 0; i < count; i ++) {
y = contactsOut[i].m_y;
z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut);
}
return count;
}
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 dgCollisionCone::SupportVertex (const dgVector& dir) const
{
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dir.m_x > m_sinAngle) {
return dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgFloat32 y0 = m_radius;
dgFloat32 z0 = dgFloat32 (0.0f);
dgFloat32 mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * m_radius * mag2;
z0 = dir.m_z * m_radius * mag2;
}
return dgVector (-m_height, y0, z0, dgFloat32 (0.0f));
}
dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const
{
dgQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dgQuaternion dq (q0.Inverse() * q1);
dgVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3, dgFloat32 (0.0f));
dgFloat32 dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) {
return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
}
dgFloat32 dirMagInv = dgRsqrt (dirMag2);
dgFloat32 dirMag = dirMag2 * dirMagInv;
dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale3 (dirMagInv * omegaMag);
}
hacd::HaI32 dgPolygonSoupDatabaseBuilder::AddConvexFace (hacd::HaI32 count, hacd::HaI32* const pool, hacd::HaI32* const facesArray)
{
dgPolySoupFilterAllocator polyhedra;
count = polyhedra.AddFilterFace(hacd::HaU32 (count), pool);
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0) {
edge = edge->m_twin;
}
hacd::HaI32 isconvex = 1;
hacd::HaI32 facesCount = 0;
hacd::HaI32 flag = 1;
while (flag) {
flag = 0;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
do {
dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
hacd::HaF64 mag2 = e0 % e0;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
count --;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3) {
flag = 1;
while (flag) {
flag = 0;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
hacd::HaF64 mag2 = e1 % e0;
if (mag2 > hacd::HaF32 (0.9999f)) {
count --;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
return 0;
}
normal = normal.Scale (dgRsqrt (mag2));
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
dgBigVector n (e0 * e1);
hacd::HaF64 mag2 = n % normal;
if (mag2 < hacd::HaF32 (1.0e-5f)) {
isconvex = 0;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (isconvex) {
dgEdge* const first = edge;
if (count >= 3) {
count = 0;
dgEdge* ptr = first;
do {
pool[count] = ptr->m_incidentVertex;
count ++;
ptr = ptr->m_next;
} while (ptr != first);
facesArray[facesCount] = count;
facesCount = 1;
}
} else {
dgPolyhedra leftOver;
dgPolyhedra polyhedra2;
dgEdge* ptr = edge;
count = 0;
do {
pool[count] = ptr->m_incidentVertex;
count ++;
ptr = ptr->m_next;
} while (ptr != edge);
polyhedra2.BeginFace();
polyhedra2.AddFace (count, pool);
polyhedra2.EndFace();
leftOver.BeginFace();
polyhedra2.ConvexPartition (&m_vertexPoints[0].m_x, sizeof (dgTriplex), &leftOver);
leftOver.EndFace();
hacd::HaI32 mark = polyhedra2.IncLRU();
hacd::HaI32 index = 0;
dgPolyhedra::Iterator iter (polyhedra2);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
if (edge->m_incidentFace < 0) {
continue;
}
if (edge->m_mark == mark) {
continue;
}
ptr = edge;
count = 0;
do {
ptr->m_mark = mark;
pool[index] = ptr->m_incidentVertex;
index ++;
count ++;
ptr = ptr->m_next;
} while (ptr != edge);
facesArray[facesCount] = count;
facesCount ++;
}
}
return facesCount;
}
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32 (1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32 (0.0f)) {
projectFactor *= dgFloat32 (-1.0f);
normal = normal.Scale3 (dgFloat32 (-1.0f));
}
if (projectFactor > dgFloat32 (0.9999f)) {
for (dgInt32 i = 0; i < m_count; i ++) {
contactsOut[count] = m_localPoly[i];
count ++;
}
#ifdef _DEBUG
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
#endif
} else if (projectFactor > dgFloat32 (0.1736f)) {
maxDist = dgFloat32 (0.0f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if (side0 > dgFloat32 (0.0f)) {
maxDist = dgMax (maxDist, side0);
contactsOut[count] = p0 - plane.Scale3 (side0);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
if (side1 <= dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
} else if (side1 > dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
} else {
maxDist = dgFloat32 (1.0e10f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if ((side0 * side1) < dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32 (1.0e-3f)) {
dgVector maxPoint (contactsOut[0]);
dgVector minPoint (contactsOut[0]);
dgVector lineDir (m_normal * normal);
dgFloat32 proj = contactsOut[0] % lineDir;
dgFloat32 maxProjection = proj;
dgFloat32 minProjection = proj;
for (dgInt32 i = 1; i < count; i ++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error (contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32 (1.0e-8f)) {
count --;
}
}
#ifdef _DEBUG
if (count > 1) {
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector e0 (contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i ++) {
dgVector e1 (contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale3 (dgRsqrt(n % n));
dgFloat32 val = n % normal;
dgAssert (val > dgFloat32 (0.9f));
}
}
#endif
return count;
}
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 relVelocErr;
dgFloat32 penetrationErr;
dgMatrix matrix0;
dgMatrix matrix1;
if (m_jointUserCallback)
{
m_jointUserCallback(*this, params.m_timestep);
}
dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
m_angles = angle.Scale(-dgFloat32(1.0f));
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgPointParam pointData;
InitPointParam(pointData, m_stiffness, p0, p1);
CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
ret = 3;
if (m_twistLimit)
{
if (angle.m_x > m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
const dgVector& dir = matrix0.m_right;
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
else if (angle.m_x < -m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
if (m_coneLimit)
{
dgFloat32 coneCos;
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_coneAngleCos)
{
dgVector p0(
matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector tangentDir(matrix0.m_front * matrix1.m_front);
tangentDir = tangentDir.Scale(
dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
CalculatePointDerivative(ret, params, tangentDir, pointData,
&m_jointForce[ret]);
ret++;
dgVector normalDir(tangentDir * matrix0.m_front);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
//restitution = contact.m_restitution;
relVelocErr = velocError % normalDir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH
* (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
CalculatePointDerivative(ret, params, normalDir, pointData,
&m_jointForce[ret]);
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
return dgUnsigned32(ret);
}
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 sinAngle;
dgFloat32 cosAngle;
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix1.m_up;
dgVector dir2 (dir0 * dir1);
dgVector dir3 (dir2 * dir0);
dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3));
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH));
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]);
ret = 4;
// dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
// m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
// if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
// m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
// }
sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
// dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle0 = dgAtan2 (sinAngle, cosAngle);
// dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
// m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
// if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
// m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
// }
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
// dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle1 = dgAtan2 (sinAngle, cosAngle);
if (m_jointAccelFnt) {
dgUnsigned32 code;
dgJointCallbackParam axisParam[2];
// linear acceleration
axisParam[0].m_accel = dgFloat32 (0.0f);
axisParam[0].m_timestep = params.m_timestep;
axisParam[0].m_minFriction = DG_MIN_BOUND;
axisParam[0].m_maxFriction = DG_MAX_BOUND;
// angular acceleration
axisParam[1].m_accel = dgFloat32 (0.0f);
axisParam[1].m_timestep = params.m_timestep;
axisParam[1].m_minFriction = DG_MIN_BOUND;
axisParam[1].m_maxFriction = DG_MAX_BOUND;
code = m_jointAccelFnt (*this, axisParam);
if (code & 1) {
if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
// CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]);
CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[0].m_accel;
SetMotorAcceleration (ret, axisParam[0].m_accel, params);
ret ++;
}
if (code & 2) {
if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[1].m_accel;
SetMotorAcceleration (ret, axisParam[1].m_accel, params);
ret ++;
}
}
return dgUnsigned32 (ret);
}
void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCylinder_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_dirVector.m_x = radio1 - radio0;
m_dirVector.m_y = m_height * dgFloat32 (2.0f);
m_dirVector.m_z = dgFloat32 (0.0f);
m_dirVector.m_w = dgFloat32 (0.0f);
m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector));
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CYLINDER_SEGMENTS];
dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count = 0;
if (normal.m_x > dgFloat32(0.99f)) {
contactsOut[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
count = 1;
} else if (normal.m_x < dgFloat32(-0.995f)) {
if (normal.m_x < dgFloat32(-0.9995f)) {
dgMatrix matrix(normal);
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i].CompProduct4(scale)) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection(normal1, origin1, contactsOut);
if (count > 6) {
dgInt32 dy = 2 * 6;
dgInt32 dx = 2 * count;
dgInt32 acc = dy - count;
dgInt32 index = 0;
for (dgInt32 i = 0; i < count; i++) {
if (acc > 0) {
contactsOut[index] = contactsOut[i];
index++;
acc -= dx;
}
acc += dy;
}
count = index;
}
for (dgInt32 i = 0; i < count; i++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
} else {
dgFloat32 magInv = dgRsqrt(normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert(dgAbsf(normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32(1.0e-4f));
dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32(0.0f), dgFloat32(0.0f));
dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
count = 0;
int i0 = 2;
dgVector test0((m_profile[i0] - origin1).DotProduct4(normal1));
for (int i = 0; (i < 3) && (count < 2); i++) {
dgVector test1((m_profile[i] - origin1).DotProduct4(normal1));
dgVector acrossPlane(test0.CompProduct4(test1));
if (acrossPlane.m_x < 0.0f) {
dgVector step(m_profile[i] - m_profile[i0]);
contactsOut[count] = m_profile[i0] - step.Scale4(test0.m_x / (step.DotProduct4(normal1).m_x));
count++;
}
i0 = i;
test0 = test1;
}
for (dgInt32 i = 0; i < count; i++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
}
return count;
}
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
dgPlane plane (normal1, - (normal1 % origin1));
count = 0;
dgVector maxDir ((normal1.m_x > dgFloat32 (0.0f)) ? m_silhuette[0].m_x : -m_silhuette[0].m_x,
(normal1.m_y > dgFloat32 (0.0f)) ? m_silhuette[0].m_y : -m_silhuette[0].m_y, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 test0 = plane.Evalue (maxDir);
dgFloat32 test1 = plane.Evalue (maxDir.Scale (dgFloat32 (-1.0f)));
if ((test0 * test1) > dgFloat32 (0.0f)) {
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
contactsOut[count] = normal1.Scale (-test0);
contactsOut[count].m_y += m_radius;
count ++;
} else {
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
contactsOut[count] = normal1.Scale (-test0);
contactsOut[count].m_y -= m_radius;
count ++;
}
}
} else {
dgVector dp (m_silhuette[1] - m_silhuette[0]);
dgFloat32 den = normal1 % dp;
_ASSERTE (dgAbsf (den) > dgFloat32 (0.0f));
test0 = -plane.Evalue (m_silhuette[0]) / den;
if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) {
contactsOut[count] = m_silhuette[0] + dp.Scale (test0);
count ++;
}
if (count < 2) {
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
dgFloat32 r = -m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc > dgFloat32 (0.0f)) {
_ASSERTE (dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = - dgFloat32 (0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 > dgFloat32 (0.0f)) {
y0 = y1;
}
_ASSERTE (y0 < dgFloat32 (0.0f));
_ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f));
count ++;
}
}
}
if (count < 2) {
dgVector dp (m_silhuette[3] - m_silhuette[2]);
den = normal1 % dp;
_ASSERTE (dgAbsf (den) > dgFloat32 (0.0f));
test0 = - plane.Evalue (m_silhuette[2]) / den;
if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) {
contactsOut[count] = m_silhuette[2] + dp.Scale (test0);
count ++;
}
}
if (count < 2) {
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
dgFloat32 r = m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc > dgFloat32 (0.0f)) {
_ASSERTE (dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = - dgFloat32 (0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 < dgFloat32 (0.0f)) {
y0 = y1;
}
_ASSERTE (y0 > dgFloat32 (0.0f));
_ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f));
count ++;
}
}
}
}
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut);
}
return count;
}
hacd::HaI32 dgPolygonSoupDatabaseBuilder::FilterFace (hacd::HaI32 count, hacd::HaI32* const pool)
{
if (count == 3) {
dgBigVector p0 (m_vertexPoints[pool[2]]);
for (hacd::HaI32 i = 0; i < 3; i ++) {
dgBigVector p1 (m_vertexPoints[pool[i]]);
dgBigVector edge (p1 - p0);
hacd::HaF64 mag2 = edge % edge;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
count = 0;
}
p0 = p1;
}
if (count == 3) {
dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal (edge0 * edge1);
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
count = 0;
}
}
} else {
dgPolySoupFilterAllocator polyhedra;
count = polyhedra.AddFilterFace (hacd::HaU32 (count), pool);
if (!count) {
return 0;
}
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0) {
edge = edge->m_twin;
}
bool flag = true;
while (flag) {
flag = false;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
do {
dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
hacd::HaF64 mag2 = e0 % e0;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3) {
flag = true;
dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
HACD_ASSERT ((normal % normal) > hacd::HaF32 (1.0e-10f));
normal = normal.Scale (dgRsqrt (normal % normal + hacd::HaF32 (1.0e-20f)));
while (flag) {
flag = false;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
hacd::HaF64 mag2 = e1 % e0;
if (mag2 > hacd::HaF32 (0.9999f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
dgBigVector n (e0 * e1);
mag2 = n % normal;
if (mag2 < hacd::HaF32 (1.0e-5f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
}
dgEdge* first = edge;
if (count >= 3) {
hacd::HaF64 best = hacd::HaF32 (2.0f);
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
hacd::HaF64 mag2 = fabs (e1 % e0);
if (mag2 < best) {
best = mag2;
first = ptr;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
count = 0;
ptr = first;
do {
pool[count] = ptr->m_incidentVertex;
count ++;
ptr = ptr->m_next;
} while (ptr != first);
}
#ifdef _DEBUG
if (count >= 3) {
hacd::HaI32 j0 = count - 2;
hacd::HaI32 j1 = count - 1;
dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
for (hacd::HaI32 j2 = 0; j2 < count; j2 ++) {
dgBigVector p0 (&m_vertexPoints[pool[j0]].m_x);
dgBigVector p1 (&m_vertexPoints[pool[j1]].m_x);
dgBigVector p2 (&m_vertexPoints[pool[j2]].m_x);
dgBigVector e0 ((p0 - p1));
dgBigVector e1 ((p2 - p1));
dgBigVector n (e1 * e0);
HACD_ASSERT ((n % normal) > hacd::HaF32 (0.0f));
j0 = j1;
j1 = j2;
}
}
#endif
}
return (count >= 3) ? count : 0;
}
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix* const initialGuess)
{
dgMatrix& mat = *this;
dgMatrix eigenVectors (dgGetIdentityMatrix());
if (initialGuess) {
eigenVectors = *initialGuess;
mat = eigenVectors.Transpose4X4() * mat * eigenVectors;
}
dgVector d (mat[0][0], mat[1][1], mat[2][2], dgFloat32 (0.0f));
dgVector b (d);
for (dgInt32 i = 0; i < 50; i++) {
dgFloat32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
if (sm < dgFloat32 (1.0e-12f)) {
// order the eigenvalue vectors
dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
if (tmp % eigenVectors.m_right < dgFloat32(0.0f)) {
dgAssert (0.0f);
eigenVectors.m_right = eigenVectors.m_right.Scale3 (-dgFloat32(1.0f));
}
break;
}
dgFloat32 thresh = dgFloat32 (0.0f);
if (i < 3) {
thresh = (dgFloat32)(0.2f / 9.0f) * sm;
}
dgVector z (dgVector::m_zero);
for (dgInt32 ip = 0; ip < 2; ip ++) {
for (dgInt32 iq = ip + 1; iq < 3; iq ++) {
dgFloat32 g = dgFloat32 (100.0f) * dgAbsf(mat[ip][iq]);
if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
mat[ip][iq] = dgFloat32 (0.0f);
} else if (dgAbsf(mat[ip][iq]) > thresh) {
dgFloat32 t;
dgFloat32 h = d[iq] - d[ip];
if (dgAbsf(h) + g == dgAbsf(h)) {
t = mat[ip][iq] / h;
} else {
dgFloat32 theta = dgFloat32 (0.5f) * h / mat[ip][iq];
t = dgFloat32(1.0f) / (dgAbsf(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
if (theta < dgFloat32 (0.0f)) {
t = -t;
}
}
dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t);
dgFloat32 s = t * c;
dgFloat32 tau = s / (dgFloat32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = dgFloat32(0.0f);
for (dgInt32 j = 0; j <= ip - 1; j ++) {
dgFloat32 g = mat[j][ip];
dgFloat32 h = mat[j][iq];
mat[j][ip] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = ip + 1; j <= iq - 1; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[j][iq];
mat[ip][j] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = iq + 1; j < 3; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[iq][j];
mat[ip][j] = g - s * (h + g * tau);
mat[iq][j] = h + s * (g - h * tau);
}
dgVector sv (s);
dgVector tauv (tau);
dgVector gv (eigenVectors[ip]);
dgVector hv (eigenVectors[iq]);
eigenVectors[ip] -= sv.CompProduct4 (hv + gv.CompProduct4(tauv));
eigenVectors[iq] += sv.CompProduct4 (gv - hv.CompProduct4(tauv));
}
}
}
b += z;
d = b;
}
eigenValues = d;
*this = eigenVectors;
}
