void DemoEntityManager::UpdatePhysics(float timestep)
{
// update the physics
if (m_world) {
dFloat timestepInSecunds = 1.0f / MAX_PHYSICS_FPS;
unsigned64 timestepMicrosecunds = unsigned64 (timestepInSecunds * 1000000.0f);
unsigned64 currentTime = dGetTimeInMicrosenconds ();
unsigned64 nextTime = currentTime - m_microsecunds;
int loops = 0;
while ((nextTime >= timestepMicrosecunds) && (loops < MAX_PHYSICS_LOOPS)) {
loops ++;
dTimeTrackerEvent(__FUNCTION__);
// run the newton update function
if (!m_reEntrantUpdate) {
m_reEntrantUpdate = true;
if (m_physicsUpdate && m_world) {
ClearDebugDisplay(m_world);
// update the physics world
if (!m_mainWindow->m_physicsUpdateMode) {
NewtonUpdate (m_world, timestepInSecunds);
} else {
NewtonUpdateAsync(m_world, timestepInSecunds);
}
}
m_reEntrantUpdate = false;
}
nextTime -= timestepMicrosecunds;
m_microsecunds += timestepMicrosecunds;
}
if (loops) {
m_physicsTime = dFloat (dGetTimeInMicrosenconds () - currentTime) / 1000000.0f;
if (m_physicsTime >= MAX_PHYSICS_LOOPS * (1.0f / MAX_PHYSICS_FPS)) {
m_microsecunds = currentTime;
}
}
}
}
void DemoEntityManager::RenderFrame ()
{
dTimeTrackerEvent(__FUNCTION__);
// Make context current
if (m_mainWindow->m_suspendVisualUpdates) {
return;
}
dFloat timestep = dGetElapsedSeconds();
m_mainWindow->CalculateFPS(timestep);
// update the the state of all bodies in the scene
unsigned64 time0 = dGetTimeInMicrosenconds ();
UpdatePhysics(timestep);
unsigned64 time1 = dGetTimeInMicrosenconds ();
m_mainThreadPhysicsTime = dFloat ((time1 - time0) / 1000.0f);
// Get the interpolated location of each body in the scene
m_cameraManager->InterpolateMatrices (this, CalculateInteplationParam());
// Our shading model--Goraud (smooth).
glShadeModel (GL_SMOOTH);
// Culling.
glCullFace (GL_BACK);
glFrontFace (GL_CCW);
glEnable (GL_CULL_FACE);
// glEnable(GL_DITHER);
// z buffer test
glEnable(GL_DEPTH_TEST);
glDepthFunc (GL_LEQUAL);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
glHint(GL_POLYGON_SMOOTH_HINT, GL_FASTEST);
glClearColor (0.5f, 0.5f, 0.5f, 0.0f );
//glClear( GL_COLOR_BUFFER_BIT );
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// set default lightning
// glDisable(GL_BLEND);
glEnable (GL_LIGHTING);
// make sure the model view matrix is set to identity before setting world space ligh sources
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
dFloat cubeColor[] = { 1.0f, 1.0f, 1.0f, 1.0 };
glMaterialParam(GL_FRONT, GL_SPECULAR, cubeColor);
glMaterialParam(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, cubeColor);
glMaterialf(GL_FRONT, GL_SHININESS, 50.0);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
// one light form the Camera eye point
GLfloat lightDiffuse0[] = { 0.5f, 0.5f, 0.5f, 0.0 };
GLfloat lightAmbient0[] = { 0.0f, 0.0f, 0.0f, 0.0 };
dVector camPosition (m_cameraManager->GetCamera()->m_matrix.m_posit);
GLfloat lightPosition0[] = {camPosition.m_x, camPosition.m_y, camPosition.m_z};
glLightfv(GL_LIGHT0, GL_POSITION, lightPosition0);
glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient0);
glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse0);
glLightfv(GL_LIGHT0, GL_SPECULAR, lightDiffuse0);
glEnable(GL_LIGHT0);
// set just one directional light
GLfloat lightDiffuse1[] = { 0.7f, 0.7f, 0.7f, 0.0 };
GLfloat lightAmbient1[] = { 0.2f, 0.2f, 0.2f, 0.0 };
GLfloat lightPosition1[] = { -500.0f, 200.0f, 500.0f, 0.0 };
glLightfv(GL_LIGHT1, GL_POSITION, lightPosition1);
glLightfv(GL_LIGHT1, GL_AMBIENT, lightAmbient1);
glLightfv(GL_LIGHT1, GL_DIFFUSE, lightDiffuse1);
glLightfv(GL_LIGHT1, GL_SPECULAR, lightDiffuse1);
glEnable(GL_LIGHT1);
// update Camera
m_cameraManager->GetCamera()->SetViewMatrix(GetWidth(), GetHeight());
// render all entities
if (m_mainWindow->m_hideVisualMeshes) {
if (m_sky) {
glPushMatrix();
m_sky->Render(timestep, this);
glPopMatrix();
}
} else {
for (dListNode* node = dList<DemoEntity*>::GetFirst(); node; node = node->GetNext()) {
DemoEntity* const entity = node->GetInfo();
glPushMatrix();
entity->Render(timestep, this);
glPopMatrix();
}
}
if (m_tranparentHeap.GetCount()) {
dMatrix modelView;
glGetFloat (GL_MODELVIEW_MATRIX, &modelView[0][0]);
while (m_tranparentHeap.GetCount()) {
const TransparentMesh& transparentMesh = m_tranparentHeap[0];
glLoadIdentity();
glLoadMatrix(&transparentMesh.m_matrix[0][0]);
transparentMesh.m_mesh->RenderTransparency();
m_tranparentHeap.Pop();
}
glLoadMatrix(&modelView[0][0]);
}
m_cameraManager->RenderPickedTarget ();
if (m_mainWindow->m_showContactPoints) {
RenderContactPoints (GetNewton());
}
if (m_mainWindow->m_showNormalForces) {
RenderNormalForces (GetNewton());
}
if (m_mainWindow->m_showNormalForces) {
// if (1) {
// see if there is a vehicle controller and
void* const vehListerNode = NewtonWorldGetPreListener(GetNewton(), VEHICLE_PLUGIN_NAME);
if (vehListerNode) {
CustomVehicleControllerManager* const manager = (CustomVehicleControllerManager*)NewtonWorldGetListenerUserData(GetNewton(), vehListerNode);
manager->Debug();
}
void* const characterListerNode = NewtonWorldGetPreListener(GetNewton(), PLAYER_PLUGIN_NAME);
if (characterListerNode) {
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*)NewtonWorldGetListenerUserData(GetNewton(), characterListerNode);
manager->Debug();
}
}
if (m_mainWindow->m_showAABB) {
RenderAABB (GetNewton());
}
if (m_mainWindow->m_showCenterOfMass) {
RenderCenterOfMass (GetNewton());
}
if (m_mainWindow->m_showJoints) {
RenderJointsDebugInfo (GetNewton(), 0.5f);
}
DEBUG_DRAW_MODE mode = m_solid;
if (m_mainWindow->m_debugDisplayMode) {
mode = (m_mainWindow->m_debugDisplayMode == 1) ? m_solid : m_lines;
DebugRenderWorldCollision (GetNewton(), mode);
}
if (m_mainWindow->m_showStatistics) {
dVector color (1.0f, 1.0f, 1.0f, 0.0f);
Print (color, 10, 20, "render fps: %7.2f", m_mainWindow->m_fps);
Print (color, 10, 42, "physics time on main thread: %7.2f ms", GetPhysicsTime() * 1000.0f);
Print (color, 10, 64, "total memory: %d kbytes", NewtonGetMemoryUsed() / (1024));
Print (color, 10, 86, "number of bodies: %d", NewtonWorldGetBodyCount(GetNewton()));
Print (color, 10, 108, "number of threads: %d", NewtonGetThreadsCount(GetNewton()));
Print (color, 10, 130, "auto sleep: %s", m_mainWindow->m_autoSleepState ? "on" : "off");
}
int lineNumber = 130 + 22;
if (m_renderHood) {
// set display for 2d render mode
dFloat width = GetWidth();
dFloat height = GetHeight();
glColor3f(1.0, 1.0, 1.0);
glPushMatrix();
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0, width, 0, height);
glPushMatrix();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glDisable(GL_LIGHTING);
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
// render 2d display
m_renderHood (this, m_renderHoodContext, lineNumber);
// restore display mode
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
}
// draw everything and swap the display buffer
glFlush();
// Swap
SwapBuffers();
}
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;
}
}
}
void dgWorldDynamicUpdate::BuildJacobianMatrix(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const
{
dTimeTrackerEvent(__FUNCTION__);
dgAssert (cluster->m_bodyCount >= 2);
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];
dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart];
dgAssert (((dgDynamicBody*) bodyArray[0].m_body)->IsRTTIType (dgBody::m_dynamicBodyRTTI));
dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_accel.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_accel)) == dgFloat32 (0.0f));
dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_alpha.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_alpha)) == dgFloat32 (0.0f));
dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce)) == dgFloat32 (0.0f));
dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque)) == dgFloat32 (0.0f));
internalForces[0].m_linear = dgVector::m_zero;
internalForces[0].m_angular = dgVector::m_zero;
if (timestep != dgFloat32 (0.0f)) {
for (dgInt32 i = 1; i < bodyCount; i ++) {
dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body;
//dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI));
dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI));
if (!body->m_equilibrium) {
dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f));
body->AddDampingAcceleration(timestep);
body->CalcInvInertiaMatrix ();
// body->ApplyGyroTorque();
internalForces[i].m_linear = dgVector::m_zero;
internalForces[i].m_angular = dgVector::m_zero;
} else if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
internalForces[i].m_linear = body->m_externalForce * dgVector::m_negOne;
internalForces[i].m_angular = body->m_externalTorque * dgVector::m_negOne;
} else {
internalForces[i].m_linear = dgVector::m_zero;
internalForces[i].m_angular = dgVector::m_zero;
}
// re use these variables for temp storage
body->m_accel = body->m_veloc;
body->m_alpha = body->m_omega;
}
} 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));
if (!body->m_equilibrium) {
dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f));
body->CalcInvInertiaMatrix ();
}
// re use these variables for temp storage
body->m_accel = body->m_veloc;
body->m_alpha = body->m_omega;
internalForces[i].m_linear = dgVector::m_zero;
internalForces[i].m_angular = dgVector::m_zero;
}
}
dgContraintDescritor constraintParams;
constraintParams.m_world = world;
constraintParams.m_threadIndex = threadID;
constraintParams.m_timestep = timestep;
constraintParams.m_invTimestep = (timestep > dgFloat32(1.0e-5f)) ? dgFloat32(1.0f / timestep) : dgFloat32(0.0f);
const dgFloat32 forceOrImpulseScale = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) : dgFloat32(0.0f);
dgJointInfo* const constraintArrayPtr = (dgJointInfo*)&world->m_jointsMemory[0];
dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart];
dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_jacobianBuffer[cluster->m_rowsStart];
dgInt32 rowCount = 0;
//const dgInt32 jointCount = island->m_jointCount;
const dgInt32 jointCount = cluster->m_activeJointCount;
for (dgInt32 i = 0; i < jointCount; i++) {
dgJointInfo* const jointInfo = &constraintArray[i];
dgConstraint* const constraint = jointInfo->m_joint;
dgAssert (dgInt32 (constraint->m_index) == i);
dgAssert(jointInfo->m_m0 < cluster->m_bodyCount);
dgAssert(jointInfo->m_m1 < cluster->m_bodyCount);
//dgAssert (constraint->m_index == dgUnsigned32(j));
rowCount = GetJacobianDerivatives(constraintParams, jointInfo, constraint, matrixRow, rowCount);
dgAssert(rowCount <= cluster->m_rowsCount);
dgAssert(jointInfo->m_m0 >= 0);
dgAssert(jointInfo->m_m0 < bodyCount);
dgAssert(jointInfo->m_m1 >= 0);
dgAssert(jointInfo->m_m1 < bodyCount);
BuildJacobianMatrix(bodyArray, jointInfo, internalForces, matrixRow, forceOrImpulseScale);
}
}
