dgInt32 dgCollisionTaperedCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgAssert (dgAbsf (normal % normal - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
}
void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
dgJacobianMatrixElement* const jacobianMatrixElements = 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;
// remember the impulse branch
//dgAssert (params->m_timeStep > dgFloat32 (0.0f));
if (params->m_timeStep > dgFloat32 (0.0f)) {
dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);
dgFloat32 dt = params->m_timeStep;
for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
} else {
const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) + Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) + Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
//dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
//dgFloat32 aRel = jacobianMatrixElements[k].m_coordenateAccel;
dgFloat32 aRel = params->m_firstPassCoefFlag ? jacobianMatrixElements[k].m_deltaAccel : jacobianMatrixElements[k].m_coordenateAccel;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
//alphaError = num / den;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
//dgFloat32 dt = desc.m_timestep;
//dgFloat32 ks = DG_POS_DAMP;
//dgFloat32 kd = DG_VEL_DAMP;
//dgFloat32 ksd = dt * ks;
//dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
//dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
//accelError = num / den;
dgFloat32 ksd = dt * ks;
dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
jacobianMatrixElements[k].m_penetration = relPosit;
dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 aRelErr = num / den;
//centripetal acceleration is stored in restitution member
jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;
}
}
} else {
for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
} else {
const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) +
Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) +
Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
jacobianMatrixElements[k].m_coordenateAccel = jacobianMatrixElements[k].m_deltaAccel - vRel;
}
}
}
}
dgWorld::dgWorld(dgMemoryAllocator* allocator):
// dgThreadHive(),
dgBodyMasterList(allocator),
dgBroadPhaseCollision(allocator),
dgBodyMaterialList(allocator),
dgBodyCollisionList(allocator),
dgActiveContacts(allocator),
dgCollidingPairCollector(),
m_perInstanceData(allocator),
m_threadsManager(),
m_dynamicSolver()
{
dgInt32 steps;
dgFloat32 freezeAccel2;
dgFloat32 freezeAlpha2;
dgFloat32 freezeSpeed2;
dgFloat32 freezeOmega2;
// init exact arithmetic functions
m_allocator = allocator;
//dgThreadHive::SetThreadCount(16);
//dgThreadHive::SetThreadCount(32);
//_control87 (_EM_ZERODIVIDE | _EM_INEXACT | _EM_OVERFLOW | _EM_INVALID, _MCW_EM);
m_inUpdate = 0;
m_bodyGroupID = 0;
// m_activeBodiesCount = 0;
m_defualtBodyGroupID = CreateBodyGroupID();
m_islandMemorySizeInBytes = DG_INITIAL_ISLAND_SIZE;
m_bodiesMemorySizeInBytes = DG_INITIAL_BODIES_SIZE;
m_jointsMemorySizeInBytes = DG_INITIAL_JOINTS_SIZE;
m_pairMemoryBufferSizeInBytes = 1024 * 64 * sizeof (void*);
m_pairMemoryBuffer = m_allocator->MallocLow (m_pairMemoryBufferSizeInBytes);
m_islandMemory = m_allocator->MallocLow (m_islandMemorySizeInBytes);
m_jointsMemory = m_allocator->MallocLow (m_jointsMemorySizeInBytes);
m_bodiesMemory = m_allocator->MallocLow (m_bodiesMemorySizeInBytes);
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
m_jacobiansMemorySizeInBytes[i] = DG_INITIAL_JACOBIAN_SIZE;
m_jacobiansMemory[i] = m_allocator->MallocLow (m_jacobiansMemorySizeInBytes[i]);
m_internalForcesMemorySizeInBytes[i] = DG_INITIAL_BODIES_SIZE;
m_internalForcesMemory[i] = m_allocator->MallocLow (m_internalForcesMemorySizeInBytes[i]);
m_contactBuffersSizeInBytes[i] = DG_INITIAL_CONTATCT_SIZE;
m_contactBuffers[i] = m_allocator->MallocLow (m_contactBuffersSizeInBytes[i]);
}
m_genericLRUMark = 0;
m_singleIslandMultithreading = 1;
m_solverMode = 0;
m_frictionMode = 0;
m_dynamicsLru = 0;
m_broadPhaseLru = 0;
m_bodiesUniqueID = 0;
// m_bodiesCount = 0;
m_frictiomTheshold = dgFloat32 (0.25f);
m_userData = NULL;
m_islandUpdate = NULL;
m_destroyCollision = NULL;
m_leavingWorldNotify = NULL;
m_destroyBodyByExeciveForce = NULL;
m_freezeAccel2 = DG_FREEZE_MAG2;
m_freezeAlpha2 = DG_FREEZE_MAG2;
m_freezeSpeed2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
m_freezeOmega2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
steps = 1;
freezeAccel2 = m_freezeAccel2;
freezeAlpha2 = m_freezeAlpha2;
freezeSpeed2 = m_freezeSpeed2;
freezeOmega2 = m_freezeOmega2;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
m_sleepTable[i].m_maxAccel = freezeAccel2;
m_sleepTable[i].m_maxAlpha = freezeAlpha2;
m_sleepTable[i].m_maxVeloc = freezeSpeed2;
m_sleepTable[i].m_maxOmega = freezeOmega2;
m_sleepTable[i].m_steps = steps;
steps += 7;
freezeAccel2 *= dgFloat32 (1.5f);
freezeAlpha2 *= dgFloat32 (1.4f);
freezeSpeed2 *= dgFloat32 (1.5f);
freezeOmega2 *= dgFloat32 (1.5f);
}
steps += 300;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;
m_cpu = dgNoSimdPresent;
m_numberOfTheads = 1;
SetHardwareMode (1);
SetThreadsCount (DG_MAXIMUN_THREADS);
m_maxTheads = m_numberOfTheads;
SetHardwareMode (0);
SetThreadsCount (1);
dgBroadPhaseCollision::Init ();
dgCollidingPairCollector::Init ();
m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
AddSentinelBody();
SetPerfomanceCounter(NULL);
}
dgVector dgCollisionSphere::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const
{
return dgVector (dgFloat32 (0.0f));
}
void dgBilateralConstraint::CalculateAngularDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, dgFloat32 stiffness, dgFloat32 jointAngle, dgForceImpactPair* const jointForce)
{
dgAssert (jointForce);
dgAssert (m_body0);
dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
jacobian0.m_linear[0] = dgFloat32 (0.0f);
jacobian0.m_linear[1] = dgFloat32 (0.0f);
jacobian0.m_linear[2] = dgFloat32 (0.0f);
jacobian0.m_linear[3] = dgFloat32 (0.0f);
jacobian0.m_angular[0] = dir.m_x;
jacobian0.m_angular[1] = dir.m_y;
jacobian0.m_angular[2] = dir.m_z;
jacobian0.m_angular[3] = dgFloat32 (0.0f);
dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgAssert (m_body1);
jacobian1.m_linear[0] = dgFloat32 (0.0f);
jacobian1.m_linear[1] = dgFloat32 (0.0f);
jacobian1.m_linear[2] = dgFloat32 (0.0f);
jacobian1.m_linear[3] = dgFloat32 (0.0f);
jacobian1.m_angular[0] = -dir.m_x;
jacobian1.m_angular[1] = -dir.m_y;
jacobian1.m_angular[2] = -dir.m_z;
jacobian1.m_angular[3] = dgFloat32 (0.0f);
const dgVector& omega0 = m_body0->GetOmega();
const dgVector& omega1 = m_body1->GetOmega();
dgFloat32 omegaError = (omega1 - omega0) % dir;
m_rowIsMotor[index] = 0;
m_motorAcceleration[index] = dgFloat32 (0.0f);
if (desc.m_timestep > dgFloat32 (0.0f)) {
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = DG_POS_DAMP;
dgFloat32 kd = DG_VEL_DAMP;
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * jointAngle + kd * omegaError + ksd * omegaError;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 alphaError = num / den;
desc.m_zeroRowAcceleration[index] = (jointAngle + omegaError * desc.m_timestep) * desc.m_invTimestep * desc.m_invTimestep;
desc.m_penetration[index] = jointAngle;
desc.m_jointAccel[index] = alphaError;
desc.m_restitution[index] = dgFloat32 (0.0f);
desc.m_jointStiffness[index] = stiffness;
desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
desc.m_forceBounds[index].m_jointForce = jointForce;
} else {
desc.m_penetration[index] = dgFloat32 (0.0f);
desc.m_jointAccel[index] = omegaError;
desc.m_restitution[index] = dgFloat32 (0.0f);
desc.m_jointStiffness[index] = stiffness;
desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
desc.m_zeroRowAcceleration[index] = dgFloat32 (0.0f);
desc.m_forceBounds[index].m_jointForce = jointForce;
}
}
void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const
{
if (level) {
dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
//p01 = p01 * p01.InvMagSqrt();
//p12 = p12 * p12.InvMagSqrt();
//p20 = p20 * p20.InvMagSqrt();
p01 = p01.Normalize();
p12 = p12.Normalize();
p20 = p20.Normalize();
dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TesselateTriangle (level - 1, p0, p01, p20, count, ouput);
TesselateTriangle (level - 1, p1, p12, p01, count, ouput);
TesselateTriangle (level - 1, p2, p20, p12, count, ouput);
TesselateTriangle (level - 1, p01, p12, p20, count, ouput);
} else {
ouput[count ++] = p0;
ouput[count ++] = p1;
ouput[count ++] = p2;
}
}
dgFloat32 dgCollisionPoint::GetVolume () const
{
dgAssert (0);
return dgFloat32 (0.0f);
}
dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
dgVector support[4];
dgInt32 featureCount = 3;
const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping();
if (vertToEdgeMapping) {
dgInt32 edgeIndex;
//support[0] = SupportVertex (normal.Scale4(normalSign), &edgeIndex);
support[0] = SupportVertex (normal, &edgeIndex);
dgFloat32 dist = normal.DotProduct4(support[0] - point).GetScalar();
if (dist <= DG_IMPULSIVE_CONTACT_PENETRATION) {
dgVector normalAlgin (normal.Abs());
if (!((normalAlgin.m_x > dgFloat32 (0.9999f)) || (normalAlgin.m_y > dgFloat32 (0.9999f)) || (normalAlgin.m_z > dgFloat32 (0.9999f)))) {
// 0.25 degrees
const dgFloat32 tiltAngle = dgFloat32 (0.005f);
const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ;
dgPlane testPlane (normal, - (normal.DotProduct4(support[0]).GetScalar()));
featureCount = 1;
const dgConvexSimplexEdge* const edge = vertToEdgeMapping[edgeIndex];
const dgConvexSimplexEdge* ptr = edge;
do {
const dgVector& p = m_vertex[ptr->m_twin->m_vertex];
dgFloat32 test1 = testPlane.Evalue(p);
dgVector dist (p - support[0]);
dgFloat32 angle2 = test1 * test1 / (dist.DotProduct4(dist).GetScalar());
if (angle2 < tiltAngle2) {
support[featureCount] = p;
featureCount ++;
}
ptr = ptr->m_twin->m_next;
} while ((ptr != edge) && (featureCount < 3));
}
}
}
dgInt32 count = 0;
switch (featureCount)
{
case 1:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
count = 1;
break;
}
case 2:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
contactsOut[1] = support[1] - normal.CompProduct4(normal.DotProduct4(support[1] - point));
count = 2;
break;
}
default:
{
dgFloat32 test[8];
dgAssert(normal.m_w == dgFloat32(0.0f));
dgPlane plane(normal, -(normal.DotProduct4(point).GetScalar()));
for (dgInt32 i = 0; i < 8; i++) {
dgAssert(m_vertex[i].m_w == dgFloat32(0.0f));
test[i] = plane.DotProduct4(m_vertex[i] | dgVector::m_wOne).m_x;
}
dgConvexSimplexEdge* edge = NULL;
for (dgInt32 i = 0; i < dgInt32 (sizeof (m_edgeEdgeMap) / sizeof (m_edgeEdgeMap[0])); i ++) {
dgConvexSimplexEdge* const ptr = m_edgeEdgeMap[i];
dgFloat32 side0 = test[ptr->m_vertex];
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if ((side0 * side1) < dgFloat32 (0.0f)) {
edge = ptr;
break;
}
}
if (edge) {
if (test[edge->m_vertex] < dgFloat32 (0.0f)) {
edge = edge->m_twin;
}
dgAssert (test[edge->m_vertex] > dgFloat32 (0.0f));
dgConvexSimplexEdge* ptr = edge;
dgConvexSimplexEdge* firstEdge = NULL;
dgFloat32 side0 = test[edge->m_vertex];
do {
dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f));
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if (side1 < side0) {
if (side1 < dgFloat32 (0.0f)) {
firstEdge = ptr;
break;
}
side0 = side1;
edge = ptr->m_twin;
ptr = edge;
}
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
if (firstEdge) {
edge = firstEdge;
ptr = edge;
do {
dgVector dp (m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]);
dgFloat32 t = plane.DotProduct4(dp).m_x;
if (t >= dgFloat32 (-1.e-24f)) {
t = dgFloat32 (0.0f);
} else {
t = test[ptr->m_vertex] / t;
if (t > dgFloat32 (0.0f)) {
t = dgFloat32 (0.0f);
}
if (t < dgFloat32 (-1.0f)) {
t = dgFloat32 (-1.0f);
}
}
dgAssert (t <= dgFloat32 (0.01f));
dgAssert (t >= dgFloat32 (-1.05f));
contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale4 (t);
count ++;
dgConvexSimplexEdge* ptr1 = ptr->m_next;
for (; ptr1 != ptr; ptr1 = ptr1->m_next) {
dgInt32 index0 = ptr1->m_twin->m_vertex;
if (test[index0] >= dgFloat32 (0.0f)) {
dgAssert (test[ptr1->m_vertex] <= dgFloat32 (0.0f));
break;
}
}
dgAssert (ptr != ptr1);
ptr = ptr1->m_twin;
} while ((ptr != edge) && (count < 8));
}
}
}
}
if (count > 2) {
count = RectifyConvexSlice (count, normal, contactsOut);
}
return count;
}
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 ();
}
dgFloat32 dgCollisionBox::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
dgAssert (localP0.m_w == dgFloat32 (0.0f));
dgAssert (localP1.m_w == dgFloat32 (0.0f));
dgInt32 index = 0;
dgFloat32 signDir = dgFloat32 (0.0f);
dgFloat32 tmin = dgFloat32 (0.0f);
dgFloat32 tmax = dgFloat32 (1.0f);
for (dgInt32 i = 0; i < 3; i++) {
dgFloat32 dp = localP1[i] - localP0[i];
if (dgAbsf (dp) < dgFloat32 (1.0e-8f)) {
if (localP0[i] <= m_size[1][i] || localP0[i] >= m_size[0][i]) {
return dgFloat32 (1.2f);
}
} else {
dp = dgFloat32 (1.0f) / dp;
dgFloat32 t1 = (m_size[1][i] - localP0[i]) * dp;
dgFloat32 t2 = (m_size[0][i] - localP0[i]) * dp;
dgFloat32 sign = dgFloat32 (-1.0f);
if (t1 > t2) {
sign = 1;
dgSwap(t1, t2);
}
if (t1 > tmin) {
signDir = sign;
index = i;
tmin = t1;
}
if (t2 < tmax) {
tmax = t2;
}
if (tmin > tmax) {
return dgFloat32 (1.2f);
}
}
}
if (tmin > dgFloat32 (0.0f)) {
dgAssert (tmin <= 1.0f);
contactOut.m_normal = dgVector (dgFloat32 (0.0f));
contactOut.m_normal[index] = signDir;
//contactOut.m_userId = SetUserDataID();
} else {
tmin = dgFloat32 (1.2f);
}
return tmin;
}
void dgCollisionBox::Serialize(dgSerialize callback, void* const userData) const
{
SerializeLow(callback, userData);
dgVector size (m_size[0].Scale3 (dgFloat32 (2.0f)));
callback (userData, &size, sizeof (dgVector));
}
dgVector dgCollisionBox::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const
{
dgAssert(dgAbsf((dir.DotProduct3(dir) - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
return point + dir.Scale4 (D_BOX_SKIN_THINCKNESS);
}
void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCylinder_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_dirVector.m_x = radio1 - radio0;
m_dirVector.m_y = m_height * dgFloat32 (2.0f);
m_dirVector.m_z = dgFloat32 (0.0f);
m_dirVector.m_w = dgFloat32 (0.0f);
m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector));
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CYLINDER_SEGMENTS];
dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionTaperedCylinder::Serialize(dgSerialize callback, void* const userData) const
{
dgVector size (m_radio0, m_radio1, m_height * dgFloat32 (2.0f), dgFloat32 (0.0f));
SerializeLow(callback, userData);
callback (userData, &size, sizeof (dgVector));
}
dgWorld::dgWorld(dgMemoryAllocator* const allocator)
:dgBodyMasterList(allocator)
,dgBodyMaterialList(allocator)
,dgBodyCollisionList(allocator)
,dgSkeletonList(allocator)
,dgInverseDynamicsList(allocator)
,dgContactList(allocator)
,dgBilateralConstraintList(allocator)
,dgWorldDynamicUpdate(allocator)
,dgMutexThread("newtonMainThread", 0)
,dgWorldThreadPool(allocator)
,dgDeadBodies(allocator)
,dgDeadJoints(allocator)
,dgWorldPluginList(allocator)
,m_broadPhase(NULL)
,m_sentinelBody(NULL)
,m_pointCollision(NULL)
,m_userData(NULL)
,m_allocator (allocator)
,m_mutex()
,m_postUpdateCallback(NULL)
,m_listeners(allocator)
,m_perInstanceData(allocator)
,m_bodiesMemory (allocator, 64)
,m_jointsMemory (allocator, 64)
,m_clusterMemory (allocator, 64)
,m_solverJacobiansMemory (allocator, 64)
,m_solverRightHandSideMemory (allocator, 64)
,m_solverForceAccumulatorMemory (allocator, 64)
,m_concurrentUpdate(false)
{
//TestAStart();
//TestSort();
dgMutexThread* const myThread = this;
SetParentThread (myThread);
// avoid small memory fragmentations on initialization
m_bodiesMemory.Resize(1024);
m_clusterMemory.Resize(1024);
m_jointsMemory.Resize(1024 * 2);
m_solverJacobiansMemory.Resize(1024 * 64);
m_solverRightHandSideMemory.Resize(1024 * 64);
m_solverForceAccumulatorMemory.Resize(1024 * 32);
m_savetimestep = dgFloat32 (0.0f);
m_allocator = allocator;
m_clusterUpdate = NULL;
m_onCollisionInstanceDestruction = NULL;
m_onCollisionInstanceCopyConstrutor = NULL;
m_serializedJointCallback = NULL;
m_deserializedJointCallback = NULL;
m_inUpdate = 0;
m_bodyGroupID = 0;
m_lastExecutionTime = 0;
m_defualtBodyGroupID = CreateBodyGroupID();
m_genericLRUMark = 0;
m_delayDelateLock = 0;
m_clusterLRU = 0;
m_useParallelSolver = 0;
m_solverIterations = DG_DEFAULT_SOLVER_ITERATION_COUNT;
m_dynamicsLru = 0;
m_numberOfSubsteps = 1;
m_bodiesUniqueID = 0;
m_frictiomTheshold = dgFloat32 (0.25f);
m_userData = NULL;
m_clusterUpdate = NULL;
m_freezeAccel2 = DG_FREEZE_ACCEL2;
m_freezeAlpha2 = DG_FREEZE_ACCEL2;
m_freezeSpeed2 = DG_FREEZE_SPEED2;
m_freezeOmega2 = DG_FREEZE_SPEED2;
m_contactTolerance = DG_PRUNE_CONTACT_TOLERANCE;
dgInt32 steps = 1;
dgFloat32 freezeAccel2 = m_freezeAccel2;
dgFloat32 freezeAlpha2 = m_freezeAlpha2;
dgFloat32 freezeSpeed2 = m_freezeSpeed2;
dgFloat32 freezeOmega2 = m_freezeOmega2;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
m_sleepTable[i].m_maxAccel = freezeAccel2;
m_sleepTable[i].m_maxAlpha = freezeAlpha2;
m_sleepTable[i].m_maxVeloc = freezeSpeed2;
m_sleepTable[i].m_maxOmega = freezeOmega2;
m_sleepTable[i].m_steps = steps;
steps += 7;
freezeAccel2 *= dgFloat32 (1.5f);
freezeAlpha2 *= dgFloat32 (1.4f);
freezeSpeed2 *= dgFloat32 (1.5f);
freezeOmega2 *= dgFloat32 (1.5f);
}
steps += 300;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;
SetThreadsCount (0);
m_broadPhase = new (allocator) dgBroadPhaseMixed(this);
//m_broadPhase = new (allocator) dgBroadPhaseSegregated (this);
//m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
dgCollision* const pointCollison = new (m_allocator) dgCollisionPoint(m_allocator);
m_pointCollision = CreateInstance(pointCollison, 0, dgGetIdentityMatrix());
pointCollison->Release();
AddSentinelBody();
// LoadPlugins();
}
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]));
}
void dgWorld::SetContactMergeTolerance(dgFloat32 tolerenace)
{
m_contactTolerance = dgMax (tolerenace, dgFloat32 (1.e-3f));
}
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 dgCollisionSphere::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgTriplex pool[1024 * 2];
dgVector tmpVectex[1024 * 2];
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 = 3;
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);
for (dgInt32 i = 0; i < count; i ++) {
tmpVectex[i] = tmpVectex[i].Scale (m_radius);
}
//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &tmpVectex[0].m_x, sizeof (dgVector), count);
for (dgInt32 i = 0; i < count; i += 3) {
callback (userData, 3, &pool[i].m_x, 0);
}
}
dgContactMaterial::dgContactMaterial()
:m_dir0 (dgFloat32 (0.0f))
,m_dir1 (dgFloat32 (0.0f))
,m_userData(NULL)
,m_aabbOverlap(NULL)
,m_contactPoint(NULL)
,m_compoundAABBOverlap(NULL)
{
// dgAssert ( dgInt32 (sizeof (dgContactMaterial) & 15) == 0);
dgAssert ((((dgUnsigned64) this) & 15) == 0);
m_point = dgVector (dgFloat32 (0.0f));
m_softness = dgFloat32 (0.1f);
m_restitution = dgFloat32 (0.4f);
m_staticFriction0 = dgFloat32 (0.9f);
m_staticFriction1 = dgFloat32 (0.9f);
m_dynamicFriction0 = dgFloat32 (0.5f);
m_dynamicFriction1 = dgFloat32 (0.5f);
m_dir0_Force.m_force = dgFloat32 (0.0f);
m_dir0_Force.m_impact = dgFloat32 (0.0f);
m_dir1_Force.m_force = dgFloat32 (0.0f);
m_dir1_Force.m_impact = dgFloat32 (0.0f);
m_normal_Force.m_force = dgFloat32 (0.0f);
m_normal_Force.m_impact = dgFloat32 (0.0f);
m_skinThickness = dgFloat32 (0.0f);
//m_skinThickness = dgFloat32 (0.1f);
//m_skinThickness = DG_MAX_COLLISION_AABB_PADDING * dgFloat32 (0.125f);
m_flags = m_collisionEnable | m_friction0Enable | m_friction1Enable;
}
dgVector dgCollisionPoint::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
return dgVector (dgFloat32 (0.0f));
}
void dgWorldDynamicUpdate::UpdateBodyVelocityParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
{
dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
dgWorld* const world = (dgWorld*) worldContext;
const dgIsland* const island = syncData->m_island;
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0];
dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
dgFloat32 maxAccNorm2 = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR;
//dgFloat32 invTimestepSrc = dgFloat32 (1.0f) / syncData->m_timestep;
dgFloat32 invTimestepSrc = syncData->m_invTimestep;
dgVector invTime (invTimestepSrc);
dgInt32* const atomicIndex = &syncData->m_atomicIndex;
dgVector forceActiveMask ((syncData->m_jointCount <= DG_SMALL_ISLAND_COUNT) ? dgVector (-1, -1, -1, -1) : dgFloat32 (0.0f));
for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body;
world->ApplyNetTorqueAndForce (body, invTime, maxAccNorm2, forceActiveMask);
}
}
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 ();
}
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;
}
}
}
}
void dgBilateralConstraint::CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param, dgForceImpactPair* const jointForce)
{
dgAssert (jointForce);
dgAssert (m_body0);
dgAssert (m_body1);
dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
dgVector r0CrossDir (param.m_r0 * dir);
jacobian0.m_linear[0] = dir.m_x;
jacobian0.m_linear[1] = dir.m_y;
jacobian0.m_linear[2] = dir.m_z;
jacobian0.m_linear[3] = dgFloat32 (0.0f);
jacobian0.m_angular[0] = r0CrossDir.m_x;
jacobian0.m_angular[1] = r0CrossDir.m_y;
jacobian0.m_angular[2] = r0CrossDir.m_z;
jacobian0.m_angular[3] = dgFloat32 (0.0f);
dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector r1CrossDir (dir * param.m_r1);
jacobian1.m_linear[0] = -dir.m_x;
jacobian1.m_linear[1] = -dir.m_y;
jacobian1.m_linear[2] = -dir.m_z;
jacobian1.m_linear[3] = dgFloat32 (0.0f);
jacobian1.m_angular[0] = r1CrossDir.m_x;
jacobian1.m_angular[1] = r1CrossDir.m_y;
jacobian1.m_angular[2] = r1CrossDir.m_z;
jacobian1.m_angular[3] = dgFloat32 (0.0f);
m_rowIsMotor[index] = 0;
m_motorAcceleration[index] = dgFloat32 (0.0f);
dgVector velocError (param.m_veloc1 - param.m_veloc0);
dgFloat32 relVeloc = velocError % dir;
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgVector positError (param.m_posit1 - param.m_posit0);
dgVector centrError (param.m_centripetal1 - param.m_centripetal0);
dgFloat32 relPosit = positError % dir;
dgFloat32 relCentr = centrError % dir;
relCentr = dgClamp (relCentr, dgFloat32(-100000.0f), dgFloat32(100000.0f));
desc.m_zeroRowAcceleration[index] = (relPosit + relVeloc * desc.m_timestep) * desc.m_invTimestep * desc.m_invTimestep;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = DG_POS_DAMP;
dgFloat32 kd = DG_VEL_DAMP;
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 accelError = num / den;
desc.m_penetration[index] = relPosit;
desc.m_penetrationStiffness[index] = dgFloat32 (0.01f/4.0f);
desc.m_jointStiffness[index] = param.m_stiffness;
desc.m_jointAccel[index] = accelError + relCentr;
// save centripetal acceleration in the restitution member
desc.m_restitution[index] = relCentr;
desc.m_forceBounds[index].m_jointForce = jointForce;
} else {
desc.m_penetration[index] = dgFloat32 (0.0f);
desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
desc.m_jointStiffness[index] = param.m_stiffness;
desc.m_jointAccel[index] = relVeloc;
desc.m_restitution[index] = dgFloat32 (0.0f);
desc.m_zeroRowAcceleration[index] = dgFloat32 (0.0f);
desc.m_forceBounds[index].m_jointForce = jointForce;
}
}
void dgWorldDynamicUpdate::LinearizeJointParallelArray(dgParallelSolverSyncData* const solverSyncData, dgJointInfo* const constraintArray, const dgIsland* const island) const
{
dgParallelSolverSyncData::dgParallelJointMap* const jointInfoMap = solverSyncData->m_jointConflicts;
dgInt32 count = island->m_jointCount;
for (dgInt32 i = 0; i < count; i++) {
dgConstraint* const joint = constraintArray[i].m_joint;
joint->m_index = i;
jointInfoMap[i].m_jointIndex = i;
jointInfoMap[i].m_color = 0;
}
jointInfoMap[count].m_color = 0x7fffffff;
jointInfoMap[count].m_jointIndex = -1;
for (dgInt32 i = 0; i < count; i++) {
dgInt32 index = 0;
dgInt32 color = jointInfoMap[i].m_color;
for (dgInt32 n = 1; n & color; n <<= 1) {
index++;
dgAssert(index < 32);
}
jointInfoMap[i].m_bashCount = index;
color = 1 << index;
dgAssert (jointInfoMap[i].m_jointIndex == i);
dgJointInfo& jointInfo = constraintArray[i];
dgConstraint* const constraint = jointInfo.m_joint;
dgDynamicBody* const body0 = (dgDynamicBody*)constraint->m_body0;
dgAssert(body0->IsRTTIType(dgBody::m_dynamicBodyRTTI));
if (body0->m_invMass.m_w > dgFloat32(0.0f)) {
for (dgBodyMasterListRow::dgListNode* jointNode = body0->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
dgConstraint* const neiborgLink = cell.m_joint;
if ((neiborgLink != constraint) && (neiborgLink->m_maxDOF)) {
dgParallelSolverSyncData::dgParallelJointMap& info = jointInfoMap[neiborgLink->m_index];
info.m_color |= color;
}
}
}
dgDynamicBody* const body1 = (dgDynamicBody*)constraint->m_body1;
dgAssert(body1->IsRTTIType(dgBody::m_dynamicBodyRTTI));
if (body1->m_invMass.m_w > dgFloat32(0.0f)) {
for (dgBodyMasterListRow::dgListNode* jointNode = body1->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
dgConstraint* const neiborgLink = cell.m_joint;
if ((neiborgLink != constraint) && (neiborgLink->m_maxDOF)) {
dgParallelSolverSyncData::dgParallelJointMap& info = jointInfoMap[neiborgLink->m_index];
info.m_color |= color;
}
}
}
}
dgSort(jointInfoMap, count, SortJointInfoByColor);
dgInt32 acc = 0;
dgInt32 bash = 0;
dgInt32 bachCount = 0;
for (int i = 0; i < count; i++) {
if (jointInfoMap[i].m_bashCount > bash) {
bash = jointInfoMap[i].m_bashCount;
solverSyncData->m_jointBatches[bachCount + 1] = acc;
bachCount ++;
dgAssert (bachCount < (dgInt32 (sizeof (solverSyncData->m_jointBatches) / sizeof (solverSyncData->m_jointBatches[0])) - 1));
}
acc ++;
}
bachCount ++;
solverSyncData->m_bachCount = bachCount;
solverSyncData->m_jointBatches[bachCount] = acc;
dgAssert (bachCount < (dgInt32 (sizeof (solverSyncData->m_jointBatches) / sizeof (solverSyncData->m_jointBatches[0])) - 1));
}
void dgBilateralConstraint::SetStiffness(dgFloat32 stiffness)
{
m_stiffness = dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
}
void dgWorld::UpdateSkeletons()
{
DG_TRACKTIME(__FUNCTION__);
dgSkeletonList& skelManager = *this;
if (skelManager.m_skelListIsDirty) {
skelManager.m_skelListIsDirty = false;
dgSkeletonList::Iterator iter(skelManager);
for (iter.Begin(); iter; iter++) {
dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo();
delete skeleton;
}
skelManager.RemoveAll();
m_dynamicsLru = m_dynamicsLru + 1;
dgUnsigned32 lru = m_dynamicsLru;
dgBodyMasterList& masterList = *this;
m_solverJacobiansMemory.ResizeIfNecessary((2 * (masterList.m_constraintCount + 1024)) * sizeof (dgBilateralConstraint*));
dgBilateralConstraint** const jointList = (dgBilateralConstraint**)&m_solverJacobiansMemory[0];
dgInt32 jointCount = 0;
for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) {
const dgBodyMasterListRow& graphNode = node->GetInfo();
dgBody* const srcBody = graphNode.GetBody();
for (dgBodyMasterListRow::dgListNode* jointNode = srcBody->m_masterNode->GetInfo().GetLast(); jointNode; jointNode = jointNode->GetPrev()) {
dgBodyMasterListCell* const cell = &jointNode->GetInfo();
dgConstraint* const constraint = cell->m_joint;
dgAssert(constraint);
dgAssert((constraint->m_body0 == srcBody) || (constraint->m_body1 == srcBody));
dgAssert((constraint->m_body0 == cell->m_bodyNode) || (constraint->m_body1 == cell->m_bodyNode));
if (constraint->IsBilateral() && (constraint->m_solverModel < 2) && (constraint->m_dynamicsLru != lru)) {
constraint->m_dynamicsLru = lru;
jointList[jointCount] = (dgBilateralConstraint*)constraint;
jointCount++;
}
}
}
dgSortIndirect(jointList, jointCount, CompareJointByInvMass);
const dgInt32 poolSize = 1024 * 4;
dgBilateralConstraint* loopJoints[64];
dgSkeletonContainer::dgNode* queuePool[poolSize];
m_dynamicsLru = m_dynamicsLru + 1;
lru = m_dynamicsLru;
for (dgInt32 i = 0; i < jointCount; i++) {
dgBilateralConstraint* const constraint = jointList[i];
if (constraint->m_dynamicsLru != lru) {
dgQueue<dgSkeletonContainer::dgNode*> queue(queuePool, poolSize);
dgInt32 loopCount = 0;
dgDynamicBody* const rootBody = (dgDynamicBody*)((constraint->GetBody0()->GetInvMass().m_w < constraint->GetBody1()->GetInvMass().m_w) ? constraint->GetBody0() : constraint->GetBody1());
dgSkeletonContainer* const skeleton = CreateNewtonSkeletonContainer(rootBody);
dgSkeletonContainer::dgNode* const rootNode = skeleton->GetRoot();
if (rootBody->GetInvMass().m_w == dgFloat32 (0.0f)) {
if (constraint->IsBilateral() && (constraint->m_dynamicsLru != lru)) {
constraint->m_dynamicsLru = lru;
dgDynamicBody* const childBody = (dgDynamicBody*)((constraint->GetBody0() == rootBody) ? constraint->GetBody1() : constraint->GetBody0());
if (!constraint->m_solverModel) {
if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) {
childBody->m_dynamicsLru = lru;
dgSkeletonContainer::dgNode* const node = skeleton->AddChild((dgBilateralConstraint*)constraint, rootNode);
queue.Insert(node);
}
}
}
} else {
queue.Insert(rootNode);
rootBody->m_dynamicsLru = lru;
}
while (!queue.IsEmpty()) {
dgInt32 count = queue.m_firstIndex - queue.m_lastIndex;
if (count < 0) {
count += queue.m_mod;
}
dgInt32 index = queue.m_lastIndex;
queue.Reset();
for (dgInt32 j = 0; j < count; j++) {
dgSkeletonContainer::dgNode* const parentNode = queue.m_pool[index];
dgDynamicBody* const parentBody = skeleton->GetBody(parentNode);
for (dgBodyMasterListRow::dgListNode* jointNode1 = parentBody->m_masterNode->GetInfo().GetFirst(); jointNode1; jointNode1 = jointNode1->GetNext()) {
dgBodyMasterListCell* const cell1 = &jointNode1->GetInfo();
dgConstraint* const constraint1 = cell1->m_joint;
if (constraint1->IsBilateral() && (constraint1->m_dynamicsLru != lru)) {
constraint1->m_dynamicsLru = lru;
dgDynamicBody* const childBody = (dgDynamicBody*)((constraint1->GetBody0() == parentBody) ? constraint1->GetBody1() : constraint1->GetBody0());
if (!constraint1->m_solverModel) {
if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) {
childBody->m_dynamicsLru = lru;
dgSkeletonContainer::dgNode* const childNode = skeleton->AddChild((dgBilateralConstraint*)constraint1, parentNode);
queue.Insert(childNode);
} else if (loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) {
loopJoints[loopCount] = (dgBilateralConstraint*)constraint1;
loopCount++;
}
} else if ((constraint1->m_solverModel != 2) && loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) {
loopJoints[loopCount] = (dgBilateralConstraint*)constraint1;
loopCount++;
}
}
}
index++;
if (index >= queue.m_mod) {
index = 0;
}
}
}
skeleton->Finalize(loopCount, loopJoints);
}
}
}
dgSkeletonList::Iterator iter(skelManager);
for (iter.Begin(); iter; iter++) {
dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo();
skeleton->ClearSelfCollision();
}
}
void dgWorld::SetFrictionThreshold (dgFloat32 acceleration)
{
m_frictiomTheshold = GetMax (dgFloat32(1.0e-2f), acceleration);
}
dgVector dgCollisionNull::CalculateVolumeIntegral (const dgMatrix& globalMatrix, const dgVector& plane, const dgCollisionInstance& parentScale) const
{
dgAssert (0);
return dgVector (dgFloat32 (0.0f));
}
