void Scene::step( void )
{
//Our internal units are pixels, and at 160dpi there are 6300 pixels per meter
float gravity[3];
gravity[0] = (m_motion_lopass[0]*1 + m_motion_hipass[0]*10) * 6300;
gravity[1] = (m_motion_lopass[1]*1 + m_motion_hipass[1]*10) * 6300;
gravity[2] = (m_motion_lopass[2]*1 + m_motion_hipass[2]*10) * 6300;
float hisq = m_motion_hipass[0]*m_motion_hipass[0] + m_motion_hipass[1]*m_motion_hipass[1] + m_motion_hipass[2]*m_motion_hipass[2];
if( hisq > 1.0f )
{
int num_woken = 0;
for( SceneObj *obj_p=m_obj_p; obj_p; obj_p=obj_p->m_next_p )
{
if( !dBodyIsEnabled(obj_p->m_dbody) )
{
dBodyEnable( obj_p->m_dbody );
num_woken++;
if( num_woken == 3 )
break;
}
}
}
dWorldSetGravity( m_dworld, gravity[0], gravity[1], gravity[2] );
do_collision();
dWorldQuickStep( m_dworld, 1.0f/60.0f );
dJointGroupEmpty( m_colljoints );
}
void CPHCharacter::Enable()
{
if(!b_exist) return;
CPHObject::activate();
dBodyEnable(m_body);
}
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags)
{
VERIFY(ref_obj);
R_ASSERT(_valid( start_pos ) );
R_ASSERT( _valid( start_size ) );
m_body = dBodyCreate (0) ;
dMass m;
dMassSetSphere(&m,1.f,100000.f);
dMassAdjust(&m,1.f);
dBodySetMass(m_body,&m);
switch(_type)
{
case etBox:
m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ;
break;
case etSphere:
m_geom = dCreateSphere (0,start_size.x);
break;
};
dGeomCreateUserData (m_geom) ;
dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ;
dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ;
dGeomSetBody (m_geom,m_body) ;
dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ;
Island() .AddBody (m_body) ;
dBodyEnable (m_body) ;
m_safe_state .create(m_body) ;
spatial_register () ;
m_flags.set(flags,TRUE);
}
void add_phys_torque(dBodyID body, float x, float y, float z)
{
if (body)
{
dBodyEnable(body);
dBodyAddTorque(body, x, y, z);
}
}
void add_phys_force(dBodyID body, float x, float y, float z)
{
if (body)
{
dBodyEnable(body);
dBodyAddForce(body, x, y, z);
}
}
void PSteerable::steeringToOde()
{
if (steerInfo.acc.length() != 0 || steerInfo.rot)
dBodyEnable(body);
Vec3f f = steerInfo.acc * mass.mass;
const dReal *angVel = dBodyGetAngularVel(body);
dBodySetAngularVel(body, angVel[0], angVel[1] + steerInfo.rot,
angVel[2]);
dBodyAddForce(body, f[0], f[1], f[2]);
}
void AvatarConstruction::Update( Real FrameTime, WorldManager* SM )
{
if(ForceEnable)
dBodyEnable(ObjectList[0].Body );
ForceEnable=false;
OnGround = false;
Moving = false;
}
void SParts::enableDynamics(bool b)
{
if (m_odeobj) {
dBodyID body = m_odeobj->body();
if (b) {
dBodyEnable(body);
} else {
dBodyDisable(body);
}
}
}
void PhysicsBody::setEnable(bool enable)
{
if(enable)
{
dBodyEnable(_BodyID);
}
else
{
dBodyDisable(_BodyID);
}
}
void Construction::Activate()
{
for(int i=0; i<nObjects; i++)
{
dBodyEnable( ObjectList[i].Body );
if( ObjectList[i].HasGeom )
dGeomEnable( ObjectList[i].Geom );
}
Active = true;
}
void set_phys_join_attr_f(dBodyID body1, dBodyID body2, int p, float f)
{
dJointID joint = find_shared_joint(body1, body2);
if (joint)
{
if (body1) dBodyEnable(body1);
if (body2) dBodyEnable(body2);
switch (p)
{
case dParamLoStop:
case dParamHiStop:
case dParamLoStop2:
case dParamHiStop2:
set_phys_joint_attr(joint, p, RAD(f));
break;
default:
set_phys_joint_attr(joint, p, f);
break;
}
}
}
bool Physics::registerObject(Object* obj){
#ifdef ODE
obj->bodyId=dBodyCreate(worldId);
dBodySetPosition(obj->bodyId, obj->pos.x, obj->pos.y, obj->pos.z);
dMassAdjust(&obj->ODEmass,obj->mass);
dBodySetMass(obj->bodyId, &obj->ODEmass);
if(obj->dynamic){
dBodyEnable(obj->bodyId);
}else{
dBodyDisable(obj->bodyId);
}
obj->bodyIdInit=true;
#endif
return true;
}
void Player::Update(scalar dt) {
const dReal * pos = dBodyGetPosition(bodyId);
position.x = pos[0];
position.y = pos[1];
position.z = pos[2];
if (moveForward) {
TGen::Vector3 hej = direction * 2.6f * dt;
dBodyEnable(bodyId);
dBodySetForce(bodyId, hej.x, hej.y, hej.z);
//dBodySetLinearVel(bodyId, hej.x, hej.y, hej.z);
}
else {
// dBodySetLinearVel(bodyId, 0.0f, 0.0f, 0.0f);
}
if (spinLeft)
lookDir += dt * 2.0f;
if (spinRight)
lookDir -= dt * 2.0f;
direction.x = sin(lookDir);
direction.z = cos(lookDir);
dMatrix3 ident;
dRSetIdentity(ident);
dBodySetRotation(bodyId, ident);
dBodySetAngularVel(bodyId, 0.0f, 0.0f, 0.0f);
dBodySetTorque(bodyId, 0.0f, 0.0f, 0.0f);
//std::cout << "player: " << std::string(position) << std::endl;
}
void ODE_Particle::Enable()
{
dBodyEnable(body);
}
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
dWorldStepFast1 (world,0.05,ITERS);
#endif
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
void keyPressed (unsigned char key, int x, int y) {
//These next 3 are for shooting our ball gun so that they go in the direction of the eye (hopefully)
double eyeX = 2*dim*Sin(th)*Cos(ph);
double eyeY = -2*dim *Sin(ph);
double eyeZ = -2*dim*Cos(th)*Cos(ph);
dReal force = 1;//for force of gun
switch(key){
case 'w': {
xpos += (Sin(th)* 0.3);
zpos -= (Cos(th)* 0.3);
//Start swaying gun while walking
walk = 1;
break;
}
case 'W': {
xpos += (Sin(th)* 0.3);
zpos -= (Cos(th)* 0.3);
//Start swaying gun while walking
walk = 1;
break;
}
//strafing
case 'a': {
{
xpos -= (Cos(th)) * 0.3;//Set x negative i.e. move object into negative x
zpos -= (Sin(th)) * 0.3;//Set z negative
//Start swaying gun while walking
walk = 1;
}
break;
}
case 'A': {
{
xpos -= (Cos(th)) * 0.3;//Set x negative i.e. move object into negative x
zpos -= (Sin(th)) * 0.3;//Set z negative
//Start swaying gun while walking
walk = 1;
}
break;
}
case 's': {
xpos -= (Sin(th)* 0.3);
zpos += (Cos(th)* 0.3);
//Start swaying gun while walking
walk = 1;
break;
}
case 'S': {
xpos -= (Sin(th)* 0.3);
zpos += (Cos(th)* 0.3);
//Start swaying gun while walking
walk = 1;
break;
}
//strafing
case 'd': {
xpos += (Cos(th)) * 0.3;//Set z positive
zpos += (Sin(th)) * 0.3;//Set x positive i.e. move object into positive x
//Start swaying gun while walking
walk = 1;
break;
}
case 'D': {
xpos += (Cos(th)) * 0.3;//Set z positive
zpos += (Sin(th)) * 0.3;//Set x positive i.e. move object into positive x
//Start swaying gun while walking
walk = 1;
break;
}
case 'l':{
light = 1 - light;
glDisable(GL_LIGHT0);//Turn the light off if on
break;
}
case 'L':{
light = 1 - light;
glDisable(GL_LIGHT0);//Turn the light off if on
break;
}
case 'y':{
ylight += 1;
break;
}
case 'Y':{
ylight -= 1;
break;
}
case 'h':{
distance += 1;
break;
}
case 'H':{
distance -= 1;
if (distance <= 0)
{
distance = 0;
}
break;
}
//Alter lighting intensities
case 'v':{
emission += 1;
if (emission >= 100)
{
emission = 100;
}
break;
}
case 'V':{
emission -= 1;
if (emission <= 0)
{
emission = 0;
}
break;
}
case 'b':{
specular += 1;
if (specular >= 100)
{
specular = 100;
}
break;
}
case 'B':{
specular -= 1;
if (specular <= 0)
{
specular = 0;
}
break;
}
case 'm':{
diffuse += 1;
if (diffuse >= 100)
{
diffuse = 100;
}
break;
}
case 'M':{
diffuse -= 1;
if (diffuse <= 0)
{
diffuse = 0;
}
break;
}
case 'n':{
ambient += 1;
if (ambient >= 100)
{
ambient = 100;
}
break;
}
case 'N':{
ambient -= 1;
if (ambient <= 0)
{
ambient = 0;
}
break;
}
//Space to fire balls
case 32:{
draw = 1;
fire = 1;//Animate gun
dMatrix3 R;
//Thanks to the source code for demo_crash in ode demo programs
dBodyEnable(ball_body2);
dBodySetPosition(ball_body2, xpos, ypos, zpos);//****Maybe lower y position to be level with the ground? did not try 11/4
dBodySetLinearVel(ball_body2, eyeX*force, ((eyeY*force)), eyeZ*force);//+ypos to give it same elevation as starting eyepos
dBodySetAngularVel (ball_body2, 0, 0, 0);//Force a linear angle, if this is not set then some default turns on and causes a curve
dRFromAxisAndAngle(R, 1, 0, 0, -(PI / 2));//Set the rotation matrix that sets the rotation of the ball
dBodySetRotation(ball_body2, R);
break;
}
//Press '1' to get another light
case 49:{
light1 = 1 - light1;
glDisable(GL_LIGHT1);//Turn the light off if on
break;
}
//Esc to close program
case 27:{
//Return ODE physics bodies to memory, requires dInitODE() in main() to not crash
dWorldDestroy(world);
dCloseODE();
//Exit the program
exit(0);
}
}
}
void CPhysicObject::addForce(const CVector& force){
if ( !dBodyIsEnabled (myBody) )
dBodyEnable(myBody);
dBodyAddForce(myBody, force.v[0], force.v[1], force.v[2]);
};
//Assistance with ode from http://www.alsprogrammingresource.com/basic_ode.html
//and from the ODE user manual, not a lot of resources out there!
void initODE()
{
int i;
int j = 0;//For placing skyPanels
int k = 0;//For placing sphere in box
int q = 0;//For placing target boxes
int r = 0;//For placing targetBoxes
int s = 0;//FOr placing targetBoxes
dReal radius = 0.5;//For sphere's in scene that bounce around/ get shot
dMass m;//For mass of sphere
dMass mp;//For mass of platform
dMass md;//For mass of doorway
dMass mpy;//For mass of pyramid
dMass mbox;//For box of balls
dMass msp;//For skyPanels
dMass mtb;//For targetBox
// Create a new, empty world and assign its ID number to World. Most applications will only need one world.
world = dWorldCreate();
// Create a new collision space and assign its ID number to Space, passing 0 instead of an existing dSpaceID.
// There are three different types of collision spaces we could create here depending on the number of objects
// in the world but dSimpleSpaceCreate is fine for a small number of objects. If there were more objects we
// would be using dHashSpaceCreate or dQuadTreeSpaceCreate (look these up in the ODE d
spacePhy = dSimpleSpaceCreate(0);
// Create a joint group object and assign its ID number to contactgroup. dJointGroupCreate used to have a
// max_size parameter but it is no longer used so we just pass 0 as its argument.
contactgroup = dJointGroupCreate(0);
//Set the gravity of the world where y is up
dWorldSetGravity(world, 0, -0.1, 0);
// These next two functions control how much error correcting and constraint force mixing occurs in the world.
// Don't worry about these for now as they are set to the default values and we could happily delete them from
// this example. Different values, however, can drastically change the behaviour of the objects colliding, so
// I suggest you look up the full info on them in the ODE docs.
dWorldSetERP(world, 0.2);
dWorldSetCFM(world, 1e-5);
// This function sets the velocity that inter-penetrating objects will separate at. The default value is infinity.
dWorldSetContactMaxCorrectingVel(world, 0.9);
// This function sets the depth of the surface layer around the world objects. Contacts are allowed to sink into
// each other up to this depth. Setting it to a small value reduces the amount of jittering between contacting
// objects, the default value is 0.
dWorldSetContactSurfaceLayer(world, 0.0);
// To save some CPU time we set the auto disable flag to 1. This means that objects that have come to rest (based
// on their current linear and angular velocity) will no longer participate in the simulation, unless acted upon
// by a moving object. If you do not want to use this feature then set the flag to 0. You can also manually enable
// or disable objects using dBodyEnable and dBodyDisable, see the docs for more info on this.
dWorldSetAutoDisableFlag(world, 1);
//This brings us to the end of the world settings, now we have to initialize the objects themselves.
//****************Ball 1
// Create a new body for our object in the world and get its ID.
ball_body = dBodyCreate(world);
//Set ball1 geometries
dMassSetZero(&ball_mass);
dMassSetSphereTotal(&ball_mass, DENSITY, 2.0);
dBodySetMass(ball_body, &ball_mass);
ball_geom = dCreateSphere(spacePhy, radius);
dGeomSetData(ball_geom, (void *)"ball");
dGeomSetBody(ball_geom, ball_body);
dBodyEnable(ball_body);
//Next we set the position of the new body
dBodySetPosition(ball_body, 5, 100, 15);
//****************Ball 2
// Create a new body for our object in the world and get its ID.
ball_body2 = dBodyCreate(world);
//Set ball1 geometries
dMassSetZero(&ball_mass2);
dMassSetSphereTotal(&ball_mass2, DENSITY, 2.0);
dBodySetMass(ball_body2, &ball_mass2);
ball_geom2 = dCreateSphere(spacePhy, radius);
dGeomSetData(ball_geom2, (void *)"ball2");
dGeomSetBody(ball_geom2, ball_body2);
dBodySetGravityMode (ball_body2, 0);
//******End of ball 2
//*************Ball 3, a new way of creating a ball with physical properties
ball_body3 = dBodyCreate(world);
dMatrix3 R;
dBodySetPosition(ball_body3, -5, 100, 15);
dRFromAxisAndAngle(R, 1, 0, 0, -1.57);
dBodySetRotation(ball_body3, R);
// Here we use dMassSetSphere instead of dMassSetBox
// and we pass the local radius variable as the third parameter
dMassSetSphere(&m, DENSITY, radius);
// To create the sphere object we use dCreateSphere and pass it the same local radius variable
ball_geom3 = dCreateSphere(spacePhy, radius);
dGeomSetBody(ball_geom3, ball_body3);
dBodySetMass(ball_body3, &m);
//*****End of ball3
//*************Box o balls properties
for (i = 0; i < 61; i++)
{
// Create a new body for our object in the world and get its ID.
balls_body[i] = dBodyCreate(world);
//Set ball1 geometries
dMassSetZero(&balls_mass[i]);
dMassSetSphereTotal(&balls_mass[i], DENSITY, 2.0);
dBodySetMass(balls_body[i], &balls_mass[i]);
balls_geom[i] = dCreateSphere(spacePhy, radius);
dGeomSetData(balls_geom[i], (void *)"ball");
dGeomSetBody(balls_geom[i], balls_body[i]);
dBodyDisable(balls_body[i]);//Disable bodies, enabled auto after hit
//Box 1
if (i <31)
{
dBodySetPosition(balls_body[i], (-20), 28.5, 7.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16)
}
//Box 2
if (i >= 31)
{
dBodySetPosition(balls_body[i], (25), 38.5, -14.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16)
}
k++;
}
//******End of Box o Balls
//**************platform geometries
//create 5 platform's with physics things
for (i = 0; i < 15; i++)
{
platform_body[i] = dBodyCreate(world);//Add the platform body to the world
//Set the geom platform
// Here we use dMassSetBox
//set DENSITY to be very high to prevent it from being moved from balls
dMassSetBox(&mp, DENSITY*1500, 4.5, 0.5, 18.0);
//It works better if the invisible physics box is slightly larger than the object we're drawing
platform_geom[i] = dCreateBox(spacePhy, 4.5, 0.5, 18.0);
dGeomSetBody(platform_geom[i], platform_body[i]);
dBodySetMass(platform_body[i], &mp);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (platform_body[i], 0);
}
//*******End of platform geometries
//*******Set the doorway properties, 18 platforms make all 3 doors
for (i = 0; i < 18; i++)
{
doorway_body[i] = dBodyCreate(world);//Add the platform body to the world
//Set the geom platform
// Here we use dMassSetBox
//set DENSITY to be very high to prevent it from being moved from balls
dMassSetBox(&md, DENSITY*1500, 4.0, 0.25, 2.0);
//It works better if the invisible physics box is slightly larger than the object we're drawing
doorway_geom[i] = dCreateBox(spacePhy, 4.0, 0.25, 2.0);
dGeomSetBody(doorway_geom[i], doorway_body[i]);
dBodySetMass(doorway_body[i], &md);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (doorway_body[i], 0);
}
//*******End of doorway properties
//*******Set the Box o' Balls properties, 6 sides total make the larger box
for (i = 0; i < 12; i++)
{
boxSide_body[i] = dBodyCreate(world);
}
//Box 1
//Need to set position here to affect the box with gravity
dBodySetPosition(boxSide_body[0], -20.0, 28.0, 16);//Front, rotated 0 deg. on y axis
dBodySetPosition(boxSide_body[1], -12.25, 28.0, 8.0);//Right, rotated 90 deg. on y axis
dBodySetPosition(boxSide_body[2], -20.0, 28.0, 0);//Back, rotated 0 deg. on y axis
dBodySetPosition(boxSide_body[3], -27.75, 28.0, 8.0);//Left, rotated 90 deg. on y axis
dBodySetPosition(boxSide_body[4], -20.0, 32.0, -0.23);//Top, rotated 90 deg. on x axis
dBodySetPosition(boxSide_body[5], -20.0, 28.0, -0.23);//Bottom, rotated 90 deg. on x axis
//Need to set position here to affect the box with gravity
//Box 2
dBodySetPosition(boxSide_body[6], 30.0, 38.0, -16);//Front, rotated 0 deg. on y axis
dBodySetPosition(boxSide_body[7], 22.25, 38.0, -8.0);//Right, rotated 90 deg. on y axis
dBodySetPosition(boxSide_body[8], 30.0, 38.0, 0);//Back, rotated 0 deg. on y axis
dBodySetPosition(boxSide_body[9], 37.75, 38.0, -8.0);//Left, rotated 90 deg. on y axis
dBodySetPosition(boxSide_body[10], 30.0, 42.0, -16.23);//Top, rotated 90 deg. on x axis*****Unsure about z here
dBodySetPosition(boxSide_body[11], 30.0, 38.0, -16.23);//Bottom, rotated 90 deg. on x axis
//Box 1
for (i = 0; i < 6; i++)
{
if (i < 4)//Small sides
{
// Here we use dMassSetBox, we want a lower density here to be able to destroy the box
dMassSetBox(&mbox, DENSITY, 16, 1, 0.25);
//It works better if the invisible physics box is slightly larger than the object we're drawing
boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.25);
dGeomSetBody(boxSide_geom[i], boxSide_body[i]);
dBodySetMass(boxSide_body[i], &mbox);
}
if (i >=4)//Large top and bottom
{
// Here we use dMassSetBox, we want a lower density here to be able to destroy the box
dMassSetBox(&mbox, DENSITY, 16, 16, 0.5);
//It works better if the invisible physics box is slightly larger than the object we're drawing
boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 0.5);
dGeomSetBody(boxSide_geom[i], boxSide_body[i]);
dBodySetMass(boxSide_body[i], &mbox);
}
//Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one)
dBodyDisable(boxSide_body[i]);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (boxSide_body[i], 1);
}
//Box 2
for (; i < 12; i++)
{
if (i < 10)//Small sides
{
// Here we use dMassSetBox, we want a lower density here to be able to destroy the box
dMassSetBox(&mbox, DENSITY, 16, 1, 0.5);
//It works better if the invisible physics box is slightly larger than the object we're drawing
boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.5);
dGeomSetBody(boxSide_geom[i], boxSide_body[i]);
dBodySetMass(boxSide_body[i], &mbox);
}
if (i >=10)//Large top and bottom
{
// Here we use dMassSetBox, we want a lower density here to be able to destroy the box
dMassSetBox(&mbox, DENSITY, 16, 16, 1.0);
//It works better if the invisible physics box is slightly larger than the object we're drawing
boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 1.0);
dGeomSetBody(boxSide_geom[i], boxSide_body[i]);
dBodySetMass(boxSide_body[i], &mbox);
}
//Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one)
dBodyDisable(boxSide_body[i]);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (boxSide_body[i], 1);
}
//*******End of Box o' Balls properties
//*******Set the pyramid properties, 120 pyramids make diamond excluding inverted that fill holes (gotta cheat)
pyramid_body = dBodyCreate(world);//Add the platform body to the world
//Set the geom platform
// Here we use dMassSetBox
//set DENSITY to be very high to prevent it from being moved from balls
dMassSetBox(&mpy, DENSITY*1500, 2.6, 4.5, 2.6);
//It works better if the invisible physics box is slightly larger than the object we're drawing
pyramid_geom = dCreateBox(spacePhy, 2.6, 4.5, 2.6);
dGeomSetBody(pyramid_geom, pyramid_body);
dBodySetMass(pyramid_body, &mpy);
dBodyDisable(pyramid_body);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (pyramid_body, 0);
//*******End of pyramid properties
//*******skyPanels physics
for(j = 0; j < 60; j++)
{
skyPanel_body[j] = dBodyCreate(world);//Add the skypanel body to the world
//Set the geom skypanel
// Here we use dMassSetBox
//set DENSITY to be very high to prevent it from being moved from balls
dMassSetBox(&msp, DENSITY*1500, 3.0, 7.0, 1.0);
//It works better if the invisible physics box is slightly larger than the object we're drawing
skyPanel_geom[j] = dCreateBox(spacePhy, 3.0, 7.0, 1.0);
dGeomSetBody(skyPanel_geom[j], skyPanel_body[j]);//Bind the geom to the body so that we only need to place the body to also set the geom
dBodySetMass(skyPanel_body[j], &msp);
dBodyDisable(skyPanel_body[j]);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (skyPanel_body[j], 0);//Gravity off
}
//*******End of skyPanels physics
//*******Set targetBox physics
for(q = 0; q < 114; q++)
{
targetBox_body[q] = dBodyCreate(world);//Add the skypanel body to the world
//Set the geom target box
// Here we use dMassSetBox
dMassSetBox(&mtb, (0.05), 4.0, 4.0, 4.0);
//It works better if the invisible physics box is slightly larger than the object we're drawing
targetBox_geom[q] = dCreateBox(spacePhy, 4.0, 4.0, 4.0);
dGeomSetBody(targetBox_geom[q], targetBox_body[q]);//Bind the geom to the body so that we only need to place the body to also set the geom
dBodySetMass(targetBox_body[q], &mtb);
dBodyDisable(targetBox_body[q]);
//Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity.
dBodySetGravityMode (targetBox_body[q], 1);//Gravity on
//Pile 1
if(q < 22)
{
dBodySetPosition(targetBox_body[q], (-60 + (s * 4)), (2 + r), -30);//Set position here, otherwise unaffected by gravity
s++;//update x axis
if (q == 8)
{
r += 4;
s = 0;
}
if (q == 14)
{
r += 4;
s = 0;
}
if (q == 18)
{
r += 4;
s = 0;
}
if (q == 20)
{
r += 4;
s = 0;
}
//Reset values for next pile of boxes
if (q == 21)
{
r = 0;
s = 0;
}
}
//Pile 2
if((q > 21) && (q < 43))
{
dBodySetPosition(targetBox_body[q], (-30 + (s * 4)), (2 + r), 90);
s++;//Update x pos
if (q == 29)
{
r += 4;
s = 0;
}
if (q == 35)
{
r += 4;
s = 0;
}
if (q == 39)
{
r += 4;
s = 0;
}
if (q == 41)
{
r += 4;
s = 0;
}
//Reset values for next pile of boxes
if (q == 42)
{
r = 0;
s = 0;
}
}
//Pile 3
if((q > 42) && (q < 64))
{
dBodySetPosition(targetBox_body[q], (110), (2 + r), (0 + (s * 4)));
s++;//Update x pos
if (q == 50)
{
r += 4;
s = 0;
}
if (q == 56)
{
r += 4;
s = 0;
}
if (q == 60)
{
r += 4;
s = 0;
}
if (q == 62)
{
r += 4;
s = 0;
}
//Reset values for next pile of boxes
if (q == 63)
{
r = 0;
s = 0;
}
}
//Pile 4
if((q > 63) && (q < 85))
{
dBodySetPosition(targetBox_body[q], (-90), (2 + r), (0 + (s * 4)));
s++;//Update x pos
if (q == 71)
{
r += 4;
s = 0;
}
if (q == 77)
{
r += 4;
s = 0;
}
if (q == 81)
{
r += 4;
s = 0;
}
if (q == 83)
{
r += 4;
s = 0;
}
//Reset values for next pile of boxes
if (q == 84)
{
r = 0;
s = 0;
}
}
//Box tower 1
if((q > 84) && (q < 94))
{
dBodySetPosition(targetBox_body[q], (-60), (2 + r), (30));
r+=1;//Put boxes slightly inside each other
if (q == 93)
{
r = 0;//Reset r
}
}
//Box tower 2
if((q > 93) && (q < 104))
{
dBodySetPosition(targetBox_body[q], (70), (2 + r), (-90));
r+=1;//Put boxes slightly inside each other
if (q == 103)
{
r = 0;//Reset r
}
}
//Box tower 3
if((q > 103) && (q < 114))
{
dBodySetPosition(targetBox_body[q], (60), (2 + r), (90));
r+=1;//Put boxes slightly inside each other
if (q == 103)
{
r = 0;//Reset r
}
}
}
//*******End of targetBox Physics
//Set the ground location:
//First three param's set the normal vector, last param sets the distance according to the plane equation equation a*x+b*y+c*z=d and must have length 1
ground = dCreatePlane(spacePhy, 0, 2.0, 0, 0);//Plane where I have most objects sitting, y =2
// Here I have set the initial linear velocity to stationary and let gravity do the work on our spheres, but you can experiment
// with the velocity vector to change the starting behavior. You can also set the rotational velocity for the new
// body using dBodySetAngularVel which takes the same parameters.
tempVect.x = 0.0;
tempVect.y = 0.0;
tempVect.z = 0.0;
dBodySetLinearVel(ball_body, tempVect.x, tempVect.y, tempVect.z);
dBodySetLinearVel(ball_body2, tempVect.x, tempVect.y, tempVect.z);
dBodySetLinearVel(ball_body3, tempVect.x, tempVect.y, tempVect.z);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
}
void CPHCharacter::UnFreezeContent()
{
dBodyEnable(m_body);
CPHObject::UnFreezeContent();
}
//===========================================================================
void cODEGenericBody::enableDynamics()
{
if (m_ode_body == NULL) { return; }
dBodyEnable(m_ode_body);
dGeomEnable(m_ode_geom);
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCCylinder (0,radius,length);
dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dReal maxheight = 0;
for (k=0; k<num; k++)
{
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRSetIdentity (R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
//<---- Convex Object
else if (cmd == 'v')
{
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
obj[i].geom[0] = dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
#else
obj[i].geom[0] = dCreateConvex (space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x' && USE_GEOM_OFFSET) {
setBody = 1;
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule (space,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate (&m2,drot[k]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition (obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// Translation *after* rotation
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<GPB; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == 'p'&& selected >= 0)
{
const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
rot[0],rot[1],rot[2],rot[3],
rot[4],rot[5],rot[6],rot[7],
rot[8],rot[9],rot[10],rot[11]);
}
else if (cmd == 'f' && selected >= 0 && selected < num) {
if (dBodyIsEnabled(obj[selected].body))
doFeedback = 1;
}
}
void Rigid::Enable()
{
VisualObject::Enable();
dBodyEnable(mBodyID);
updatePhysica();
}
static void callback(void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
/* Two geoms associated with the same body do not collide. */
if (b1 == 0 || b2 == 0 || b1 != b2)
{
dContact contact[MAX_CONTACTS];
size_t sz = sizeof (dContact);
int i, n;
/* Find and enumerate all collision points. */
if ((n = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sz)))
{
struct geom_data *d1 = get_data(o1);
struct geom_data *d2 = get_data(o2);
/* Compute collision parameters from geom parameters. */
dReal bounce = MAX(d1->bounce, d2->bounce);
dReal friction = MIN(d1->friction, d2->friction);
dReal soft_erp = MIN(d1->soft_erp, d2->soft_erp);
dReal soft_cfm = MAX(d1->soft_cfm, d2->soft_cfm);
unsigned long category1 = dGeomGetCategoryBits(o1);
unsigned long category2 = dGeomGetCategoryBits(o2);
if (b1) dBodyEnable(b1);
if (b2) dBodyEnable(b2);
/* Create a contact joint for each collision, if requsted. */
if ((category1 & d2->response) ||
(category2 & d1->response))
for (i = 0; i < n; ++i)
if (dGeomGetClass(contact[0].geom.g1) != dRayClass &&
dGeomGetClass(contact[0].geom.g2) != dRayClass)
{
dJointID c;
contact[i].surface.mode = dContactBounce
| dContactSoftCFM
| dContactSoftERP;
contact[i].surface.mu = friction;
contact[i].surface.bounce = bounce;
contact[i].surface.soft_cfm = soft_cfm;
contact[i].surface.soft_erp = soft_erp;
contact[i].surface.bounce_vel = (dReal) 0.10;
c = dJointCreateContact(world, group, contact + i);
dJointAttach(c, b1, b2);
}
/* Report contacts to the Lua script, if requested. */
if ((category1 & d2->callback) ||
(category2 & d1->callback))
for (i = 0; i < n; ++i)
{
float p[3];
float v[3];
p[0] = (float) contact[i].geom.pos[0];
p[1] = (float) contact[i].geom.pos[1];
p[2] = (float) contact[i].geom.pos[2];
v[0] = (float) contact[i].geom.normal[0];
v[1] = (float) contact[i].geom.normal[1];
v[2] = (float) contact[i].geom.normal[2];
do_contact_script(d1->entity, d2->entity,
p, v, (float) contact[i].geom.depth);
}
}
}
}
void Physics::perframe(){
//dWorldStep(worldId);
return; //not implemented yet
#ifdef ODE
for(int i=0; i<level->colliders.size(); i++){
Object* obj=level->colliders[i];
if(level->colliders[i]->collideType=="box" || level->colliders[i]->collideType=="cube"){
if(level->colliders[i]->oldCollideType!="box" && level->colliders[i]->oldCollideType!="cube"){
obj->geomId=dCreateBox(spaceId,obj->collideParameters.x,obj->collideParameters.y,obj->collideParameters.z);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
if(obj->boxBuilt){
float lx=level->colliders[i]->box.px-level->colliders[i]->box.nx;
float ly=level->colliders[i]->box.py-level->colliders[i]->box.ny;
float lz=level->colliders[i]->box.pz-level->colliders[i]->box.nz;
dGeomBoxSetLengths (level->colliders[i]->geomId, lx, ly, lz);
}
}else if(obj->collideType=="sphere"){
if(obj->oldCollideType!="sphere"){
obj->geomId=dCreateSphere(spaceId,obj->collideParameters.x);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
}
}else if(obj->collideType=="plane"){
obj->geomId=dCreatePlane(spaceId,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x);
}else if(obj->collideType=="cylindar"){
obj->geomId=dCreateCCylinder(spaceId,obj->collideParameters.x,obj->collideParameters.y);
}else if(obj->collideType=="triangle"){
if(obj->oldCollideType!="triangle"){
dTriMeshDataID tridat=dGeomTriMeshDataCreate();
obj->geomId=dCreateTriMesh (spaceId, tridat,
NULL,
NULL,
NULL);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
}
if(obj->collide){
dGeomEnable(obj->geomId);
}else{
dGeomDisable(obj->geomId);
}
for(int j=0; j<level->colliders.size(); j++){
if(!level->colliders[i]->geomIdInit || !level->colliders[j]->geomIdInit){
continue;
}
dContactGeom contactg[2];
int cnt=dCollide(level->colliders[i]->geomId,level->colliders[j]->geomId,0,contactg,sizeof(dContactGeom));
if(cnt>0){
dSurfaceParameters surf;
surf.mode=0;
surf.mode=dContactBounce;
surf.mu=level->colliders[i]->friction;
surf.bounce=level->colliders[i]->bounce;
surf.bounce_vel=level->colliders[i]->bounceThreshold;
dContact contact;
contact.geom=contactg[0];
contact.surface=surf;
dJointID joint=dJointCreateContact(worldId,0,&contact);
if(!level->colliders[i]->dynamic){
dJointAttach(joint,level->colliders[j]->bodyId,0);
}else if(!level->colliders[j]->dynamic){
dJointAttach(joint,level->colliders[i]->bodyId,0);
}else{
dJointAttach(joint,level->colliders[i]->bodyId,level->colliders[j]->bodyId);
}
}
}
}
dWorldSetGravity(worldId,gravity.x,gravity.y,gravity.z);
dWorldQuickStep(worldId,1);
for(int i=0; i<level->dynamicObjects.size(); i++){
if(!level->dynamicObjects[i]->bodyIdInit){
continue;
}
if(level->dynamicObjects[i]->dynamic){
dBodyEnable(level->dynamicObjects[i]->bodyId);
}else{
dBodyDisable(level->dynamicObjects[i]->bodyId);
}
const dReal* pos=new dReal[3];
pos=dBodyGetPosition(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->pos.x=pos[0];
level->dynamicObjects[i]->pos.y=pos[1];
level->dynamicObjects[i]->pos.z=pos[2];
const dReal* rot=new dReal[3];
rot=dBodyGetRotation(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->rot.x=rot[0];
level->dynamicObjects[i]->rot.y=rot[1];
level->dynamicObjects[i]->rot.z=rot[2];
}
#endif
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
// called when a key pressed
static void command( int cmd )
{
int i,k;
dReal sides[3];
dMass m;
cmd = locase( cmd );
if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' )
{
if ( num < NUM )
{
i = num;
num++;
}
else
{
i = nextobj;
nextobj++;
if ( nextobj >= num ) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy( obj[i].body );
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] );
}
memset( &obj[i],0,sizeof( obj[i] ) );
}
obj[i].body = dBodyCreate( world );
for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if ( random_pos )
{
dBodySetPosition( obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 );
dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 );
}
else
{
dReal maxheight = 0;
for ( k=0; k<num; k++ )
{
const dReal *pos = dBodyGetPosition( obj[k].body );
if ( pos[2] > maxheight ) maxheight = pos[2];
}
dBodySetPosition( obj[i].body, 0,0,maxheight+1 );
dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 );
}
dBodySetRotation( obj[i].body,R );
dBodySetData( obj[i].body,( void* )( size_t )i );
if ( cmd == 'b' )
{
dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] );
obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] );
}
else if ( cmd == 'c' )
{
sides[0] *= 0.5;
dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] );
obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] );
}
else if ( cmd == 's' )
{
sides[0] *= 0.5;
dMassSetSphere( &m,DENSITY,sides[0] );
obj[i].geom[0] = dCreateSphere( space,sides[0] );
}
else if ( cmd == 'v' )
{
obj[i].geom[0] = dCreateConvex( space,
convexBunnyPlanes,
convexBunnyPlaneCount,
convexBunnyPoints,
convexBunnyPointCount,
convexBunnyPolygons );
/// Use equivalent TriMesh to set mass
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount,
( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) );
dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 );
dMassSetTrimesh( &m, DENSITY, triMesh );
dGeomDestroy( triMesh );
dGeomTriMeshDataDestroy( new_tmdata );
printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] );
dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] );
dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] );
}
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body );
}
dBodySetMass( obj[i].body,&m );
}
if ( cmd == ' ' )
{
selected++;
if ( selected >= num ) selected = 0;
if ( selected < 0 ) selected = 0;
}
else if ( cmd == 'd' && selected >= 0 && selected < num )
{
dBodyDisable( obj[selected].body );
}
else if ( cmd == 'e' && selected >= 0 && selected < num )
{
dBodyEnable( obj[selected].body );
}
else if ( cmd == 'a' )
{
show_aabb ^= 1;
}
else if ( cmd == 't' )
{
show_contacts ^= 1;
}
else if ( cmd == 'r' )
{
random_pos ^= 1;
}
}
