That was OK for when some only wanted to save one one simulation run, but if some one wants to save a complete simulation saving keyframe check points in the midle,
then the rotation and the centrer of mass drift.
most people do not save more than one run, but somehow you are making some find of keyframe animation, which is cool too.
It is fixed now but you will have to wait until tonight when I post SDK 2.23, in the mean time this is the demo
- Code: Select all
// deterministm.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
#include <Windows.h>
#include <Newton.h>
#include <stdio.h>
NewtonWorld* gNewtonWorld = NULL;
NewtonBody* gDynBody = NULL;
const int numPushes = 4;
const float updateStep = 1.0f / 60.0f;
float vel[3] = {5.0f, 0.0f, 0.0f};
float zero[3] = { 0.0f, 0.0f, 0.0f };
////////////////////////////////////////////////////////////////////////////////
void initPhysicsWorld()
{
gNewtonWorld = NewtonCreate();
NewtonSetPlatformArchitecture(gNewtonWorld, 0);
NewtonSetThreadsCount(gNewtonWorld, 1);
NewtonSetSolverModel(gNewtonWorld, 1);
//NewtonSetFrictionModel(gNewtonWorld, 0);
// set an initial world size
float worldMin[3] = { -500.0f,-500.0f,-500.0f };
float worldMax[3] = { 500.0f,500.0f,500.0f };
NewtonSetWorldSize(gNewtonWorld, worldMin, worldMax);
}
////////////////////////////////////////////////////////////////////////////////
void destroyPhysicsWorld()
{
if (gNewtonWorld)
{
NewtonDestroyAllBodies(gNewtonWorld);
NewtonDestroy(gNewtonWorld);
gNewtonWorld = NULL;
}
}
////////////////////////////////////////////////////////////////////////////////
void applyForceAndTorqueCallback(const NewtonBody* body, float timestep, int threadIndex)
{
// Get mass / inertia of body
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
// Apply gravity force
#define GRAVITY -10.0f
float gravityForce[3] = { 0.0f, mass * GRAVITY, 0.0f };
NewtonBodySetForce(body, gravityForce);
}
////////////////////////////////////////////////////////////////////////////////
NewtonBody* addRigidBodyToWorld(NewtonCollision* shape, float mtx[16], float mass)
{
// Create body
NewtonBody* pRB = NewtonCreateBody(gNewtonWorld, shape);
NewtonBodySetMatrix(pRB, mtx);
// Calculate / set inertia and center of mass
float inertia[3];
float origin[3] = {0, 0, 0};
NewtonConvexCollisionCalculateInertialMatrix(shape, inertia, origin);
NewtonBodySetMassMatrix(pRB, mass, mass * inertia[0], mass * inertia[1], mass * inertia[2]);
NewtonBodySetCentreOfMass(pRB, origin);
// Set force and transform callbacks
NewtonBodySetForceAndTorqueCallback(pRB, applyForceAndTorqueCallback);
//NewtonBodySetTransformCallback(pRB, setTransformCallback);
return pRB;
}
////////////////////////////////////////////////////////////////////////////////
void addBodies()
{
// Add static ground body
int groundID = 0;
float groundOffsetMtx[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
float groundMtx[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
float groundMass = 0.0f;
// Create a tree collision shape with a grid of faces at height 0.5
NewtonCollision* pGroundShape = NewtonCreateTreeCollision(gNewtonWorld, 0);
NewtonTreeCollisionBeginBuild(pGroundShape);
float x0z0[3] = {0.0f, 0.5f, 0.0f};
int numXQuads = 10;
int numZQuads = 10;
float xInc = 50.0f / float(numXQuads);
float zInc = 50.0f / float(numZQuads);
for (int i = 0; i < numXQuads; i++)
{
for (int j = 0; j < numZQuads; j++)
{
float startPt[3] = {x0z0[0] + xInc * i, x0z0[1], x0z0[2] + zInc * j};
float faceVerts[3][3];
faceVerts[0][0] = startPt[0];
faceVerts[0][1] = startPt[1];
faceVerts[0][2] = startPt[2];
faceVerts[1][0] = startPt[0];
faceVerts[1][1] = startPt[1];
faceVerts[1][2] = startPt[2] + zInc;
faceVerts[2][0] = startPt[0] + xInc;
faceVerts[2][1] = startPt[1];
faceVerts[2][2] = startPt[2];
NewtonTreeCollisionAddFace(pGroundShape, 3, &(faceVerts[0][0]), 12, 0);
faceVerts[0][0] = startPt[0];
faceVerts[0][1] = startPt[1];
faceVerts[0][2] = startPt[2] + zInc;
faceVerts[1][0] = startPt[0] + xInc;
faceVerts[1][1] = startPt[1];
faceVerts[1][2] = startPt[2] + zInc;
faceVerts[2][0] = startPt[0] + xInc;
faceVerts[2][1] = startPt[1];
faceVerts[2][2] = startPt[2];
NewtonTreeCollisionAddFace(pGroundShape, 3, &(faceVerts[0][0]), 12, 0);
}
}
NewtonTreeCollisionEndBuild(pGroundShape, 0);
// NewtonTreeCollisionEndBuild(pGroundShape, 1);
addRigidBodyToWorld(pGroundShape, groundMtx, groundMass);
NewtonReleaseCollision(gNewtonWorld, pGroundShape);
// Create dynamic cylinder to move along ground
int dynID = 1;
float dynOffsetMtx[16] = {0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
float dynMtx[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 19.5, 2.0, 5.0, 1};
float dynMass = 1.0f;
// NewtonCollision* pDynShape = NewtonCreateChamferCylinder(gNewtonWorld, 0.5, 0.1f, dynID, dynOffsetMtx);
NewtonCollision* pDynShape = NewtonCreateBox(gNewtonWorld, 0.5, 0.5f, 0.5f, dynID, dynOffsetMtx);
NewtonBody* pDynBody = addRigidBodyToWorld(pDynShape, dynMtx, dynMass);
NewtonReleaseCollision(gNewtonWorld, pDynShape);
gDynBody = pDynBody;
}
////////////////////////////////////////////////////////////////////////////////
//void printResults(int pushIndex, float mtx[], bool inSync)
void printResults(int pushIndex, bool inSync)
{
// Print out results
switch (pushIndex)
{
case 0:
printf("Initial - ");
break;
case 1:
case 2:
case 3:
case 4:
printf("After %d - ", pushIndex);
break;
}
// printf("%f %f %f", mtx[12], mtx[13], mtx[14]);
if (inSync)
printf(" --> In Sync\n");
else
printf(" --> OUT OF SYNC\n");
}
////////////////////////////////////////////////////////////////////////////////
void applyVelocity()
{
NewtonBodySetVelocity(gDynBody, vel);
NewtonBodySetOmega(gDynBody, zero);
NewtonBodySetForce(gDynBody, zero);
NewtonBodySetTorque(gDynBody, zero);
}
// make and array of world states each state to hold M bodies, for this example m = 100
// this should used some kind of SDT container or equivalents
class WorldState
{
// the state on one body
struct BODY_STATE
{
NewtonBody* m_body;
float m_matrix[16];
float m_veloc[3];
float m_omega[3];
};
// the state of a world is teh state of all its bodies
struct WORLD_STATE
{
int m_bodyCount;
BODY_STATE m_bodies[100];
};
// an animation is an array of many states
int m_Count;
WORLD_STATE m_allStates[20];
public:
WorldState ()
{
m_Count = 0;
}
void SaveState ()
{
// get the state for this track
WORLD_STATE* worldState = &m_allStates[m_Count];
// invalidate the cache in the newton world
NewtonInvalidateCache(gNewtonWorld);
int bodyCount = 0;
// save the state of each body in a newton world
for (NewtonBody* body = NewtonWorldGetFirstBody(gNewtonWorld); body; body = NewtonWorldGetNextBody(gNewtonWorld, body)) {
BODY_STATE* bodyState = &worldState->m_bodies[bodyCount];
bodyState->m_body = body;
NewtonBodyGetMatrix(body, bodyState->m_matrix);
NewtonBodyGetVelocity(body, bodyState->m_veloc);
NewtonBodyGetOmega(body, bodyState->m_omega);
bodyCount ++;
}
worldState->m_bodyCount = bodyCount;
m_Count ++;
}
void RestoreState (int frame)
{
WORLD_STATE* worldState = &m_allStates[frame];
for (int i = 0; i < worldState->m_bodyCount; i ++) {
BODY_STATE* bodyState = &worldState->m_bodies[i];
NewtonBodySetMatrix(bodyState->m_body, bodyState->m_matrix);
NewtonBodySetVelocity(bodyState->m_body, bodyState->m_veloc);
NewtonBodySetOmega(bodyState->m_body, bodyState->m_omega);
}
NewtonInvalidateCache(gNewtonWorld);
}
void CompareandAndPrintReport (int frame)
{
WORLD_STATE* worldState = &m_allStates[frame];
int outOfSync = 0;
for (int i = 0; i < worldState->m_bodyCount; i ++) {
BODY_STATE* bodyState = &worldState->m_bodies[i];
// get the body state from the world into a temporary state
BODY_STATE tmpState;
tmpState.m_body = bodyState->m_body;
NewtonBodyGetMatrix(bodyState->m_body, tmpState.m_matrix);
NewtonBodyGetVelocity(bodyState->m_body, tmpState.m_veloc);
NewtonBodyGetOmega(bodyState->m_body, tmpState.m_omega);
// compare the state to the save one
outOfSync |= memcmp(bodyState, &tmpState, sizeof (BODY_STATE));
}
printResults(frame, outOfSync ? false : true);
}
};
WorldState gWorldState;
void RunSimulation()
{
// saved the state before start a simulation run and also invalidate the cache
// NewtonBodyGetMatrix(gDynBody, initialMatrix);
gWorldState.SaveState ();
for (int i = 0; i < numPushes; )
{
// Update
NewtonUpdate(gNewtonWorld, updateStep);
// When the body goes to sleep, check resulting matrix and apply new velocity
int sleepState = NewtonBodyGetSleepState(gDynBody);
if (sleepState == 1)
{
// Apply new velocity if this isn't the last step
applyVelocity();
// save the world state at this point
gWorldState.SaveState ();
// next push
i ++;
}
}
// save the last state
gWorldState.SaveState ();
}
void ReplaySimulation (int from)
{
// saved the state before start a simulation run and also invalidate the cache
// NewtonBodyGetMatrix(gDynBody, initialMatrix);
gWorldState.RestoreState (from);
for (int i = from; i < numPushes; )
{
// Update
NewtonUpdate(gNewtonWorld, updateStep);
// When the body goes to sleep, check resulting matrix and apply new velocity
int sleepState = NewtonBodyGetSleepState(gDynBody);
if (sleepState == 1)
{
// Apply new velocity if this isn't the last step
applyVelocity();
// check if the worlds are in sync
gWorldState.CompareandAndPrintReport (i + 1);
// Need to Invalidate because at each check point the world was reset.
NewtonInvalidateCache(gNewtonWorld);
//gWorldState.RestoreState (i + 1);
// next push
i ++;
}
}
}
////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
// Create world
initPhysicsWorld();
// Add bodies
addBodies();
// play the simulation and save the states
printf("First playthrough\n");
RunSimulation();
// Replay all
printf("\nReplay all\n");
ReplaySimulation(0);
// Replay 1-4
printf("\nReplay 1-4\n");
ReplaySimulation(1);
// Replay 2-4
printf("\nReplay 2-4\n");
ReplaySimulation(2);
// Replay 2-4
printf("\nReplay 3-4\n");
ReplaySimulation(3);
// Replay all
printf("\nReplay all\n");
ReplaySimulation(0);
// Destroy world
destroyPhysicsWorld();
return 1;
}
and this is the output
Replay all
After 1 - --> In Sync
After 2 - --> In Sync
After 3 - --> In Sync
After 4 - --> In Sync
Replay 1-4
After 2 - --> In Sync
After 3 - --> In Sync
After 4 - --> In Sync
Replay 2-4
After 3 - --> In Sync
After 4 - --> In Sync
Replay 3-4
After 4 - --> In Sync
Replay all
After 1 - --> In Sync
After 2 - --> In Sync
After 3 - --> In Sync
After 4 - --> In Sync
I will upload SDK 2.23 tonight. 2.22 will only be in Sync in the first run.
