DG_INLINE void InitKinematicMassMatrix (const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgFloat32 correctionFactor)
{
dgAssert (0);
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
m_bodyForce.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
m_jointForce[m_dof] = -dgClamp(row->m_penetration * correctionFactor, dgFloat32(-0.25f), dgFloat32(0.25f));
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE bool ValidateForcesLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, const dgForcePair& force)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
bool valid = true;
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; (i < m_dof) && valid; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
dgFloat32 f = force.m_joint[i] + row->m_force;
valid = valid && (f >= row->m_lowerBoundFrictionCoefficent);
valid = valid && (f <= row->m_upperBoundFrictionCoefficent);
}
}
return valid;
}
DG_INLINE void Factorize(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32 (0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert (m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
if (((row->m_lowerBoundFrictionCoefficent < dgFloat32 (-1.0e9f)) && row->m_upperBoundFrictionCoefficent > dgFloat32 (1.0e9f))) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof ++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
dgGoogol::dgGoogol(dgFloat64 value)
:m_sign(0)
,m_exponent(0)
{
dgInt32 exp;
dgFloat64 mantissa = fabs (frexp(value, &exp));
m_exponent = dgInt16 (exp);
m_sign = (value >= 0) ? 0 : 1;
memset (m_mantissa, 0, sizeof (m_mantissa));
m_mantissa[0] = (dgInt64 (dgFloat64 (dgUnsigned64(1)<<62) * mantissa));
// it looks like GCC have problems with this
//dgAssert (m_mantissa[0] >= 0);
dgAssert ((m_mantissa[0] & dgUnsigned64(1)<<63) == 0);
}
dgKinematicBody* dgWorld::CreateKinematicBody (dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgKinematicBody* const body = new (m_allocator) dgKinematicBody();
dgAssert (dgInt32 (sizeof (dgBody) & 0xf) == 0);
dgAssert ((dgUnsigned64 (body) & 0xf) == 0);
InitBody (body, collision, matrix);
return body;
}
dgGoogol dgGoogol::operator* (const dgGoogol &A) const
{
dgAssert (dgInt64 (m_mantissa[0]) >= 0);
dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
if (m_mantissa[0] && A.m_mantissa[0]) {
dgUnsigned64 mantissaAcc[DG_GOOGOL_SIZE * 2];
memset (mantissaAcc, 0, sizeof (mantissaAcc));
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
dgUnsigned64 a = m_mantissa[i];
if (a) {
dgUnsigned64 mantissaScale[2 * DG_GOOGOL_SIZE];
memset (mantissaScale, 0, sizeof (mantissaScale));
A.ScaleMantissa (&mantissaScale[i], a);
dgUnsigned64 carrier = 0;
for (dgInt32 j = 0; j < 2 * DG_GOOGOL_SIZE; j ++) {
const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - j;
dgUnsigned64 m0 = mantissaAcc[k];
dgUnsigned64 m1 = mantissaScale[k];
mantissaAcc[k] = m0 + m1 + carrier;
carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
}
}
}
dgUnsigned64 carrier = 0;
dgInt32 bits = dgUnsigned64(LeadingZeros (mantissaAcc[0]) - 2);
for (dgInt32 i = 0; i < 2 * DG_GOOGOL_SIZE; i ++) {
const dgInt32 k = 2 * DG_GOOGOL_SIZE - 1 - i;
dgUnsigned64 a = mantissaAcc[k];
mantissaAcc[k] = (a << dgUnsigned64(bits)) | carrier;
carrier = a >> dgUnsigned64(64 - bits);
}
dgInt32 exp = m_exponent + A.m_exponent - (bits - 2);
dgGoogol tmp;
tmp.m_sign = m_sign ^ A.m_sign;
tmp.m_exponent = dgInt16 (exp);
memcpy (tmp.m_mantissa, mantissaAcc, sizeof (m_mantissa));
return tmp;
}
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;
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
//dgSpatialVector& accel = force.m_joint;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
//dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
dgFloat32 force = row->m_force;
if ((force >= row->m_lowerBoundFrictionCoefficent) && (force <= row->m_upperBoundFrictionCoefficent)) {
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
accel.m_joint[i] = -residual.GetScalar();
} else {
dgAssert (0);
dgSwap (m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i --;
m_dof --;
clampedValue --;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void UpdateFactorizeLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgForcePair& force, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
dgFloat32 f = force.m_joint[i] + row->m_force;
if (f < row->m_lowerBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
} else if (f > row->m_upperBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
if ((force > row->m_lowerBoundFrictionCoefficent) && (force < row->m_upperBoundFrictionCoefficent)) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
void dgPolygonSoupDatabaseBuilder::OptimizeByGroupID(
dgPolygonSoupDatabaseBuilder& source, dgInt32 faceNumber,
dgInt32 faceIndexNumber, dgPolygonSoupDatabaseBuilder& leftOver)
{
dgInt32 indexPool[1024 * 1];
dgInt32 atributeData[1024 * 1];
dgVector vertexPool[1024 * 1];
dgPolyhedra polyhedra(m_allocator);
dgInt32 attribute = source.m_vertexIndex[faceIndexNumber];
for (dgInt32 i = 0; i < dgInt32(sizeof(atributeData) / sizeof(dgInt32)); i++)
{
indexPool[i] = i;
atributeData[i] = attribute;
}
leftOver.Begin();
polyhedra.BeginFace();
for (dgInt32 i = faceNumber; i < source.m_faceCount; i++)
{
dgInt32 indexCount;
indexCount = source.m_faceVertexCount[i];
_ASSERTE(indexCount < 1024);
if (source.m_vertexIndex[faceIndexNumber] == attribute)
{
dgEdge* const face = polyhedra.AddFace(indexCount - 1,
&source.m_vertexIndex[faceIndexNumber + 1]);
if (!face)
{
dgInt32 faceArray;
for (dgInt32 j = 0; j < indexCount - 1; j++)
{
dgInt32 index;
index = source.m_vertexIndex[faceIndexNumber + j + 1];
vertexPool[j] = source.m_vertexPoints[index];
}
faceArray = indexCount - 1;
leftOver.AddMesh(&vertexPool[0].m_x, indexCount - 1, sizeof(dgVector),
1, &faceArray, indexPool, atributeData, dgGetIdentityMatrix());
}
else
{
// set the attribute
dgEdge* ptr = face;
do
{
ptr->m_userData = dgUnsigned64(attribute);
ptr = ptr->m_next;
} while (ptr != face);
}
}
faceIndexNumber += indexCount;
}
leftOver.Optimize(false);
polyhedra.EndFace();
dgPolyhedra facesLeft(m_allocator);
facesLeft.BeginFace();
polyhedra.ConvexPartition(&source.m_vertexPoints[0].m_x, sizeof(dgBigVector),
&facesLeft);
facesLeft.EndFace();
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter(polyhedra);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
dgEdge* ptr = edge;
dgInt32 indexCount = 0;
do
{
ptr->m_mark = mark;
vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
indexCount++;
ptr = ptr->m_next;
} while (ptr != edge);
if (indexCount >= 3)
{
AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount,
indexPool, atributeData, dgGetIdentityMatrix());
}
}
mark = facesLeft.IncLRU();
dgPolyhedra::Iterator iter1(facesLeft);
for (iter1.Begin(); iter1; iter1++)
{
dgEdge* const edge = &(*iter1);
if (edge->m_incidentFace < 0)
{
continue;
}
if (edge->m_mark == mark)
{
continue;
}
dgEdge* ptr = edge;
dgInt32 indexCount = 0;
do
{
ptr->m_mark = mark;
vertexPool[indexCount] = source.m_vertexPoints[ptr->m_incidentVertex];
indexCount++;
ptr = ptr->m_next;
} while (ptr != edge);
if (indexCount >= 3)
{
AddMesh(&vertexPool[0].m_x, indexCount, sizeof(dgVector), 1, &indexCount, indexPool, atributeData, dgGetIdentityMatrix());
}
}
}
dgGoogol::operator double() const
{
dgFloat64 mantissa = (dgFloat64(1.0f) / dgFloat64 (dgUnsigned64(1)<<62)) * dgFloat64 (m_mantissa[0]);
mantissa = ldexp(mantissa, m_exponent) * (m_sign ? dgFloat64 (-1.0f) : dgFloat64 (1.0f));
return mantissa;
}
