void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrix;
dQuaternion bodyRotation;
dVector veloc(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega(0.0f, 0.0f, 0.0f, 0.0f);
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
// apply the player motion, by calculation the desired plane linear and angular velocity
manager->ApplyPlayerMove (this, timestep);
// get the body motion state
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetVelocity(m_body, &veloc[0]);
NewtonBodyGetOmega(m_body, &omega[0]);
// integrate body angular velocity
NewtonBodyGetRotation (m_body, &bodyRotation.m_q0);
bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
matrix = dMatrix (bodyRotation, matrix.m_posit);
// integrate linear velocity
dFloat normalizedTimeLeft = 1.0f;
dFloat step = timestep * dSqrt (veloc % veloc) ;
dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
int prevContactCount = 0;
CustomControllerConvexCastPreFilter castFilterData (m_body);
NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector updir (matrix.RotateVector(m_upVector));
dVector scale;
NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
//const dFloat radio = m_outerRadio * 4.0f;
const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);
NewtonWorldConvexCastReturnInfo upConstratint;
memset (&upConstratint, 0, sizeof (upConstratint));
upConstratint.m_normal[0] = m_upVector.m_x;
upConstratint.m_normal[1] = m_upVector.m_y;
upConstratint.m_normal[2] = m_upVector.m_z;
upConstratint.m_normal[3] = m_upVector.m_w;
for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
if ((veloc % veloc) < 1.0e-6f) {
break;
}
dFloat timetoImpact;
NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
if (contactCount) {
contactCount = manager->ProcessContacts (this, info, contactCount);
}
if (contactCount) {
matrix.m_posit += veloc.Scale (timetoImpact * timestep);
if (timetoImpact > 0.0f) {
matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ;
}
normalizedTimeLeft -= timetoImpact;
dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
for (int i = 1; i < contactCount; i ++) {
dVector n0 (info[i-1].m_normal);
for (int j = 0; j < i; j ++) {
dVector n1 (info[j].m_normal);
if ((n0 % n1) > 0.9999f) {
info[i] = info[contactCount - 1];
i --;
contactCount --;
break;
}
}
}
int count = 0;
if (!m_isJumping) {
upConstratint.m_point[0] = matrix.m_posit.m_x;
upConstratint.m_point[1] = matrix.m_posit.m_y;
upConstratint.m_point[2] = matrix.m_posit.m_z;
upConstratint.m_point[3] = matrix.m_posit.m_w;
speed[count] = 0.0f;
bounceNormal[count] = dVector (upConstratint.m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
count ++;
}
for (int i = 0; i < contactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (info[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
count ++;
}
for (int i = 0; i < prevContactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (prevInfo[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
count ++;
}
dFloat residual = 10.0f;
dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
residual = 0.0f;
for (int k = 0; k < count; k ++) {
dVector normal (bounceNormal[k]);
dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
dFloat x = speed[k] + v;
if (x < 0.0f) {
v = 0.0f;
x = 0.0f;
}
if (dAbs (v) > residual) {
residual = dAbs (v);
}
auxBounceVeloc += normal.Scale (x - speed[k]);
speed[k] = x;
}
}
dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i < count; i ++) {
dVector normal (bounceNormal[i]);
velocStep += normal.Scale (speed[i]);
}
veloc += velocStep;
dFloat velocMag2 = velocStep % velocStep;
if (velocMag2 < 1.0e-6f) {
dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
matrix.m_posit += veloc.Scale (advanceTime);
normalizedTimeLeft -= advanceTime / timestep;
}
prevContactCount = contactCount;
memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));
} else {
matrix.m_posit = destPosit;
matrix.m_posit.m_w = 1.0f;
break;
}
}
NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);
// determine if player is standing on some plane
dMatrix supportMatrix (matrix);
supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
if (m_isJumping) {
dVector dst (matrix.m_posit);
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
} else {
step = dAbs (updir % veloc.Scale (timestep));
dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
}
// set player velocity, position and orientation
NewtonBodySetVelocity(m_body, &veloc[0]);
NewtonBodySetMatrix (m_body, &matrix[0][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;
}
}
}
