dgCollision::dgCollision (dgWorld* const world, dgDeserialize deserialization, void* const userData)
{
dgInt32 signature[4];
deserialization (userData, &signature, sizeof (signature));
deserialization (userData, &m_offset, sizeof (dgMatrix));
m_rtti = 0;
m_refCount = 1;
m_signature = dgUnsigned32 (signature[0]);
m_userDataID = dgUnsigned32 (signature[2]);
m_allocator = world->GetAllocator();
m_collsionId = dgCollisionID (signature[1]);
}
dgUnsigned32 NewtonUserJoint::JacobianDerivative (dgContraintDescritor & params)
{
m_rows = 0;
m_param = ¶ms;
m_jacobianFnt ((NewtonJoint *)this, params.m_timestep, params.m_threadIndex);
return dgUnsigned32 (m_rows);
}
void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgAssert (collision);
m_bodiesUniqueID ++;
body->m_world = this;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
body->SetLinearDamping (dgFloat32 (0.1045f)) ;
body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));
body->AttachCollision(collision);
body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);
dgMatrix inertia (dgGetIdentityMatrix());
inertia[0][0] = DG_INFINITE_MASS;
inertia[1][1] = DG_INFINITE_MASS;
inertia[2][2] = DG_INFINITE_MASS;
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), inertia);
body->SetMatrix (matrix);
if (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)) {
m_broadPhase->Add (body);
}
}
dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
dgVector lateralDir (matrix0.m_front * matrix1.m_front);
dgInt32 ret = 0;
dgFloat32 mag = lateralDir % lateralDir;
if (mag > dgFloat32 (1.0e-6f)) {
mag = dgSqrt (mag);
lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag);
dgFloat32 angle = dgAsin (mag);
CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]);
dgVector frontDir (lateralDir * matrix1.m_front);
CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
} else {
CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]);
CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
}
return dgUnsigned32 (ret);
}
void dgWorld::SetThreadsCount (dgInt32 count)
{
//count = 1;
m_threadsManager.CreateThreaded (count);
m_numberOfTheads = dgUnsigned32 (m_threadsManager.GetThreadCount());
}
dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
m_angle = -angle.m_x;
dgAssert (dgAbsf (1.0f - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (1.0f - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (1.0f - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f));
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;
dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
// dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f);
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]);
CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]);
dgInt32 ret = 5;
if (m_jointAccelFnt) {
dgJointCallbackParam axisParam;
axisParam.m_accel = dgFloat32 (0.0f);
axisParam.m_timestep = params.m_timestep;
axisParam.m_minFriction = DG_MIN_BOUND;
axisParam.m_maxFriction = DG_MAX_BOUND;
if (m_jointAccelFnt (*this, &axisParam)) {
if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[5].m_low = axisParam.m_minFriction;
params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculateAngularDerivative (5, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[5]);
// params.m_jointAccel[5] = axisParam.m_accel;
SetMotorAcceleration (5, axisParam.m_accel, params);
ret = 6;
}
}
return dgUnsigned32 (ret);
}
dgBody* dgWorld::CreateBody(dgCollision* const collision, const dgMatrix& matrix)
{
dgBody* body;
_ASSERTE (collision);
body = new (m_allocator) dgBody();
_ASSERTE ((sizeof (dgBody) & 0xf) == 0);
_ASSERTE ((dgUnsigned64 (body) & 0xf) == 0);
memset (body, 0, sizeof (dgBody));
// m_bodiesCount ++;
m_bodiesUniqueID ++;
body->m_world = this;
body->m_freeze = false;
body->m_sleeping = false;
body->m_autoSleep = true;
body->m_isInWorld = true;
body->m_equilibrium = false;
body->m_continueCollisionMode = false;
body->m_collideWithLinkedBodies = true;
body->m_solverInContinueCollision = false;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
// dgBodyActiveList___::AddBody(body);
// _ASSERTE (body->m_activeNode);
// body->m_freezeAccel2 = m_freezeAccel2;
// body->m_freezeAlpha2 = m_freezeAlpha2;
// body->m_freezeSpeed2 = m_freezeSpeed2;
// body->m_freezeOmega2 = m_freezeOmega2;
body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
body->SetLinearDamping (dgFloat32 (0.1045f)) ;
body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));
body->AttachCollision(collision);
body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
dgBroadPhaseCollision::Add (body);
//body->SetMatrix (dgGetIdentityMatrix());
body->SetMatrix (matrix);
body->m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
return body;
}
dgUnsigned32 dgThreads::GetPerfomanceTicks(dgUnsigned32 threadIndex) const
{
if (dgInt32(threadIndex) <= m_numOfThreads)
{
return dgUnsigned32(m_localData[threadIndex].m_ticks);
}
else
{
return 0;
}
}
dgUserConstraint::dgUserConstraint(dgWorld* const world, dgBody* const body0, dgBody* const body1, dgInt32 constraintID)
:dgBilateralConstraint()
{
m_maxDOF = 6;
m_enableCollision = false;
m_constId = dgUnsigned32 (m_unknownConstraint + constraintID);
m_body0 = body0;
m_body1 = body1;
m_userData = NULL;
m_destructor = NULL;
world->AttachConstraint(this, body0, body1);
}
dgUnsigned32 dgContact::JacobianDerivative (dgContraintDescritor& params)
{
dgInt32 frictionIndex = 0;
if (m_maxDOF) {
dgInt32 i = 0;
frictionIndex = GetCount();
for (dgList<dgContactMaterial>::dgListNode* node = GetFirst(); node; node = node->GetNext()) {
const dgContactMaterial& contact = node->GetInfo();
JacobianContactDerivative (params, contact, i, frictionIndex);
i ++;
}
}
return dgUnsigned32 (frictionIndex);
}
void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgAssert (collision);
m_bodiesUniqueID ++;
body->m_world = this;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
body->SetLinearDamping (dgFloat32 (0.1045f)) ;
body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));
body->AttachCollision(collision);
body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
body->SetMatrix (matrix);
m_broadPhase->Add (body);
}
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 relVelocErr;
dgFloat32 penetrationErr;
dgMatrix matrix0;
dgMatrix matrix1;
if (m_jointUserCallback)
{
m_jointUserCallback(*this, params.m_timestep);
}
dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
m_angles = angle.Scale(-dgFloat32(1.0f));
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgPointParam pointData;
InitPointParam(pointData, m_stiffness, p0, p1);
CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
ret = 3;
if (m_twistLimit)
{
if (angle.m_x > m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
const dgVector& dir = matrix0.m_right;
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
else if (angle.m_x < -m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
if (m_coneLimit)
{
dgFloat32 coneCos;
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_coneAngleCos)
{
dgVector p0(
matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector tangentDir(matrix0.m_front * matrix1.m_front);
tangentDir = tangentDir.Scale(
dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
CalculatePointDerivative(ret, params, tangentDir, pointData,
&m_jointForce[ret]);
ret++;
dgVector normalDir(tangentDir * matrix0.m_front);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
//restitution = contact.m_restitution;
relVelocErr = velocError % normalDir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH
* (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
CalculatePointDerivative(ret, params, normalDir, pointData,
&m_jointForce[ret]);
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
return dgUnsigned32(ret);
}
dgInt32 dgPolygonSoupDatabaseBuilder::AddConvexFace(dgInt32 count,
dgInt32* const pool, dgInt32* const facesArray)
{
dgPolySoupFilterAllocator polyhedra(m_allocator);
count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0)
{
edge = edge->m_twin;
}
dgInt32 isconvex = 1;
dgInt32 facesCount = 0;
dgInt32 flag = 1;
while (flag)
{
flag = 0;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
do
{
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
dgFloat64 mag2 = e0 % e0;
if (mag2 < dgFloat32(1.0e-6f))
{
count--;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3)
{
flag = 1;
while (flag)
{
flag = 0;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = e1 % e0;
if (mag2 > dgFloat32(0.9999f))
{
count--;
flag = 1;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat32(1.0e-8f))
{
return 0;
}
normal = normal.Scale(dgRsqrt (mag2));
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgBigVector n(e0 * e1);
dgFloat64 mag2 = n % normal;
if (mag2 < dgFloat32(1.0e-5f))
{
isconvex = 0;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (isconvex)
{
dgEdge* const first = edge;
if (count >= 3)
{
count = 0;
dgEdge* ptr = first;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != first);
facesArray[facesCount] = count;
facesCount = 1;
}
}
else
{
dgPolyhedra leftOver(m_allocator);
dgPolyhedra polyhedra2(m_allocator);
dgEdge* ptr = edge;
count = 0;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != edge);
polyhedra2.BeginFace();
polyhedra2.AddFace(count, pool);
polyhedra2.EndFace();
leftOver.BeginFace();
polyhedra2.ConvexPartition(&m_vertexPoints[0].m_x, sizeof(dgTriplex),
&leftOver);
leftOver.EndFace();
_ASSERTE(leftOver.GetCount() == 0);
dgInt32 mark = polyhedra2.IncLRU();
dgInt32 index = 0;
dgPolyhedra::Iterator iter(polyhedra2);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
ptr = edge;
count = 0;
do
{
ptr->m_mark = mark;
pool[index] = ptr->m_incidentVertex;
index++;
count++;
ptr = ptr->m_next;
} while (ptr != edge);
facesArray[facesCount] = count;
facesCount++;
}
}
return facesCount;
}
void dgBallConstraint::SetConeLimitState(bool state)
{
m_coneLimit = dgUnsigned32(state);
}
void dgBallConstraint::SetLatealLimitState(bool state)
{
m_lateralLimit = dgUnsigned32(state);
}
dgUnsigned32 dgCorkscrewConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
m_angle = -angle.m_x;
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
matrix1.m_posit += matrix1.m_front.Scale3 (m_posit);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f));
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
// const dgVector& p0 = matrix0.m_posit;
// const dgVector& p1 = matrix1.m_posit;
dgVector p0 (matrix0.m_posit);
dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front));
dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]);
dgInt32 ret = 4;
if (m_jointAccelFnt) {
dgUnsigned32 code;
dgJointCallbackParam axisParam[2];
// linear acceleration
axisParam[0].m_accel = dgFloat32 (0.0f);
axisParam[0].m_timestep = params.m_timestep;
axisParam[0].m_minFriction = DG_MIN_BOUND;
axisParam[0].m_maxFriction = DG_MAX_BOUND;
// angular acceleration
axisParam[1].m_accel = dgFloat32 (0.0f);
axisParam[1].m_timestep = params.m_timestep;
axisParam[1].m_minFriction = DG_MIN_BOUND;
axisParam[1].m_maxFriction = DG_MAX_BOUND;
code = m_jointAccelFnt (*this, axisParam);
if (code & 1) {
if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculatePointDerivative (ret, params, matrix0.m_front, pointDataP, &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[0].m_accel;
SetMotorAcceleration (ret, axisParam[0].m_accel, params);
ret ++;
}
if (code & 2) {
if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
// dgVector p (p0 + dir1);
// dgPointParam pointData;
// InitPointParam (pointData, m_stiffness, p, p);
// CalculatePointDerivative (ret, params, dir2, pointData, &m_jointForce[ret]);
CalculateAngularDerivative (ret, params, matrix0.m_front, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[1].m_accel;
SetMotorAcceleration (ret, axisParam[1].m_accel, params);
ret ++;
}
}
return dgUnsigned32 (ret);
}
void dgBallConstraint::SetTwistLimitState(bool state)
{
m_twistLimit = dgUnsigned32(state);
}
void dgCollidingPairCollector::AddPair (dgBody* const bodyPtr0, dgBody* const bodyPtr1, dgInt32 threadIndex)
{
if ((bodyPtr0 != m_sentinel) && (bodyPtr1 != m_sentinel)) {
dgWorld* const world = (dgWorld*) this;
if (bodyPtr0->GetSleepState() & bodyPtr1->GetSleepState()) {
dgContact* contact = NULL;
if (bodyPtr0->m_invMass.m_w != dgFloat32 (0.0f)) {
for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr0, bodyPtr1); link; link = world->FindConstraintLinkNext (link, bodyPtr1)) {
dgConstraint* const constraint = link->GetInfo().m_joint;
if (constraint->GetId() == dgContactConstraintId) {
contact = (dgContact*)constraint;
break;
}
}
} else if (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f)) {
_ASSERTE (bodyPtr1->m_invMass.m_w != dgFloat32 (0.0f));
for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (bodyPtr1, bodyPtr0); link; link = world->FindConstraintLinkNext (link, bodyPtr0)) {
dgConstraint* const constraint = link->GetInfo().m_joint;
if (constraint->GetId() == dgContactConstraintId) {
contact = (dgContact*)constraint;
break;
}
}
} else {
return;
}
if (contact) {
_ASSERTE (contact->GetId() == dgContactConstraintId);
contact->m_broadphaseLru = dgInt32 (world->m_broadPhaseLru);
}
} else {
dgBody* tmpbody0 (bodyPtr0);
dgBody* tmpbody1 (bodyPtr1);
if (tmpbody0->m_uniqueID > tmpbody1->m_uniqueID) {
Swap(tmpbody0, tmpbody1);
}
dgBody* const body0 (tmpbody0);
dgBody* const body1 (tmpbody1);
_ASSERTE (body0->GetWorld());
_ASSERTE (body1->GetWorld());
_ASSERTE (body0->GetWorld() == world);
_ASSERTE (body1->GetWorld() == world);
dgContact* contact = NULL;
if (body0->m_invMass.m_w != dgFloat32 (0.0f)) {
for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body0, body1); link; link = world->FindConstraintLinkNext (link, body1)) {
dgConstraint* const constraint = link->GetInfo().m_joint;
if (constraint->GetId() == dgContactConstraintId) {
contact = (dgContact*)constraint;
} else {
if (!constraint->IsCollidable()) {
return;
}
}
}
} else if (body1->m_invMass.m_w != dgFloat32 (0.0f)) {
_ASSERTE (body1->m_invMass.m_w != dgFloat32 (0.0f));
for (dgBodyMasterListRow::dgListNode* link = world->FindConstraintLink (body1, body0); link; link = world->FindConstraintLinkNext (link, body0)) {
dgConstraint* const constraint = link->GetInfo().m_joint;
if (constraint->GetId() == dgContactConstraintId) {
contact = (dgContact*)constraint;
} else {
if (!constraint->IsCollidable()) {
return;
}
}
}
} else {
return;
}
if (!(body0->m_collideWithLinkedBodies & body1->m_collideWithLinkedBodies)) {
if (world->AreBodyConnectedByJoints (body0, body1)) {
return;
}
}
_ASSERTE (!contact || contact->GetId() == dgContactConstraintId);
dgUnsigned32 group0_ID = dgUnsigned32 (body0->m_bodyGroupId);
dgUnsigned32 group1_ID = dgUnsigned32 (body1->m_bodyGroupId);
if (group1_ID < group0_ID) {
Swap (group0_ID, group1_ID);
}
dgUnsigned32 key = (group1_ID << 16) + group0_ID;
const dgBodyMaterialList& materialList = *world;
const dgContactMaterial* const material = &materialList.Find (key)->GetInfo();
// if (material->m_collisionEnable) {
if (material->m_flags & dgContactMaterial::m_collisionEnable__) {
dgInt32 processContacts;
processContacts = 1;
if (material->m_aabbOverlap) {
processContacts = material->m_aabbOverlap (*material, *body0, *body1, threadIndex);
}
if (processContacts) {
_ASSERTE (!body0->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));
_ASSERTE (!body1->m_collision->IsType (dgCollision::dgCollisionNull_RTTI));
dgThreadPairCache& pairChache = *m_chacheBuffers[threadIndex];
if (pairChache.m_count >= DG_CACHE_PAIR_BUFFER) {
world->dgGetUserLock();
FlushChache (&pairChache);
world->dgReleasedUserLock();
}
dgInt32 count = pairChache.m_count;
pairChache.m_chacheBuffer[count].m_body0 = body0;
pairChache.m_chacheBuffer[count].m_body1 = body1;
pairChache.m_chacheBuffer[count].m_material = material;
pairChache.m_chacheBuffer[count].m_contact = contact;
pairChache.m_count = count + 1;
}
}
}
}
}
dgCollisionInstance::dgCollisionInstance(const dgWorld* const constWorld, dgDeserialize serialize, void* const userData, dgInt32 revisionNumber)
:m_globalMatrix(dgGetIdentityMatrix())
,m_localMatrix (dgGetIdentityMatrix())
,m_aligmentMatrix (dgGetIdentityMatrix())
,m_scale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_invScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_maxScale(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (0.0f))
,m_material()
,m_world(constWorld)
,m_childShape (NULL)
,m_subCollisionHandle(NULL)
,m_parent(NULL)
,m_skinThickness(dgFloat32 (0.0f))
,m_collisionMode(1)
,m_refCount(1)
,m_scaleType(m_unit)
{
dgInt32 saved;
dgInt32 signature;
dgInt32 primitive;
dgInt32 scaleType;
serialize (userData, &m_globalMatrix, sizeof (m_globalMatrix));
serialize (userData, &m_localMatrix, sizeof (m_localMatrix));
serialize (userData, &m_aligmentMatrix, sizeof (m_aligmentMatrix));
serialize (userData, &m_scale, sizeof (m_scale));
serialize (userData, &m_invScale, sizeof (m_invScale));
serialize (userData, &m_maxScale, sizeof (m_maxScale));
serialize (userData, &m_skinThickness, sizeof (m_skinThickness));
serialize (userData, &m_material, sizeof (m_material));
serialize (userData, &m_collisionMode, sizeof (m_collisionMode));
serialize (userData, &scaleType, sizeof (scaleType));
serialize (userData, &primitive, sizeof (primitive));
serialize (userData, &signature, sizeof (signature));
serialize (userData, &saved, sizeof (saved));
m_scaleType = dgScaleType(scaleType);
dgWorld* const world = (dgWorld*) constWorld;
if (saved) {
const dgCollision* collision = NULL;
dgBodyCollisionList::dgTreeNode* node = world->dgBodyCollisionList::Find (dgUnsigned32 (signature));
if (node) {
collision = node->GetInfo();
collision->AddRef();
} else {
dgCollisionID primitiveType = dgCollisionID(primitive);
dgMemoryAllocator* const allocator = world->GetAllocator();
switch (primitiveType)
{
case m_heightField:
{
collision = new (allocator) dgCollisionHeightField (world, serialize, userData, revisionNumber);
break;
}
case m_boundingBoxHierachy:
{
collision = new (allocator) dgCollisionBVH (world, serialize, userData, revisionNumber);
break;
}
case m_compoundCollision:
{
collision = new (allocator) dgCollisionCompound (world, serialize, userData, this, revisionNumber);
break;
}
case m_compoundFracturedCollision:
{
collision = new (allocator) dgCollisionCompoundFractured (world, serialize, userData, this, revisionNumber);
break;
}
case m_sceneCollision:
{
collision = new (allocator) dgCollisionScene (world, serialize, userData, this, revisionNumber);
break;
}
case m_sphereCollision:
{
collision = new (allocator) dgCollisionSphere (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_boxCollision:
{
collision = new (allocator) dgCollisionBox (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_coneCollision:
{
collision = new (allocator) dgCollisionCone (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_capsuleCollision:
{
collision = new (allocator) dgCollisionCapsule (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_cylinderCollision:
{
collision = new (allocator) dgCollisionCylinder (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_chamferCylinderCollision:
{
collision = new (allocator) dgCollisionChamferCylinder (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_convexHullCollision:
{
collision = new (allocator) dgCollisionConvexHull (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
case m_nullCollision:
{
collision = new (allocator) dgCollisionNull (world, serialize, userData, revisionNumber);
node = world->dgBodyCollisionList::Insert (collision, collision->GetSignature());
collision->AddRef();
break;
}
// case m_deformableMesh:
// {
// dgAssert (0);
// return NULL;
// }
default:
dgAssert (0);
}
}
m_childShape = collision;
}
dgDeserializeMarker (serialize, userData);
}
//dgUnsigned32 dgWorld::GetPerfomanceTicks (dgInt32 thread, dgUnsigned32 entry) const
dgUnsigned32 dgWorld::GetPerfomanceTicks (dgUnsigned32 entry) const
{
entry = ClampValue(dgUnsigned32 (entry), dgUnsigned32 (0), dgUnsigned32 (m_counterSize - 1));
return m_perfomanceCounters[entry];
}
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);
}
}
}
}
}
}
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 sinAngle;
dgFloat32 cosAngle;
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix1.m_up;
dgVector dir2 (dir0 * dir1);
dgVector dir3 (dir2 * dir0);
dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3));
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH));
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]);
ret = 4;
// dgVector sinAngle0 (matrix1.m_up * matrix0.m_up);
// m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f));
// if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) {
// m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0;
// }
sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front;
cosAngle = matrix0.m_up % matrix1.m_up;
// dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle0 = dgAtan2 (sinAngle, cosAngle);
// dgVector sinAngle1 (matrix0.m_front * matrix1.m_front);
// m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f));
// if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) {
// m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1;
// }
sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
cosAngle = matrix0.m_front % matrix1.m_front;
// dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f);
m_angle1 = dgAtan2 (sinAngle, cosAngle);
if (m_jointAccelFnt) {
dgUnsigned32 code;
dgJointCallbackParam axisParam[2];
// linear acceleration
axisParam[0].m_accel = dgFloat32 (0.0f);
axisParam[0].m_timestep = params.m_timestep;
axisParam[0].m_minFriction = DG_MIN_BOUND;
axisParam[0].m_maxFriction = DG_MAX_BOUND;
// angular acceleration
axisParam[1].m_accel = dgFloat32 (0.0f);
axisParam[1].m_timestep = params.m_timestep;
axisParam[1].m_minFriction = DG_MIN_BOUND;
axisParam[1].m_maxFriction = DG_MAX_BOUND;
code = m_jointAccelFnt (*this, axisParam);
if (code & 1) {
if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
// CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]);
CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[0].m_accel;
SetMotorAcceleration (ret, axisParam[0].m_accel, params);
ret ++;
}
if (code & 2) {
if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction;
params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction;
params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]);
//params.m_jointAccel[ret] = axisParam[1].m_accel;
SetMotorAcceleration (ret, axisParam[1].m_accel, params);
ret ++;
}
}
return dgUnsigned32 (ret);
}
dgInt32 dgPolygonSoupDatabaseBuilder::FilterFace(dgInt32 count,
dgInt32* const pool)
{
if (count == 3)
{
dgBigVector p0(m_vertexPoints[pool[2]]);
for (dgInt32 i = 0; i < 3; i++)
{
dgBigVector p1(m_vertexPoints[pool[i]]);
dgBigVector edge(p1 - p0);
dgFloat64 mag2 = edge % edge;
if (mag2 < dgFloat32(1.0e-6f))
{
count = 0;
}
p0 = p1;
}
if (count == 3)
{
dgBigVector edge0(m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1(m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal(edge0 * edge1);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat32(1.0e-8f))
{
count = 0;
}
}
}
else
{
dgPolySoupFilterAllocator polyhedra(m_allocator);
count = polyhedra.AddFilterFace(dgUnsigned32(count), pool);
if (!count)
{
return 0;
}
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0)
{
edge = edge->m_twin;
}
bool flag = true;
while (flag)
{
flag = false;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
do
{
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
dgFloat64 mag2 = e0 % e0;
if (mag2 < dgFloat32(1.0e-6f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3)
{
flag = true;
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
_ASSERTE((normal % normal) > dgFloat32 (1.0e-10f));
normal = normal.Scale(dgRsqrt (normal % normal + dgFloat32 (1.0e-20f)));
while (flag)
{
flag = false;
if (count >= 3)
{
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = e1 % e0;
if (mag2 > dgFloat32(0.9999f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
dgBigVector n(e0 * e1);
mag2 = n % normal;
if (mag2 < dgFloat32(1.0e-5f))
{
count--;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
}
dgEdge* first = edge;
if (count >= 3)
{
dgFloat64 best = dgFloat32(2.0f);
dgEdge* ptr = edge;
dgBigVector p0(&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector p1(&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0(p1 - p0);
e0 = e0.Scale(dgRsqrt (e0 % e0 + dgFloat32(1.0e-10f)));
do
{
dgBigVector p2(
&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
dgBigVector e1(p2 - p1);
e1 = e1.Scale(dgRsqrt (e1 % e1 + dgFloat32(1.0e-10f)));
dgFloat64 mag2 = fabs(e1 % e0);
if (mag2 < best)
{
best = mag2;
first = ptr;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
count = 0;
ptr = first;
do
{
pool[count] = ptr->m_incidentVertex;
count++;
ptr = ptr->m_next;
} while (ptr != first);
}
#ifdef _DEBUG
if (count >= 3)
{
dgInt32 j0 = count - 2;
dgInt32 j1 = count - 1;
dgBigVector normal(
polyhedra.FaceNormal(edge, &m_vertexPoints[0].m_x,
sizeof(dgBigVector)));
for (dgInt32 j2 = 0; j2 < count; j2++)
{
dgBigVector p0(&m_vertexPoints[pool[j0]].m_x);
dgBigVector p1(&m_vertexPoints[pool[j1]].m_x);
dgBigVector p2(&m_vertexPoints[pool[j2]].m_x);
dgBigVector e0((p0 - p1));
dgBigVector e1((p2 - p1));
dgBigVector n(e1 * e0);
_ASSERTE((n % normal) > dgFloat32 (0.0f));
j0 = j1;
j1 = j2;
}
}
#endif
}
return (count >= 3) ? count : 0;
}
void dgWorld::SetFrictionMode (dgInt32 mode)
{
m_frictionMode = dgUnsigned32 (mode);
}
void dgWorld::SetSolverMode (dgInt32 mode)
{
m_solverMode = dgUnsigned32 (GetMax (0, mode));
}
dgUnsigned32 dgCollision::Quantize(dgFloat32 value)
{
return dgUnsigned32 (value * 1024.0f);
}
