dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count = 0;
const dgFloat32 inclination = dgFloat32 (0.9999f);
if (normal.m_x < -inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else if (normal.m_x > inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
count = 1;
contactsOut[0] = SupportVertex (normal, NULL);
}
return count;
}
dgFloat32 dgCollisionDeformableMesh::CalculateSurfaceArea (const dgDeformableNode* const node0, const dgDeformableNode* const node1, dgVector& minBox, dgVector& maxBox) const
{
minBox = dgVector (dgMin (node0->m_minBox.m_x, node1->m_minBox.m_x), dgMin (node0->m_minBox.m_y, node1->m_minBox.m_y), dgMin (node0->m_minBox.m_z, node1->m_minBox.m_z), dgFloat32 (0.0f));
maxBox = dgVector (dgMax (node0->m_maxBox.m_x, node1->m_maxBox.m_x), dgMax (node0->m_maxBox.m_y, node1->m_maxBox.m_y), dgMax (node0->m_maxBox.m_z, node1->m_maxBox.m_z), dgFloat32 (0.0f));
dgVector side0 (maxBox - minBox);
dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
return side0 % side1;
}
void dgCollisionInstance::SetScale (const dgVector& scale)
{
dgFloat32 scaleX = dgAbs (scale.m_x);
dgFloat32 scaleY = dgAbs (scale.m_y);
dgFloat32 scaleZ = dgAbs (scale.m_z);
dgAssert (scaleX > dgFloat32 (0.0f));
dgAssert (scaleY > dgFloat32 (0.0f));
dgAssert (scaleZ > dgFloat32 (0.0f));
if (IsType(dgCollision::dgCollisionCompound_RTTI)) {
dgAssert (m_scaleType == m_unit);
dgCollisionCompound* const compound = (dgCollisionCompound*) m_childShape;
compound->ApplyScale(scale);
} else if ((dgAbs (scaleX - scaleY) < dgFloat32 (1.0e-4f)) && (dgAbs (scaleX - scaleZ) < dgFloat32 (1.0e-4f))) {
if ((dgAbs (scaleX - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f))) {
m_scaleType = m_unit;
m_scale = dgVector (dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
m_maxScale = m_scale;
m_invScale = m_scale;
} else {
m_scaleType = m_uniform;
m_scale = dgVector (scaleX, scaleX, scaleX, dgFloat32 (0.0f));
m_maxScale = m_scale;
m_invScale = dgVector (dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleX, dgFloat32 (0.0f));
}
} else {
m_scaleType = m_nonUniform;
m_maxScale = dgMax(scaleX, scaleY, scaleZ);
m_scale = dgVector (scaleX, scaleY, scaleZ, dgFloat32 (0.0f));
m_invScale = dgVector (dgFloat32 (1.0f) / scaleX, dgFloat32 (1.0f) / scaleY, dgFloat32 (1.0f) / scaleZ, dgFloat32 (0.0f));
}
}
void dgCollisionInstance::SetGlobalScale (const dgVector& scale)
{
if ((dgAbsf (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbsf (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) {
m_localMatrix.m_posit = m_localMatrix.m_posit.Scale3 (scale.m_x * m_invScale.m_x);
SetScale (scale);
} else {
// extract the original local matrix
dgMatrix localMatrix (m_aligmentMatrix * m_localMatrix);
// create a new scale matrix
localMatrix[0] = localMatrix[0].CompProduct4 (scale);
localMatrix[1] = localMatrix[1].CompProduct4 (scale);
localMatrix[2] = localMatrix[2].CompProduct4 (scale);
localMatrix[3] = localMatrix[3].CompProduct4 (scale);
localMatrix[3][3] = dgFloat32 (1.0f);
// decompose into to align * scale * local
localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix);
m_localMatrix = m_aligmentMatrix * m_localMatrix;
m_aligmentMatrix = m_aligmentMatrix.Transpose();
bool isIdentity = true;
for (dgInt32 i = 0; i < 3; i ++) {
isIdentity &= dgAbsf (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f);
isIdentity &= dgAbsf (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f);
}
m_scaleType = isIdentity ? m_nonUniform : m_global;
m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]);
m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f));
}
}
void dgSolver::CalculateForces()
{
m_firstPassCoef = dgFloat32(0.0f);
const dgInt32 passes = m_solverPasses;
const dgInt32 threadCounts = m_world->GetThreadCount();
for (dgInt32 step = 0; step < 4; step++) {
CalculateJointsAcceleration();
dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32(2.0f);
for (dgInt32 k = 0; (k < passes) && (accNorm > DG_SOLVER_MAX_ERROR); k++) {
CalculateJointsForce();
CalculateBodyForce();
accNorm = dgFloat32(0.0f);
for (dgInt32 i = 0; i < threadCounts; i++) {
accNorm = dgMax(accNorm, m_accelNorm[i]);
}
}
IntegrateBodiesVelocity();
}
UpdateForceFeedback();
dgInt32 hasJointFeeback = 0;
for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i++) {
hasJointFeeback |= m_hasJointFeeback[i];
}
CalculateBodiesAcceleration();
if (hasJointFeeback) {
UpdateKinematicFeedback();
}
}
void TriangleBox (const dgVector* const position, const dgInt32* const faceIndices, dgVector& minP, dgVector& maxP) const
{
minP = position[faceIndices[0]];
maxP = position[faceIndices[0]];
for (dgInt32 i = 1; i < 3; i ++) {
dgInt32 index = faceIndices[i];
const dgVector& p = position[index];
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
}
dgFloat64 dgSymmetricBiconjugateGradientSolve::Solve (dgInt32 size, dgFloat64 tolerance, dgFloat64* const x, const dgFloat64* const b) const
{
dgStack<dgFloat64> bufferR0(size);
dgStack<dgFloat64> bufferP0(size);
dgStack<dgFloat64> matrixTimesP0(size);
dgStack<dgFloat64> bufferConditionerInverseTimesR0(size);
dgFloat64* const r0 = &bufferR0[0];
dgFloat64* const p0 = &bufferP0[0];
dgFloat64* const MinvR0 = &bufferConditionerInverseTimesR0[0];
dgFloat64* const matrixP0 = &matrixTimesP0[0];
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
bool continueExecution = InversePrecoditionerTimeVector (p0, r0);
dgInt32 iter = 0;
dgFloat64 num = DotProduct (size, r0, p0);
dgFloat64 error2 = num;
for (dgInt32 j = 0; (j < size) && (error2 > tolerance) && continueExecution; j ++) {
MatrixTimeVector (matrixP0, p0);
dgFloat64 den = DotProduct (size, p0, matrixP0);
dgAssert (fabs(den) > dgFloat64 (0.0f));
dgFloat64 alpha = num / den;
ScaleAdd (size, x, x, alpha, p0);
if ((j % 50) != 49) {
ScaleAdd (size, r0, r0, -alpha, matrixP0);
} else {
MatrixTimeVector (matrixP0, x);
Sub(size, r0, b, matrixP0);
}
//dgUnsigned64 xxx0 = dgGetTimeInMicrosenconds();
continueExecution = InversePrecoditionerTimeVector (MinvR0, r0);
//xxx0 = dgGetTimeInMicrosenconds() - xxx0;
//dgTrace (("%d\n", dgUnsigned64 (xxx0)));
dgFloat64 num1 = DotProduct (size, r0, MinvR0);
dgFloat64 beta = num1 / num;
ScaleAdd (size, p0, MinvR0, beta, p0);
num = DotProduct (size, r0, MinvR0);
iter ++;
error2 = num;
if (j > 10) {
error2 = dgFloat64 (0.0f);
for (dgInt32 i = 0; i < size; i ++) {
error2 = dgMax (error2, r0[i] * r0[i]);
}
}
}
dgAssert (iter < size);
return num;
}
void dgCollisionInstance::SetGlobalScale (const dgVector& scale)
{
// calculate current matrix
dgMatrix matrix(dgGetIdentityMatrix());
matrix[0][0] = m_scale.m_x;
matrix[1][1] = m_scale.m_y;
matrix[2][2] = m_scale.m_z;
matrix = m_aligmentMatrix * matrix * m_localMatrix;
// extract the original local matrix
dgMatrix transpose (matrix.Transpose());
dgVector globalScale (dgSqrt (transpose[0].DotProduct(transpose[0]).GetScalar()), dgSqrt (transpose[1].DotProduct(transpose[1]).GetScalar()), dgSqrt (transpose[2].DotProduct(transpose[2]).GetScalar()), dgFloat32 (1.0f));
dgVector invGlobalScale (dgFloat32 (1.0f) / globalScale.m_x, dgFloat32 (1.0f) / globalScale.m_y, dgFloat32 (1.0f) / globalScale.m_z, dgFloat32 (1.0f));
dgMatrix localMatrix (m_aligmentMatrix.Transpose() * m_localMatrix);
localMatrix.m_posit = matrix.m_posit * invGlobalScale;
dgAssert (localMatrix.m_posit.m_w == dgFloat32 (1.0f));
if ((dgAbs (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbs (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) {
m_localMatrix = localMatrix;
m_localMatrix.m_posit = m_localMatrix.m_posit * scale | dgVector::m_wOne;
m_aligmentMatrix = dgGetIdentityMatrix();
SetScale (scale);
} else {
// create a new scale matrix
localMatrix[0] = localMatrix[0] * scale;
localMatrix[1] = localMatrix[1] * scale;
localMatrix[2] = localMatrix[2] * scale;
localMatrix[3] = localMatrix[3] * scale;
localMatrix[3][3] = dgFloat32 (1.0f);
// decompose into to align * scale * local
localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix);
m_localMatrix = m_aligmentMatrix * m_localMatrix;
m_aligmentMatrix = m_aligmentMatrix.Transpose();
dgAssert (m_localMatrix.TestOrthogonal());
dgAssert (m_aligmentMatrix.TestOrthogonal());
//dgMatrix xxx1 (dgGetIdentityMatrix());
//xxx1[0][0] = m_scale.m_x;
//xxx1[1][1] = m_scale.m_y;
//xxx1[2][2] = m_scale.m_z;
//dgMatrix xxx (m_aligmentMatrix * xxx1 * m_localMatrix);
bool isIdentity = true;
for (dgInt32 i = 0; i < 3; i ++) {
isIdentity &= dgAbs (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f);
isIdentity &= dgAbs (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f);
}
m_scaleType = isIdentity ? m_nonUniform : m_global;
m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]);
m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f));
}
}
void dgContact::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
dgJacobianMatrixElement* const rowMatrix = params->m_rowMatrix;
const dgVector& bodyVeloc0 = m_body0->m_veloc;
const dgVector& bodyOmega0 = m_body0->m_omega;
const dgVector& bodyVeloc1 = m_body1->m_veloc;
const dgVector& bodyOmega1 = m_body1->m_omega;
dgInt32 count = params->m_rowsCount;
dgFloat32 timestep = dgFloat32 (1.0f);
dgFloat32 invTimestep = dgFloat32 (1.0f);
if (params->m_timeStep > dgFloat32 (0.0f)) {
timestep = params->m_timeStep;
invTimestep = params->m_invTimeStep;
}
for (dgInt32 k = 0; k < count; k ++) {
// if (!rowMatrix[k].m_accelIsMotor)
// {
dgJacobianMatrixElement* const row = &rowMatrix[k];
dgVector relVeloc (row->m_Jt.m_jacobianM0.m_linear.CompProduct4(bodyVeloc0) + row->m_Jt.m_jacobianM0.m_angular.CompProduct4(bodyOmega0) + row->m_Jt.m_jacobianM1.m_linear.CompProduct4(bodyVeloc1) + row->m_Jt.m_jacobianM1.m_angular.CompProduct4(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dgFloat32 aRel = row->m_deltaAccel;
if (row->m_normalForceIndex < 0) {
dgFloat32 restitution = (vRel <= dgFloat32 (0.0f)) ? (dgFloat32 (1.0f) + row->m_restitution) : dgFloat32 (1.0f);
dgFloat32 penetrationVeloc = dgFloat32 (0.0f);
if (row->m_penetration > DG_RESTING_CONTACT_PENETRATION * dgFloat32 (0.125f)) {
if (vRel > dgFloat32 (0.0f)) {
dgFloat32 penetrationCorrection = vRel * timestep;
dgAssert (penetrationCorrection >= dgFloat32 (0.0f));
row->m_penetration = dgMax (dgFloat32 (0.0f), row->m_penetration - penetrationCorrection);
} else {
dgFloat32 penetrationCorrection = -vRel * timestep * row->m_restitution * dgFloat32 (8.0f);
if (penetrationCorrection > row->m_penetration) {
row->m_penetration = dgFloat32 (0.001f);
}
}
penetrationVeloc = -(row->m_penetration * row->m_penetrationStiffness);
}
vRel *= restitution;
vRel = dgMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
}
row->m_coordenateAccel = (aRel - vRel * invTimestep);
// }
}
}
void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgSoaFloat* const internalForces = (dgSoaFloat*)&m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_WORK_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
accNorm += accel2;
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
}
m_accelNorm[threadID] = accNorm;
}
void dgSolver::InitWeights()
{
DG_TRACKTIME();
const dgJointInfo* const jointArray = m_jointArray;
const dgInt32 jointCount = m_cluster->m_jointCount;
dgBodyProxy* const weight = m_bodyProxyArray;
memset(m_bodyProxyArray, 0, m_cluster->m_bodyCount * sizeof(dgBodyProxy));
for (dgInt32 i = 0; i < jointCount; i++) {
const dgJointInfo* const jointInfo = &jointArray[i];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
weight[m0].m_weight += dgFloat32(1.0f);
weight[m1].m_weight += dgFloat32(1.0f);
}
m_bodyProxyArray[0].m_weight = dgFloat32(1.0f);
dgFloat32 extraPasses = dgFloat32(0.0f);
const dgInt32 bodyCount = m_cluster->m_bodyCount;
dgSkeletonList& skeletonList = *m_world;
const dgInt32 lru = skeletonList.m_lruMarker;
skeletonList.m_lruMarker += 1;
m_skeletonCount = 0;
for (dgInt32 i = 1; i < bodyCount; i++) {
extraPasses = dgMax(weight[i].m_weight, extraPasses);
dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body;
dgSkeletonContainer* const container = body->GetSkeleton();
if (container && (container->GetLru() != lru)) {
container->SetLru(lru);
m_skeletonArray[m_skeletonCount] = container;
m_skeletonCount++;
}
}
const dgInt32 conectivity = 7;
m_solverPasses += 2 * dgInt32(extraPasses) / conectivity + 1;
}
void dgSolver::InitWeights()
{
const dgJointInfo* const jointArray = m_jointArray;
const dgInt32 jointCount = m_cluster->m_jointCount;
dgBodyProxy* const weight = m_bodyProxyArray;
memset(m_bodyProxyArray, 0, m_cluster->m_bodyCount * sizeof(dgBodyProxy));
for (dgInt32 i = 0; i < jointCount; i++) {
const dgJointInfo* const jointInfo = &jointArray[i];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
weight[m0].m_weight += dgFloat32(1.0f);
weight[m1].m_weight += dgFloat32(1.0f);
}
m_bodyProxyArray[0].m_weight = dgFloat32(1.0f);
dgFloat32 extraPasses = dgFloat32(0.0f);
const dgInt32 bodyCount = m_cluster->m_bodyCount;
for (dgInt32 i = 1; i < bodyCount; i++) {
extraPasses = dgMax(weight[i].m_weight, extraPasses);
}
const dgInt32 conectivity = 7;
m_solverPasses += 2 * dgInt32(extraPasses) / conectivity + 1;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
void dgWorld::SetSolverMode (dgInt32 mode)
{
m_solverMode = dgUnsigned32 (dgMax (0, mode));
}
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const
{
if (cluster->m_activeJointCount) {
SortClusters(cluster, timestep, threadID);
}
if (!cluster->m_isContinueCollision) {
if (cluster->m_activeJointCount) {
BuildJacobianMatrix (cluster, threadID, timestep);
CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
//CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
} else {
IntegrateExternalForce(cluster, timestep, threadID);
}
IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID);
} else {
// calculate reaction forces and new velocities
BuildJacobianMatrix (cluster, threadID, timestep);
IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR);
// see if the island goes to sleep
bool isAutoSleep = true;
bool stackSleeping = true;
dgInt32 sleepCounter = 10000;
dgWorld* const world = (dgWorld*) this;
const dgInt32 bodyCount = cluster->m_bodyCount;
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0];
dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart];
const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG;
dgFloat32 maxAccel = dgFloat32 (0.0f);
dgFloat32 maxAlpha = dgFloat32 (0.0f);
dgFloat32 maxSpeed = dgFloat32 (0.0f);
dgFloat32 maxOmega = dgFloat32 (0.0f);
const dgFloat32 speedFreeze = world->m_freezeSpeed2;
const dgFloat32 accelFreeze = world->m_freezeAccel2;
const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f));
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
dgAssert (body->m_invMass.m_w);
const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel);
const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha);
const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc);
const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega);
maxAccel = dgMax (maxAccel, accel2);
maxAlpha = dgMax (maxAlpha, alpha2);
maxSpeed = dgMax (maxSpeed, speed2);
maxOmega = dgMax (maxOmega, omega2);
bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze);
if (equilibrium) {
dgVector veloc (body->m_veloc * forceDampVect);
dgVector omega = body->m_omega * forceDampVect;
body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc;
body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega;
}
body->m_equilibrium = dgUnsigned32 (equilibrium);
stackSleeping &= equilibrium;
isAutoSleep &= body->m_autoSleep;
sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter);
}
// clear accel and angular acceleration
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
}
if (isAutoSleep) {
if (stackSleeping) {
// the island went to sleep mode,
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgBody* const body = bodyArray[i].m_body;
dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
body->m_veloc = dgVector::m_zero;
body->m_omega = dgVector::m_zero;
}
} else {
// island is not sleeping but may be resting with small residual velocity for a long time
// see if we can force to go to sleep
if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) ||
(maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) ||
(maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) ||
(maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) {
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->m_sleepingCounter = 0;
}
}
} else {
dgInt32 index = 0;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) &&
(maxAlpha <= world->m_sleepTable[i].m_maxAlpha) &&
(maxSpeed <= world->m_sleepTable[i].m_maxVeloc) &&
(maxOmega <= world->m_sleepTable[i].m_maxOmega)) {
index = i;
break;
}
}
dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep);
if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) {
// force island to sleep
stackSleeping = true;
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgBody* const body = bodyArray[i].m_body;
dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI));
body->m_accel = dgVector::m_zero;
body->m_alpha = dgVector::m_zero;
body->m_veloc = dgVector::m_zero;
body->m_omega = dgVector::m_zero;
body->m_equilibrium = true;
}
} else {
sleepCounter ++;
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->m_sleepingCounter = sleepCounter;
}
}
}
}
}
}
if (!(isAutoSleep & stackSleeping)) {
// island is not sleeping, need to integrate island velocity
const dgUnsigned32 lru = world->GetBroadPhase()->m_lru;
const dgInt32 jointCount = cluster->m_jointCount;
dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0];
dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];
dgFloat32 timeRemaining = timestep;
const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep;
for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) {
// calculate the closest time to impact
dgFloat32 timeToImpact = timeRemaining;
for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) {
dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
if (contact->GetId() == dgConstraint::m_contactConstraint) {
dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0;
dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1;
if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) {
dgVector p;
dgVector q;
dgVector normal;
timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f)));
}
}
}
if (timeToImpact > timeTol) {
timeRemaining -= timeToImpact;
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeToImpact);
body->UpdateWorlCollisionMatrix();
}
}
} else {
if (timeToImpact >= dgFloat32 (-1.0e-5f)) {
for (dgInt32 j = 1; j < bodyCount; j++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body;
if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeToImpact);
body->UpdateWorlCollisionMatrix();
}
}
}
CalculateClusterContacts (cluster, timeRemaining, lru, threadID);
BuildJacobianMatrix (cluster, threadID, 0.0f);
IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR);
bool clusterReceding = true;
const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS);
for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) {
dgFloat32 smallTimeStep = dgMin (step, timeRemaining);
timeRemaining -= smallTimeStep;
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity (smallTimeStep);
body->UpdateWorlCollisionMatrix();
}
}
clusterReceding = false;
if (timeRemaining > timeTol) {
CalculateClusterContacts (cluster, timeRemaining, lru, threadID);
bool isColliding = false;
for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) {
dgContact* const contact = (dgContact*) constraintArray[j].m_joint;
if (contact->GetId() == dgConstraint::m_contactConstraint) {
const dgBody* const body0 = contact->m_body0;
const dgBody* const body1 = contact->m_body1;
const dgVector& veloc0 = body0->m_veloc;
const dgVector& veloc1 = body1->m_veloc;
const dgVector& omega0 = body0->m_omega;
const dgVector& omega1 = body1->m_omega;
const dgVector& com0 = body0->m_globalCentreOfMass;
const dgVector& com1 = body1->m_globalCentreOfMass;
for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) {
const dgContactMaterial* const contactMaterial = &node->GetInfo();
dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0));
dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1));
dgVector vRel (vel0 - vel1);
dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f));
dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w;
isColliding |= (speed < dgFloat32 (0.0f));
}
}
}
clusterReceding = !isColliding;
}
}
}
}
if (timeRemaining > dgFloat32 (0.0)) {
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->IntegrateVelocity(timeRemaining);
body->UpdateCollisionMatrix (timeRemaining, threadID);
}
}
} else {
for (dgInt32 j = 1; j < bodyCount; j ++) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body;
if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) {
body->UpdateCollisionMatrix (timestep, threadID);
}
}
}
}
}
}
void dgWorld::SetFrictionThreshold (dgFloat32 acceleration)
{
m_frictiomTheshold = dgMax (dgFloat32(1.0e-2f), acceleration);
}
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
m_rtti |= dgCollisionSphere_RTTI;
m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE);
m_edgeCount = DG_SPHERE_EDGE_COUNT;
m_vertexCount = DG_SPHERE_VERTEX_COUNT;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgInt32 indexList[256];
dgVector tmpVectex[256];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 index = 1;
dgInt32 count = 0;
TesselateTriangle (index, p4, p0, p2, count, tmpVectex);
TesselateTriangle (index, p4, p2, p1, count, tmpVectex);
TesselateTriangle (index, p4, p1, p3, count, tmpVectex);
TesselateTriangle (index, p4, p3, p0, count, tmpVectex);
TesselateTriangle (index, p5, p2, p0, count, tmpVectex);
TesselateTriangle (index, p5, p1, p2, count, tmpVectex);
TesselateTriangle (index, p5, p3, p1, count, tmpVectex);
TesselateTriangle (index, p5, p0, p3, count, tmpVectex);
//dgAssert (count == EDGE_COUNT);
dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f);
dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
for (dgInt32 i = 0; i < vertexCount; i ++) {
m_unitSphere[i] = tmpVectex[i];
}
dgPolyhedra polyhedra(m_allocator);
polyhedra.BeginFace();
for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
dgAssert (edge);
#else
polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
#endif
}
polyhedra.EndFace();
dgUnsigned64 i1 = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i1;
i1 ++;
}
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];
}
}
for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
m_vertex[i] = m_unitSphere[i].Scale (m_radius);
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
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.Scale3 (-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.Scale3(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);
dgAssert (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];
SetMotorAcceleration (ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret ++;
} else if (angle.m_x < - m_twistAngle) {
dgVector p0 (matrix0.m_posit + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH));
InitPointParam (pointData, m_stiffness, p0, p0);
dgVector dir (matrix0.m_right.Scale3 (-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);
dgAssert (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.Scale3(MIN_JOINT_PIN_LENGTH));
InitPointParam (pointData, m_stiffness, p0, p0);
dgVector tangentDir (matrix0.m_front * matrix1.m_front);
tangentDir = tangentDir.Scale3 (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 (dgMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
dgAssert (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);
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_SOA_WORD_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
forceM0.m_linear.m_x = m_soaZero;
forceM0.m_linear.m_y = m_soaZero;
forceM0.m_linear.m_z = m_soaZero;
forceM0.m_angular.m_x = m_soaZero;
forceM0.m_angular.m_y = m_soaZero;
forceM0.m_angular.m_z = m_soaZero;
forceM1.m_linear.m_x = m_soaZero;
forceM1.m_linear.m_y = m_soaZero;
forceM1.m_linear.m_z = m_soaZero;
forceM1.m_angular.m_x = m_soaZero;
forceM1.m_angular.m_y = m_soaZero;
forceM1.m_angular.m_z = m_soaZero;
const dgInt32 rowsCount = jointInfo->m_pairCount;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[rowStart + j];
dgSoaFloat f(row->m_force);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, f);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, f);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, f);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, f);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, f);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, f);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, f);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, f);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, f);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, f);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, f);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, f);
}
dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgJacobian* const tempInternalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[m_cluster->m_bodyCount];
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgJacobian m_body0Force;
dgJacobian m_body1Force;
m_body0Force.m_linear = dgVector(forceM0.m_linear.m_x[j], forceM0.m_linear.m_y[j], forceM0.m_linear.m_z[j], dgFloat32(0.0f));
m_body0Force.m_angular = dgVector(forceM0.m_angular.m_x[j], forceM0.m_angular.m_y[j], forceM0.m_angular.m_z[j], dgFloat32(0.0f));
m_body1Force.m_linear = dgVector(forceM1.m_linear.m_x[j], forceM1.m_linear.m_y[j], forceM1.m_linear.m_z[j], dgFloat32(0.0f));
m_body1Force.m_angular = dgVector(forceM1.m_angular.m_x[j], forceM1.m_angular.m_y[j], forceM1.m_angular.m_z[j], dgFloat32(0.0f));
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
if (m0) {
dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock);
tempInternalForces[m0].m_linear += m_body0Force.m_linear;
tempInternalForces[m0].m_angular += m_body0Force.m_angular;
}
if (m1) {
dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock);
tempInternalForces[m1].m_linear += m_body1Force.m_linear;
tempInternalForces[m1].m_angular += m_body1Force.m_angular;
}
}
}
accNorm += accel2;
}
m_accelNorm[threadID] = accNorm;
}
dgMeshEffect * dgMeshEffect::CreateVoronoiConvexDecomposition (dgMemoryAllocator * const allocator, dgInt32 pointCount, dgInt32 pointStrideInBytes, const dgFloat32 * const pointCloud, dgInt32 materialId, const dgMatrix & textureProjectionMatrix)
{
dgFloat32 normalAngleInRadians = 30.0f * 3.1416f / 180.0f;
dgStack<dgBigVector> buffer (pointCount + 16);
dgBigVector * const pool = &buffer[0];
dgInt32 count = 0;
dgFloat64 quantizeFactor = dgFloat64 (16.0f);
dgFloat64 invQuantizeFactor = dgFloat64 (1.0f) / quantizeFactor;
dgInt32 stride = pointStrideInBytes / sizeof (dgFloat32);
dgBigVector pMin (dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (1.0e10f), dgFloat32 (0.0f));
dgBigVector pMax (dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (-1.0e10f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < pointCount; i ++)
{
dgFloat64 x = pointCloud[i * stride + 0];
dgFloat64 y = pointCloud[i * stride + 1];
dgFloat64 z = pointCloud[i * stride + 2];
x = floor (x * quantizeFactor) * invQuantizeFactor;
y = floor (y * quantizeFactor) * invQuantizeFactor;
z = floor (z * quantizeFactor) * invQuantizeFactor;
dgBigVector p (x, y, z, dgFloat64 (0.0f));
pMin = dgBigVector (dgMin (x, pMin.m_x), dgMin (y, pMin.m_y), dgMin (z, pMin.m_z), dgFloat64 (0.0f));
pMax = dgBigVector (dgMax (x, pMax.m_x), dgMax (y, pMax.m_y), dgMax (z, pMax.m_z), dgFloat64 (0.0f));
pool[count] = p;
count ++;
}
// add the bbox as a barrier
pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
count += 8;
dgStack<dgInt32> indexList (count);
count = dgVertexListToIndexList (&pool[0].m_x, sizeof (dgBigVector), 3, count, &indexList[0], dgFloat64 (5.0e-2f));
dgAssert (count >= 8);
dgFloat64 maxSize = dgMax (pMax.m_x - pMin.m_x, pMax.m_y - pMin.m_y, pMax.m_z - pMin.m_z);
pMin -= dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
pMax += dgBigVector (maxSize, maxSize, maxSize, dgFloat64 (0.0f));
// add the a guard zone, so that we do no have to clip
dgInt32 guadVertexKey = count;
pool[count + 0] = dgBigVector ( pMin.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 1] = dgBigVector ( pMax.m_x, pMin.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 2] = dgBigVector ( pMin.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 3] = dgBigVector ( pMax.m_x, pMax.m_y, pMin.m_z, dgFloat64 (0.0f));
pool[count + 4] = dgBigVector ( pMin.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 5] = dgBigVector ( pMax.m_x, pMin.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 6] = dgBigVector ( pMin.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
pool[count + 7] = dgBigVector ( pMax.m_x, pMax.m_y, pMax.m_z, dgFloat64 (0.0f));
count += 8;
dgDelaunayTetrahedralization delaunayTetrahedras (allocator, &pool[0].m_x, count, sizeof (dgBigVector), dgFloat32 (0.0f));
delaunayTetrahedras.RemoveUpperHull ();
// delaunayTetrahedras.Save("xxx0.txt");
dgInt32 tetraCount = delaunayTetrahedras.GetCount();
dgStack<dgBigVector> voronoiPoints (tetraCount + 32);
dgStack<dgDelaunayTetrahedralization::dgListNode *> tetradrumNode (tetraCount);
dgTree<dgList<dgInt32>, dgInt32> delanayNodes (allocator);
dgInt32 index = 0;
const dgHullVector * const delanayPoints = delaunayTetrahedras.GetHullVertexArray();
for (dgDelaunayTetrahedralization::dgListNode * node = delaunayTetrahedras.GetFirst(); node; node = node->GetNext())
{
dgConvexHull4dTetraherum & tetra = node->GetInfo();
voronoiPoints[index] = tetra.CircumSphereCenter (delanayPoints);
tetradrumNode[index] = node;
for (dgInt32 i = 0; i < 4; i ++)
{
dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * header = delanayNodes.Find (tetra.m_faces[0].m_index[i]);
if (!header)
{
dgList<dgInt32> list (allocator);
header = delanayNodes.Insert (list, tetra.m_faces[0].m_index[i]);
}
header->GetInfo().Append (index);
}
index ++;
}
dgMeshEffect * const voronoiPartition = new (allocator) dgMeshEffect (allocator);
voronoiPartition->BeginPolygon();
dgFloat64 layer = dgFloat64 (0.0f);
dgTree<dgList<dgInt32>, dgInt32>::Iterator iter (delanayNodes);
for (iter.Begin(); iter; iter ++)
{
dgTree<dgList<dgInt32>, dgInt32>::dgTreeNode * const nodeNode = iter.GetNode();
const dgList<dgInt32> & list = nodeNode->GetInfo();
dgInt32 key = nodeNode->GetKey();
if (key < guadVertexKey)
{
dgBigVector pointArray[512];
dgInt32 indexArray[512];
dgInt32 count = 0;
for (dgList<dgInt32>::dgListNode * ptr = list.GetFirst(); ptr; ptr = ptr->GetNext())
{
dgInt32 i = ptr->GetInfo();
pointArray[count] = voronoiPoints[i];
count ++;
dgAssert (count < dgInt32 (sizeof (pointArray) / sizeof (pointArray[0])));
}
count = dgVertexListToIndexList (&pointArray[0].m_x, sizeof (dgBigVector), 3, count, &indexArray[0], dgFloat64 (1.0e-3f));
if (count >= 4)
{
dgMeshEffect convexMesh (allocator, &pointArray[0].m_x, count, sizeof (dgBigVector), dgFloat64 (0.0f));
if (convexMesh.GetCount())
{
convexMesh.CalculateNormals (normalAngleInRadians);
convexMesh.UniformBoxMapping (materialId, textureProjectionMatrix);
for (dgInt32 i = 0; i < convexMesh.m_pointCount; i ++)
convexMesh.m_points[i].m_w = layer;
for (dgInt32 i = 0; i < convexMesh.m_atribCount; i ++)
convexMesh.m_attrib[i].m_vertex.m_w = layer;
voronoiPartition->MergeFaces (&convexMesh);
layer += dgFloat64 (1.0f);
}
}
}
}
voronoiPartition->EndPolygon (dgFloat64 (1.0e-8f), false);
// voronoiPartition->SaveOFF("xxx0.off");
//voronoiPartition->ConvertToPolygons();
return voronoiPartition;
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceJacobi1(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgVector accNorm(dgVector::m_zero);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
cacheForce[0] = dgFloat32(1.0f);
cacheForce[1] = dgFloat32(1.0f);
cacheForce[2] = dgFloat32(1.0f);
cacheForce[3] = dgFloat32(1.0f);
dgFloat32* const normalForce = &cacheForce[4];
dgVector maxAccel(1.0e10f);
dgFloat32 prevError = dgFloat32(1.0e20f);
dgVector firstPass(dgVector::m_one);
for (dgInt32 j = 0; (j < 5) && (maxAccel.GetScalar() > restAcceleration) && (prevError - maxAccel.GetScalar()) > dgFloat32(1.0e-2f); j++) {
prevError = maxAccel.GetScalar();
maxAccel = dgVector::m_zero;
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
dgVector diag(row->m_JMinv.m_jacobianM0.m_linear * linearM0 + row->m_JMinv.m_jacobianM0.m_angular * angularM0 +
row->m_JMinv.m_jacobianM1.m_linear * linearM1 + row->m_JMinv.m_jacobianM1.m_angular * angularM1);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
const dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
const dgFloat32 frictionNormal = normalForce[frictionIndex];
dgVector lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgVector upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
dgVector accelAbs(accel.Abs());
maxAccel = maxAccel.GetMax(accelAbs);
accNorm = accNorm.GetMax(accelAbs * firstPass);
dgAssert(maxAccel.m_x >= dgAbsf(accel.m_x));
dgFloat32 f = (force.GetMax(lowerFrictionForce).GetMin(upperFrictionForce)).GetScalar();
dgVector deltaForce(f - row->m_force);
row->m_force = f;
normalForce[i] = f;
dgVector deltaforce0(scale0 * deltaForce);
dgVector deltaforce1(scale1 * deltaForce);
linearM0 += row->m_Jt.m_jacobianM0.m_linear * deltaforce0;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * deltaforce0;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * deltaforce1;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * deltaforce1;
}
firstPass = dgVector::m_zero;
}
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
row->m_maxImpact = dgMax(dgAbsf(row->m_force), row->m_maxImpact);
}
return accNorm.GetScalar();
}
void dgCollisionCone::Init (dgFloat32 radius, dgFloat32 height)
{
m_rtti |= dgCollisionCone_RTTI;
m_radius = dgMax(dgAbsf(radius), dgFloat32(2.0f) * D_CONE_SKIN_THINCKNESS);
m_height = dgMax (dgAbsf (height * dgFloat32 (0.5f)), dgFloat32 (2.0f) * D_CONE_SKIN_THINCKNESS);
dgFloat32 angle = dgFloat32(0.0f);
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i++) {
dgFloat32 sinAngle = dgSin(angle);
dgFloat32 cosAngle = dgCos(angle);
m_vertex[i] = dgVector(-m_height, m_radius * cosAngle, m_radius * sinAngle, dgFloat32(0.0f));
angle += dgPI2 / DG_CONE_SEGMENTS;
}
m_vertex[DG_CONE_SEGMENTS] = dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
m_edgeCount = DG_CONE_SEGMENTS * 4;
m_vertexCount = DG_CONE_SEGMENTS + 1;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CONE_SEGMENTS];
dgInt32 j = DG_CONE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = DG_CONE_SEGMENTS;
j = i;
polyhedra.AddFace (3, wireframe);
}
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[i] = DG_CONE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CONE_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_profile[0] = dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
m_profile[1] = dgVector(-m_height, m_radius, dgFloat32(0.0f), dgFloat32(0.0f));
m_profile[2] = dgVector(-m_height, -m_radius, dgFloat32(0.0f), dgFloat32(0.0f));
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z)
{
m_rtti |= dgCollisionBox_RTTI;
m_size[0].m_x = dgMax (dgAbsf (size_x) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_y = dgMax (dgAbsf (size_y) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_z = dgMax (dgAbsf (size_z) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
m_size[0].m_w = dgFloat32 (0.0f);
m_size[1].m_x = - m_size[0].m_x;
m_size[1].m_y = - m_size[0].m_y;
m_size[1].m_z = - m_size[0].m_z;
m_size[1].m_w = dgFloat32 (0.0f);
m_edgeCount = 24;
m_vertexCount = 8;
m_vertex[0] = dgVector ( m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[1] = dgVector (-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[2] = dgVector ( m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[3] = dgVector (-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[4] = dgVector ( m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[5] = dgVector (-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[6] = dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[7] = dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
dgCollisionConvex::m_vertex = m_vertex;
dgCollisionConvex::m_simplex = m_edgeArray;
if (!m_initSimplex) {
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgInt32 i = 0; i < 6; i ++) {
polyhedra.AddFace (4, &m_faces[i][0]);
}
polyhedra.EndFace();
int index = 0;
dgInt32 mark = polyhedra.IncLRU();;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = index;
index ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
dgAssert (index == 24);
polyhedra.IncLRU();
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = ptr->m_incidentVertex;
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData];
}
dgInt32 count = 0;
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
edge->m_mark = mark;
edge->m_twin->m_mark = mark;
m_edgeEdgeMap[count] = &m_simplex[edge->m_userData];
count ++;
dgAssert (count <= 12);
}
}
m_initSimplex = 1;
}
SetVolumeAndCG ();
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * 2 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
// this is a degenerated hull hull to add some thickness and for a thick plane
delete convexHull;
dgStack<dgVector> tmp(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
tmp[i][0] = dgFloat32 (buffer[i*3 + 0]);
tmp[i][1] = dgFloat32 (buffer[i*3 + 1]);
tmp[i][2] = dgFloat32 (buffer[i*3 + 2]);
tmp[i][2] = dgFloat32 (0.0f);
}
dgObb sphere;
sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count);
dgInt32 index = 0;
dgFloat32 size = dgFloat32 (1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (sphere.m_size[i] < size) {
index = i;
size = sphere.m_size[i];
}
}
dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
normal[index] = dgFloat32 (1.0f);
dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f)));
for (dgInt32 i = 0; i < count; i ++) {
dgVector p1 (tmp[i] + step);
dgVector p2 (tmp[i] - step);
buffer[i * 3 + 0] = p1.m_x;
buffer[i * 3 + 1] = p1.m_y;
buffer[i * 3 + 2] = p1.m_z;
buffer[(i + count) * 3 + 0] = p2.m_x;
buffer[(i + count) * 3 + 1] = p2.m_y;
buffer[(i + count) * 3 + 2] = p2.m_z;
}
count *= 2;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
dgAssert (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgAssert (convexHull);
dgInt32 vertexCount = convexHull->GetVertexCount();
if (vertexCount < 4) {
delete convexHull;
return false;
}
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
// bool edgeRemoved = false;
// while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
// edgeRemoved = true;
// }
// if (edgeRemoved) {
// if (!CheckConvex (polyhedra, hullVertexArray)) {
// delete convexHull;
// return false;
// }
// }
while (RemoveCoplanarEdge (polyhedra, hullVertexArray));
}
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
if (vertexMap[edge->m_incidentVertex] == -1) {
vertexMap[edge->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f);
}
}
delete convexHull;
vertexCount = m_vertexCount;
mark = polyhedra.IncLRU();;
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_faceCount = 0;
dgStack<char> faceMarks (m_edgeCount);
memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!faceMarks[i]) {
dgConvexSimplexEdge* ptr = face;
do {
dgAssert ((ptr - m_simplex) >= 0);
faceMarks[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
if (vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
// create a face structure for support vertex
dgStack<dgConvexBox> boxTree (vertexCount);
dgTree<dgVector,dgInt32> sortTree(GetAllocator());
dgStack<dgTree<dgVector,dgInt32>::dgTreeNode*> vertexNodeList(vertexCount);
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < vertexCount; i ++) {
const dgVector& p = m_vertex[i];
vertexNodeList[i] = sortTree.Insert (p, i);
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
boxTree[0].m_box[0] = minP;
boxTree[0].m_box[1] = maxP;
boxTree[0].m_leftBox = -1;
boxTree[0].m_rightBox = -1;
boxTree[0].m_vertexStart = 0;
boxTree[0].m_vertexCount = vertexCount;
dgInt32 boxCount = 1;
dgInt32 stack = 1;
dgInt32 stackBoxPool[64];
stackBoxPool[0] = 0;
while (stack) {
stack --;
dgInt32 boxIndex = stackBoxPool[stack];
dgConvexBox& box = boxTree[boxIndex];
if (box.m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (box.m_vertexCount)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = box.m_vertexCount - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = box.m_vertexCount / 2;
}
if (i0 >= (box.m_vertexCount - 1)){
i0 = box.m_vertexCount / 2;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = i0; i < box.m_vertexCount; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_rightBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0;
boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < i0; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_leftBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart;
boxTree[boxCount].m_vertexCount = i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
}
}
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
m_vertex[i] = vertexNodeList[i]->GetInfo();
vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i);
}
m_supportTreeCount = boxCount;
m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox)));
memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const ptr = &m_simplex[i];
dgTree<dgVector,dgInt32>::dgTreeNode* const node = sortTree.Find(ptr->m_vertex);
dgInt32 index = dgInt32 (node->GetInfo().m_w);
ptr->m_vertex = dgInt16 (index);
}
}
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const edge = &m_simplex[i];
m_vertexToEdgeMapping[edge->m_vertex] = edge;
}
SetVolumeAndCG ();
return true;
}
void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
dgPointParam pointData;
dgFloat32 impulseOrForceScale = (params.m_timestep > dgFloat32 (0.0f)) ? params.m_invTimestep : dgFloat32 (1.0f);
InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData);
dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
dgFloat32 restitution = contact.m_restitution;
dgFloat32 relVelocErr = velocError % contact.m_normal;
dgFloat32 penetration = dgClamp (contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f));
dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness;
dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
dgAssert (dgAbsf (penetrationVeloc - MAX_PENETRATION_STIFFNESS * contact.m_softness * penetration) < dgFloat32 (1.0e-6f));
if (relVelocErr > REST_RELATIVE_VELOCITY) {
relVelocErr *= (restitution + dgFloat32 (1.0f));
}
params.m_restitution[normalIndex] = restitution;
params.m_penetration[normalIndex] = penetration;
params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*) &contact.m_normal_Force;
params.m_jointStiffness[normalIndex] = dgFloat32 (1.0f);
params.m_isMotor[normalIndex] = 0;
// params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
params.m_jointAccel[normalIndex] = dgMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * impulseOrForceScale;
if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) {
params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force;
}
// first dir friction force
if (contact.m_flags & dgContactMaterial::m_friction0Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData);
relVelocErr = velocError % contact.m_dir0;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override0Accel) {
if (contact.m_flags & dgContactMaterial::m_override0Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
params.m_isMotor[jacobIndex] = 1;
} else {
//params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
} else {
params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir0_Force;
}
// if (contact.m_friction1Enable) {
if (contact.m_flags & dgContactMaterial::m_friction1Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData);
relVelocErr = velocError % contact.m_dir1;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override1Accel) {
if (contact.m_flags & dgContactMaterial::m_override1Accel) {
dgAssert (0);
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
params.m_isMotor[jacobIndex] = 1;
} else {
//params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
} else {
params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir1_Force;
}
//dgTrace (("p(%f %f %f)\n", params.m_jointAccel[normalIndex], params.m_jointAccel[normalIndex + 1], params.m_jointAccel[normalIndex + 2]));
}
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;
}
void dgWorldDynamicUpdate::CalculateForcesGameModeParallel (dgParallelSolverSyncData* const syncData) const
{
dgWorld* const world = (dgWorld*) this;
const dgInt32 threadCounts = world->GetThreadCount();
const dgInt32 passes = syncData->m_passes;
const dgInt32 maxPasses = syncData->m_maxPasses;
const dgInt32 batchCount = syncData->m_bachCount;
syncData->m_firstPassCoef = dgFloat32 (0.0f);
for (dgInt32 step = 0; step < maxPasses; step ++) {
syncData->m_atomicIndex = 0;
for (dgInt32 i = 0; i < threadCounts; i ++) {
world->QueueJob (CalculateJointsAccelParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
syncData->m_firstPassCoef = dgFloat32 (1.0f);
dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32 (2.0f);
for (dgInt32 k = 0; (k < passes) && (accNorm > DG_SOLVER_MAX_ERROR); k ++) {
dgInt32 batchIndex = 0;
dgInt32 count = syncData->m_jointBatches[batchIndex + 1];
while ((batchIndex < batchCount) && (count >= threadCounts * 8)) {
syncData->m_bachIndex = syncData->m_jointBatches[batchIndex + 1];
syncData->m_atomicIndex = syncData->m_jointBatches[batchIndex];
for (dgInt32 i = 0; i < threadCounts; i ++) {
world->QueueJob (CalculateJointsForceParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
accNorm = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < threadCounts; i ++) {
accNorm = dgMax (accNorm, syncData->m_accelNorm[i]);
}
batchIndex++;
count = syncData->m_jointBatches[batchIndex + 1] - syncData->m_jointBatches[batchIndex];
}
if (batchIndex < batchCount) {
syncData->m_bachIndex = syncData->m_jointBatches[batchCount];
syncData->m_atomicIndex = syncData->m_jointBatches[batchIndex];
CalculateJointsForceParallelKernel (syncData, world, 0);
accNorm = dgMax(accNorm, syncData->m_accelNorm[0]);
}
}
syncData->m_atomicIndex = 1;
for (dgInt32 j = 0; j < threadCounts; j ++) {
world->QueueJob (CalculateJointsVelocParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
}
if (syncData->m_timestepRK != dgFloat32 (0.0f)) {
syncData->m_atomicIndex = 0;
for (dgInt32 j = 0; j < threadCounts; j ++) {
world->QueueJob (UpdateFeedbackForcesParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
dgInt32 hasJointFeeback = 0;
for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i ++) {
hasJointFeeback |= syncData->m_hasJointFeeback[i];
}
syncData->m_atomicIndex = 1;
for (dgInt32 j = 0; j < threadCounts; j++) {
world->QueueJob(UpdateBodyVelocityParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
if (hasJointFeeback) {
syncData->m_atomicIndex = 0;
for (dgInt32 j = 0; j < threadCounts; j++) {
world->QueueJob(KinematicCallbackUpdateParallelKernel, syncData, world);
}
world->SynchronizationBarrier();
}
} else {
const dgInt32 count = syncData->m_bodyCount;
const dgIsland* const island = syncData->m_island;
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*)&world->m_bodiesMemory[0];
dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
for (dgInt32 i = 1; i < count; i++) {
dgBody* const body = bodyArray[i].m_body;
if (body->m_active) {
body->m_netForce = dgVector::m_zero;
body->m_netTorque = dgVector::m_zero;
}
}
}
}
dgAABBPointTree4d* dgConvexHull4d::BuildTree (dgAABBPointTree4d* const parent, dgHullVector* const points, dgInt32 count, dgInt32 baseIndex, dgInt8** memoryPool, dgInt32& maxMemSize) const
{
dgAABBPointTree4d* tree = NULL;
dgAssert (count);
dgBigVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f));
dgBigVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f));
if (count <= DG_VERTEX_CLUMP_SIZE_4D) {
dgAABBPointTree4dClump* const clump = new (*memoryPool) dgAABBPointTree4dClump;
*memoryPool += sizeof (dgAABBPointTree4dClump);
maxMemSize -= sizeof (dgAABBPointTree4dClump);
dgAssert (maxMemSize >= 0);
dgAssert (clump);
clump->m_count = count;
for (dgInt32 i = 0; i < count; i ++) {
clump->m_indices[i] = i + baseIndex;
const dgBigVector& p = points[i];
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
minP.m_w = dgMin (p.m_w, minP.m_w);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
maxP.m_w = dgMax (p.m_w, maxP.m_w);
}
clump->m_left = NULL;
clump->m_right = NULL;
tree = clump;
} else {
dgBigVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgBigVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < count; i ++) {
const dgBigVector& p = points[i];
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
minP.m_w = dgMin (p.m_w, minP.m_w);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
maxP.m_w = dgMax (p.m_w, maxP.m_w);
median = median + p;
varian = varian + p.CompProduct4(p);
}
varian = varian.Scale4 (dgFloat32 (count)) - median.CompProduct4(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 4; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgBigVector center = median.Scale4 (dgFloat64 (1.0f) / dgFloat64 (count));
dgFloat64 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat64 val = points[i0][index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat64 val = points[i1][index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(points[i0], points[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = count / 2;
}
if (i0 >= (count - 1)){
i0 = count / 2;
}
tree = new (*memoryPool) dgAABBPointTree4d;
*memoryPool += sizeof (dgAABBPointTree4d);
maxMemSize -= sizeof (dgAABBPointTree4d);
dgAssert (maxMemSize >= 0);
dgAssert (i0);
dgAssert (count - i0);
tree->m_left = BuildTree (tree, points, i0, baseIndex, memoryPool, maxMemSize);
tree->m_right = BuildTree (tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize);
}
dgAssert (tree);
tree->m_parent = parent;
tree->m_box[0] = minP - dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f));
tree->m_box[1] = maxP + dgBigVector (dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f), dgFloat64 (1.0e-3f));
return tree;
}
void dgWorld::SetContactMergeTolerance(dgFloat32 tolerenace)
{
m_contactTolerance = dgMax (tolerenace, dgFloat32 (1.e-3f));
}
