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); } }