dgGoogol dgGoogol::operator+ (const dgGoogol &A) const
{
dgGoogol tmp;
dgAssert (dgInt64 (m_mantissa[0]) >= 0);
dgAssert (dgInt64 (A.m_mantissa[0]) >= 0);
if (m_mantissa[0] && A.m_mantissa[0]) {
dgUnsigned64 mantissa0[DG_GOOGOL_SIZE];
dgUnsigned64 mantissa1[DG_GOOGOL_SIZE];
dgUnsigned64 mantissa[DG_GOOGOL_SIZE];
CopySignedMantissa (mantissa0);
A.CopySignedMantissa (mantissa1);
dgInt32 exponetDiff = m_exponent - A.m_exponent;
dgInt32 exponent = m_exponent;
if (exponetDiff > 0) {
ShiftRightMantissa (mantissa1, exponetDiff);
} else if (exponetDiff < 0) {
exponent = A.m_exponent;
ShiftRightMantissa (mantissa0, -exponetDiff);
}
dgUnsigned64 carrier = 0;
for (dgInt32 i = DG_GOOGOL_SIZE - 1; i >= 0; i --) {
dgUnsigned64 m0 = mantissa0[i];
dgUnsigned64 m1 = mantissa1[i];
mantissa[i] = m0 + m1 + carrier;
carrier = CheckCarrier (m0, m1) | CheckCarrier (m0 + m1, carrier);
}
dgInt8 sign = 0;
if (dgInt64 (mantissa[0]) < 0) {
sign = 1;
NegateMantissa (mantissa);
}
dgInt32 bits = NormalizeMantissa (mantissa);
if (bits <= (-64 * DG_GOOGOL_SIZE)) {
tmp.m_sign = 0;
tmp.m_exponent = 0;
} else {
tmp.m_sign = sign;
tmp.m_exponent = dgInt16 (exponent + bits);
}
memcpy (tmp.m_mantissa, mantissa, sizeof (m_mantissa));
} else if (A.m_mantissa[0]) {
tmp = A;
} else {
tmp = *this;
}
dgAssert (dgInt64 (tmp.m_mantissa[0]) >= 0);
return tmp;
}
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;
}
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);
}
void dgWorldDynamicUpdate::CalculateClusterReactionForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep, dgFloat32 maxAccNorm) const
{
dTimeTrackerEvent(__FUNCTION__);
dgWorld* const world = (dgWorld*) this;
const dgInt32 bodyCount = cluster->m_bodyCount;
// const dgInt32 jointCount = island->m_jointCount;
const dgInt32 jointCount = cluster->m_activeJointCount;
dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart];
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*)&world->m_bodiesMemory[0];
dgJointInfo* const constraintArrayPtr = (dgJointInfo*)&world->m_jointsMemory[0];
dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart];
dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];
dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_jacobianBuffer[cluster->m_rowsStart];
const dgInt32 derivativesEvaluationsRK4 = 4;
dgFloat32 invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f);
dgFloat32 invStepRK = (dgFloat32(1.0f) / dgFloat32(derivativesEvaluationsRK4));
dgFloat32 timestepRK = timestep * invStepRK;
dgFloat32 invTimestepRK = invTimestep * dgFloat32(derivativesEvaluationsRK4);
dgAssert(bodyArray[0].m_body == world->m_sentinelBody);
dgVector speedFreeze2(world->m_freezeSpeed2 * dgFloat32(0.1f));
dgVector freezeOmega2(world->m_freezeOmega2 * dgFloat32(0.1f));
dgJointAccelerationDecriptor joindDesc;
joindDesc.m_timeStep = timestepRK;
joindDesc.m_invTimeStep = invTimestepRK;
joindDesc.m_firstPassCoefFlag = dgFloat32(0.0f);
dgInt32 skeletonCount = 0;
dgInt32 skeletonMemorySizeInBytes = 0;
dgInt32 lru = dgAtomicExchangeAndAdd(&dgSkeletonContainer::m_lruMarker, 1);
dgSkeletonContainer* skeletonArray[DG_MAX_SKELETON_JOINT_COUNT];
dgInt32 memorySizes[DG_MAX_SKELETON_JOINT_COUNT];
for (dgInt32 i = 1; i < bodyCount; i++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
dgSkeletonContainer* const container = body->GetSkeleton();
if (container && (container->m_lru != lru)) {
container->m_lru = lru;
memorySizes[skeletonCount] = container->GetMemoryBufferSizeInBytes(constraintArray, matrixRow);
skeletonMemorySizeInBytes += memorySizes[skeletonCount];
skeletonArray[skeletonCount] = container;
skeletonCount++;
dgAssert(skeletonCount < dgInt32(sizeof(skeletonArray) / sizeof(skeletonArray[0])));
}
}
dgInt8* const skeletonMemory = (dgInt8*)dgAlloca(dgVector, skeletonMemorySizeInBytes / sizeof(dgVector));
dgAssert((dgInt64(skeletonMemory) & 0x0f) == 0);
skeletonMemorySizeInBytes = 0;
for (dgInt32 i = 0; i < skeletonCount; i++) {
skeletonArray[i]->InitMassMatrix(constraintArray, matrixRow, &skeletonMemory[skeletonMemorySizeInBytes]);
skeletonMemorySizeInBytes += memorySizes[i];
}
const dgInt32 passes = world->m_solverMode;
for (dgInt32 step = 0; step < derivativesEvaluationsRK4; step++) {
for (dgInt32 i = 0; i < jointCount; i++) {
dgJointInfo* const jointInfo = &constraintArray[i];
dgConstraint* const constraint = jointInfo->m_joint;
joindDesc.m_rowsCount = jointInfo->m_pairCount;
joindDesc.m_rowMatrix = &matrixRow[jointInfo->m_pairStart];
constraint->JointAccelerations(&joindDesc);
}
joindDesc.m_firstPassCoefFlag = dgFloat32(1.0f);
dgFloat32 accNorm(maxAccNorm * dgFloat32(2.0f));
for (dgInt32 i = 0; (i < passes) && (accNorm > maxAccNorm); i++) {
accNorm = dgFloat32(0.0f);
for (dgInt32 j = 0; j < jointCount; j++) {
dgJointInfo* const jointInfo = &constraintArray[j];
dgFloat32 accel = CalculateJointForceGaussSeidel(jointInfo, bodyArray, internalForces, matrixRow, maxAccNorm);
accNorm = (accel > accNorm) ? accel : accNorm;
}
}
for (dgInt32 j = 0; j < skeletonCount; j++) {
skeletonArray[j]->CalculateJointForce(constraintArray, bodyArray, internalForces, matrixRow);
}
if (timestepRK != dgFloat32(0.0f)) {
dgVector timestep4(timestepRK);
for (dgInt32 i = 1; i < bodyCount; i++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
dgAssert(body->m_index == i);
if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
const dgJacobian& forceAndTorque = internalForces[i];
dgVector force(body->m_externalForce + forceAndTorque.m_linear);
dgVector torque(body->m_externalTorque + forceAndTorque.m_angular);
dgVector velocStep((force.Scale4(body->m_invMass.m_w)) * timestep4);
dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4);
body->m_veloc += velocStep;
body->m_omega += omegaStep;
dgAssert(body->m_veloc.m_w == dgFloat32(0.0f));
dgAssert(body->m_omega.m_w == dgFloat32(0.0f));
}
}
} else {
for (dgInt32 i = 1; i < bodyCount; i++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
const dgVector& linearMomentum = internalForces[i].m_linear;
const dgVector& angularMomentum = internalForces[i].m_angular;
body->m_veloc += linearMomentum.Scale4(body->m_invMass.m_w);
body->m_omega += body->m_invWorldInertiaMatrix.RotateVector(angularMomentum);
}
}
}
dgInt32 hasJointFeeback = 0;
if (timestepRK != dgFloat32(0.0f)) {
for (dgInt32 i = 0; i < jointCount; i++) {
dgJointInfo* const jointInfo = &constraintArray[i];
dgConstraint* const constraint = jointInfo->m_joint;
const dgInt32 first = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
for (dgInt32 j = 0; j < count; j++) {
dgJacobianMatrixElement* const row = &matrixRow[j + first];
dgFloat32 val = row->m_force;
dgAssert(dgCheckFloat(val));
row->m_jointFeebackForce->m_force = val;
row->m_jointFeebackForce->m_impact = row->m_maxImpact * timestepRK;
}
hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0);
}
const dgVector invTime(invTimestep);
const dgVector maxAccNorm2(maxAccNorm * maxAccNorm);
for (dgInt32 i = 1; i < bodyCount; i++) {
dgBody* const body = bodyArray[i].m_body;
CalculateNetAcceleration(body, invTime, maxAccNorm2);
}
if (hasJointFeeback) {
for (dgInt32 i = 0; i < jointCount; i++) {
if (constraintArray[i].m_joint->m_updaFeedbackCallback) {
constraintArray[i].m_joint->m_updaFeedbackCallback(*constraintArray[i].m_joint, timestep, threadID);
}
}
}
} else {
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;
}
}
}
