static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldQuickStep (world,0.02);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
if (doFeedback)
{
if (fbnum>MAX_FEEDBACKNUM)
printf("joint feedback buffer overflow!\n");
else
{
dVector3 sum = {0, 0, 0};
printf("\n");
for (int i=0; i<fbnum; i++) {
dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
printf("%f %f %f\n", f[0], f[1], f[2]);
sum[0] += f[0];
sum[1] += f[1];
sum[2] += f[2];
}
printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
dMass m;
dBodyGetMass(obj[selected].body, &m);
printf("Object G=%f\n", GRAVITY*m.mass);
}
doFeedback = 0;
fbnum = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
void Object::UpdateDisableState()
{
bool disabled = true;
if (iBody == 0)
return;
if (dBodyIsEnabled(iBody) == 0)
return;
const dReal *lv = dBodyGetLinearVel(iBody);
const dReal *av = dBodyGetAngularVel(iBody);
if ((lv[0]*lv[0]+lv[1]*lv[1]+lv[2]*lv[2]) > DISABLE_THRESHOLD)
disabled = false;
if ((av[0]*av[0]+av[1]*av[1]+av[2]*av[2]) > DISABLE_THRESHOLD)
disabled = false;
if (disabled = false)
disabledSteps++;
if (disabledSteps > AUTO_DISABLE_STEPS)
{
dBodyDisable(iBody);
}
};
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldQuickStep (world,0.05);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
void CPHShellSplitterHolder::PhDataUpdate(dReal step)
{
SPLITTER_I i=m_splitters.begin(),e=m_splitters.end();
for(;i!=e;++i)
{
switch(i->m_type)
{
case CPHShellSplitter::splElement:
{
CPHElement* element=m_pShell->elements[i->m_element];
dBodyID body=element->get_body();//!element->EnabledStateOnStep()
if(!dBodyIsEnabled(body)) return;//
i->m_breaked=(element->FracturesHolder()->PhDataUpdate(element))||i->m_breaked;
break;
}
case CPHShellSplitter::splJoint:
{
CPHJoint *j=m_pShell->joints[i->m_joint];
//if(j->bActive)
i->m_breaked=j->JointDestroyInfo()->Update()||i->m_breaked;
break;
}
default: NODEFAULT;
}
m_has_breaks=m_has_breaks||i->m_breaked;
}
}
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 );
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
//if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
/*{
for (int i = 1; i < IndexCount; i++) {
dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
}
}*/
{const dReal* Pos = dGeomGetPosition(TriMesh);
const dReal* Rot = dGeomGetRotation(TriMesh);
{for (int i = 0; i < IndexCount / 3; i++){
const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
}}}
if (Ray){
dVector3 Origin, Direction;
dGeomRayGet(Ray, Origin, Direction);
dReal Length = dGeomRayGetLength(Ray);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}
}
void BaseWidget::setPosition(const dReal *position)
{
if(! dBodyIsEnabled(body)) return;
//Update position
int xPos = ABSOLUTE(position[0]);
int yPos = ABSOLUTE(position[1]);
// Update Qt's position
qDebug("Position: %i, %i", xPos, yPos);
move(xPos, yPos);
}
bool Construction::IsActive()
{
for(int i=0; i<nObjects; i++)
{
if(dBodyIsEnabled( ObjectList[i].Body ) == false )
return false;
}
return true;
}
static void simLoop (int pause)
{
const dReal stepsize = 0.02;
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) {
if (mov_type == 1)
moveplat_1(stepsize);
else
moveplat_2(stepsize);
dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
updatecam();
dWorldQuickStep (world,stepsize);
//dWorldStep (world,stepsize);
}
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
dsSetColor (1,0,0);
drawGeom (platform,0,0,show_aabb);
//usleep(5000);
}
void Sphere::Update()
{
const dReal *p,*r;
UpdateDisableState();
if(dBodyIsEnabled(iBody) == 0)
return;
p = dBodyGetPosition(iBody);
iPosition.x = p[0];
iPosition.y = p[1];
iPosition.z = p[2];
r = dGeomGetRotation(iGeom);
iRotate.x = p[0];
iRotate.y = p[1];
iRotate.z = p[2];
// v = dBodyGetLinearVel(iBody);
// dBodyAddForce(iBody,v[0]*v[0]+v[1]*v[1]+v[2]*v[2],0,0);
// printf("x: %f, y: %f z: %f\n",p[0],p[1],p[2]);
};
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
int color_changed = 0;
if (i==selected) {
dsSetColor (0,0.7,1);
color_changed = 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed = 1;
}
for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
{for (int i = 0; i < RayCount; i++){
dVector3 Origin, Direction;
dGeomRayGet(Rays[i], Origin, Direction);
dReal Length = dGeomRayGetLength(Rays[i]);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}}
}
void internal_collisionCallback(void *data, dGeomID o0, dGeomID o1)
{
if (dGeomIsSpace(o0) || dGeomIsSpace(o1))
{
// Colliding a space with either a geom or another space.
dSpaceCollide2(o0, o1, data, &internal_collisionCallback);
if (dGeomIsSpace(o0))
{
// Colliding all geoms internal to the space.
dSpaceCollide((dSpaceID) o0, data,
&internal_collisionCallback);
}
if (dGeomIsSpace(o1))
{
// Colliding all geoms internal to the space.
dSpaceCollide((dSpaceID) o1, data,
&internal_collisionCallback);
}
}
else
{
// Colliding two geoms.
// The following is a set of special cases where we might
// want to skip collision detection (but not all are
// enforced here for various reasons):
// 1. Two static Solids (neither geom has a body) AND
// neither Solid has a CollisionEventHandler AND there are
// not global handlers: this is enforced.
// 2. Two Shapes that are part of the same Solid (they
// share a body): this is not enforced because ODE
// already takes care of it.
// 3. Two sleeping Solids (note: both must have bodies to
// check this): this is enforced. (ODE should handle
// this, but it doesn't.)
// 4. Two Solids connected by a fixed Joint: this is
// enforced.
// 5. Two Solids connected by a Joint (besides ODE
// contact joints, which should never generate more
// contacts) with contacts disabled (note: both must have
// bodies to check this): this is enforced.
// 6. Solid0 is static, Solid1 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 7. Solid1 is static, Solid0 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 8. The two Solids' contact groups do not generate
// contacts when they collide AND neither Solid has a
// CollisionEventHandler AND there are no global handlers.
// Get the geoms' ODE body IDs.
dBodyID o0BodyID = dGeomGetBody(o0);
dBodyID o1BodyID = dGeomGetBody(o1);
bool solid0Static = (0 == o0BodyID);
bool solid1Static = (0 == o1BodyID);
// Check if both Solids are dynamic (i.e. have ODE bodies).
bool bothHaveBodies = true;
if (0 == o0BodyID || 0 == o1BodyID)
{
bothHaveBodies = false;
}
// If the two Solids are connected by a common Joint, get
// a pointer to that Joint.
Joint* commonJoint = NULL;
if (bothHaveBodies && dAreConnectedExcluding(o0BodyID,
o1BodyID, dJointTypeContact))
{
// This will become non-NULL only if there exists an ODE
// joint connecting the two bodies.
commonJoint = internal_getCommonJoint(o0BodyID, o1BodyID);
}
// Get pointers to the geoms' GeomData structures.
GeomData* geomData0 = (GeomData*) dGeomGetData(o0);
GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));
// Get pointers to the geoms' ShapeData structures.
const ShapeData* shape0 = geomData0->shape;
const ShapeData* shape1 = geomData1->shape;
// Get a pointer to the ODESimulator.
ODESimulator* sim = (ODESimulator*)data;
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim->groupsMakeContacts(
shape0->contactGroup, shape1->contactGroup);
// Find out whether the Simulator has static-to-sleeping
// contacts disabled.
bool ignoreStaticSleepingContacts =
!sim->areStaticSleepingContactsEnabled();
// Get pointers to the geoms' Solids.
Solid* solid0 = geomData0->solid;
Solid* solid1 = geomData1->solid;
// Get pointers to the two Solids' CollisionEventHandlers.
// These will be NULL if the Solids don't use
// CollisionEventHandlers.
CollisionEventHandler* handler0 =
solid0->getCollisionEventHandler();
CollisionEventHandler* handler1 =
solid1->getCollisionEventHandler();
bool neitherHasCollisionHandler = !(handler0 || handler1);
bool noGlobalCollisionHandlers =
sim->getNumGlobalCollisionEventHandlers() == 0;
// Now do the actual tests to see if we should return early.
// It is important here that we don't call dBodyIsEnabled on
// a static body because that crashes ODE.
bool case1 = neitherHasCollisionHandler &&
noGlobalCollisionHandlers && solid0Static && solid1Static;
//bool case2= o0BodyID == o1BodyID;
bool case3 = bothHaveBodies && !dBodyIsEnabled(o0BodyID)
&& !dBodyIsEnabled(o1BodyID);
bool case4 = commonJoint &&
commonJoint->getType() == FIXED_JOINT;
bool case5 = commonJoint
&& !commonJoint->areContactsEnabled();
bool case6 = solid0Static && 0 != o1BodyID
&& !dBodyIsEnabled(o1BodyID)
&& ignoreStaticSleepingContacts
&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
bool case7 = solid1Static && 0 != o0BodyID
&& !dBodyIsEnabled(o0BodyID)
&& ignoreStaticSleepingContacts
&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
bool case8 = !makeContacts && neitherHasCollisionHandler
&& noGlobalCollisionHandlers;
if (case1 || case3 || case4 || case5 || case6 || case7 || case8)
{
return;
}
// Now actually test for collision between the two geoms.
// This is one of the more expensive operations.
dWorldID theWorldID = sim->internal_getWorldID();
dJointGroupID theJointGroupID =
sim->internal_getJointGroupID();
dContactGeom contactArray[globals::maxMaxContacts];
int numContacts = dCollide(o0, o1, sim->getMaxContacts(),
contactArray, sizeof(dContactGeom));
// If the two objects didn't make any contacts, they weren't
// touching, so just return.
if (0 == numContacts)
{
return ;
}
// If at least one of the Solids has a CollisionEventHandler,
// send it a CollisionEvent.
if (handler0 || handler1 || !noGlobalCollisionHandlers)
{
// Call the CollisionEventHandlers. Note that we only
// use one contact point per collision: just the first one
// in the contact array. The order of the Solids
// passed to the event handlers is important: the first
// one should be the one whose event handler is
// getting called.
CollisionEvent e;
e.thisSolid = solid0;
e.otherSolid = solid1;
e.pos[0] = (real) contactArray[0].pos[0];
e.pos[1] = (real) contactArray[0].pos[1];
e.pos[2] = (real) contactArray[0].pos[2];
e.normal[0] = (real) contactArray[0].normal[0];
e.normal[1] = (real) contactArray[0].normal[1];
e.normal[2] = (real) contactArray[0].normal[2];
e.depth = (real) contactArray[0].depth;
if (handler0)
{
handler0->internal_pushCollisionEvent(e);
}
if (handler1)
{
// For the other Solid's CollisionEventHandler, we need
// to invert the normal and swap the Solid pointers.
e.normal *= -1;
e.thisSolid = solid1;
e.otherSolid = solid0;
handler1->internal_pushCollisionEvent(e);
}
sim->internal_recordCollision(e);
}
// Old version...
//// Early check to save some time.
//if (solid0->getCollisionEventHandler()
// || solid1->getCollisionEventHandler())
//{
// // Call the event handlers. Note: we only use one
// // contact point per collision; just use the first one
// // in the contact array. The order of the Solids
// // passed to the event handlers is important: the first
// // one should be the one whose event handler is
// // getting called.
// CollisionEvent e;
// e.solid0 = solid0;
// e.solid1 = solid1;
// e.pos[0] = contactArray[0].pos[0];
// e.pos[1] = contactArray[0].pos[1];
// e.pos[2] = contactArray[0].pos[2];
// e.normal[0] = contactArray[0].normal[0];
// e.normal[1] = contactArray[0].normal[1];
// e.normal[2] = contactArray[0].normal[2];
// e.depth = contactArray[0].depth;
// EventHandler* eventHandler =
// solid0->getCollisionEventHandler();
// if (eventHandler)
// {
// generateContacts0 =
// eventHandler->handleCollisionEvent(e);
// }
// e.normal *= -1; // Invert normal.
// e.solid0 = solid1; // Swap solid pointers.
// e.solid1 = solid0;
// eventHandler = solid1->getCollisionEventHandler();
// if (eventHandler)
// {
// generateContacts1 =
// eventHandler->handleCollisionEvent(e);
// }
//}
if (makeContacts)
{
// Invalidate the "freely-spinning" parameters.
((ODESolid*)solid0)->internal_setFreelySpinning(false);
((ODESolid*)solid1)->internal_setFreelySpinning(false);
for (int i = 0; i < numContacts; ++i)
{
const Material* m0 = &(shape0->material);
const Material* m1 = &(shape1->material);
dContact tempContact;
tempContact.surface.mode = dContactBounce |
dContactSoftERP; // | dContactSoftCFM;
// Average the hardness of the two materials.
assert(m0->hardness >= 0 && m0->hardness <= 1
&& m1->hardness >= 0 && m1->hardness <= 1);
real hardness = (m0->hardness + m1->hardness) *
(real) 0.5;
// Convert hardness to ERP. As hardness goes from
// 0.0 to 1.0, ERP goes from min to max.
tempContact.surface.soft_erp = hardness *
(defaults::ode::maxERP - defaults::ode::minERP) +
defaults::ode::minERP;
// Don't use contact CFM anymore. Just let it use
// the global value set in the ODE world.
//tempContact.surface.soft_cfm =
// defaults::ode::minCFM;
// As friction goes from 0.0 to 1.0, mu goes from 0.0
// to max, though it is set to dInfinity when
// friction == 1.0.
assert(m0->friction >= 0 && m0->friction <= 1
&& m1->friction >= 0 && m1->friction <= 1);
if (1.0 == m0->friction && 1.0 == m1->friction)
{
tempContact.surface.mu = dInfinity;
}
else
{
tempContact.surface.mu =
sqrt(m0->friction * m1->friction) *
defaults::ode::maxFriction;
}
// Average the bounciness of the two materials.
assert(m0->bounciness >= 0 && m0->bounciness <= 1
&& m1->bounciness >= 0 && m1->bounciness <= 1);
real bounciness = (m0->bounciness + m1->bounciness) *
(real) 0.5;
// ODE's bounce parameter, a.k.a. restitution.
tempContact.surface.bounce = bounciness;
// ODE's bounce_vel parameter is a threshold:
// the relative velocity of the two objects must be
// greater than this for bouncing to occur at all.
tempContact.surface.bounce_vel =
defaults::bounceThreshold;
// Old way to calculate bounce threshold; threshold
// was scaled by the collision bounciness, but this
// makes all objects with non-zero bounciness
// always bounce. This is undesirable because it
// takes a while for bouncing to totally diminish.
//tempContact.surface.bounce_vel =
// defaults::ode::maxBounceVel - bounciness *
// defaults::ode::maxBounceVel;
tempContact.geom = contactArray[i];
dJointID contactJoint = dJointCreateContact(
theWorldID, theJointGroupID, &tempContact);
// Note: the following line of code replaces the
// rest of this function which is commented out.
// TODO: test this and make sure the mass ratio
// issues are unimportant and that we never need
// "one-sided" contacts between two Solids.
dJointAttach(contactJoint, o0BodyID, o1BodyID);
//if (!bothHaveBodies)
//{
// // at least one object is static, so just handle
// // things like normal
// dJointAttach(contactJoint, o0BodyID, o1BodyID);
//}
//else
//{
// // TODO: We probably need to remove the following chunk of
// // code. The first case is obsolete since now both sides
// // always get contacts, if at all. (There isn't really a
// // good reason to have one side use contacts but not the
// // other; the side not wanting contacts would appear to be
// // static, so just make it static in the first place. On
// // the other hand, this may end up being desirable if
// // an object should be moved by some objects but not
// // others, and the "others" *do* collid with the first
// // object... but this situation may never come up. The
// // second case, using mass ratios to determine whether two
// // objects should collide, might not be an issue. Mass
// // ratios might only be a problem when the two objects are
// // constantly connected with a Joint.
// // Handle one-sided contacts for two cases: 1) only one of
// // the above event handlers actually wants contacts generated,
// // 2) if the mass ratio is above some threshold, treat it as
// // a one-sided collision solution: treat the more massive
// // object as static, calculate the collision like normal
// // (with the massive one being static), then also add the
// // massive object's velocity to the smaller one (velocity
// // calculated at the point of collision).
// // calculate mass ratio (use mass1 / mass2); if
// // the ratio is too high, mass1 is too large; if
// // the ratio is too low, mass2 is too large
// real massRatio = 0;
// dMass mass0, mass1;
// dBodyGetMass(o0BodyID, &mass0);
// dBodyGetMass(o1BodyID, &mass1);
// massRatio = mass0.mass / mass1.mass;
// // here we handle all the different collision
// // cases: one solid or the other or both may want
// // contacts generated; also, the mass ratio may
// // be outside the acceptable range
// if (true == generateContacts0 && true ==
// generateContacts1)
// {
// // both want contacts, neither wants to be
// // static
// if (massRatio > defaults::ode::maxMassRatio)
// {
// // ratio is too high - mass0 is too large,
// // treat solid0 as static
// dBodyEnable(o1BodyID);
// dJointAttach(contactJoint, 0, o1BodyID);
// }
// else if (massRatio <
// defaults::ode::minMassRatio)
// {
// // ratio is too low - mass1 is too large,
// // treat solid1 as static
// dBodyEnable(o0BodyID);
// dJointAttach(contactJoint, o0BodyID, 0);
// }
// else
// {
// //ratio is good - no static objects
// dJointAttach(contactJoint, o0BodyID,
// o1BodyID);
// }
// }
// else if (true == generateContacts0)
// {
// // solid0 wants contacts, solid1 wants to be
// // static
// if (massRatio > defaults::ode::maxMassRatio)
// {
// // ratio is too high - mass0 is too large,
// // treat solid0 and solid1 as static
// // i.e. don't generate a contact joint
// }
// else
// {
// // this block handles two cases which have
// // the same result:
// // 1. ratio is too low - mass1 is too
// // large, treat solid1 as static
// // 2. ratio is good - treat solid1 as
// // static
// dBodyEnable(o0BodyID);
// dJointAttach(contactJoint, o0BodyID, 0);
// }
// }
// else //generateContacts1 must be true
// {
// // solid1 wants contacts, solid0 wants to be
// // static
// if (massRatio < defaults::ode::minMassRatio)
// {
// // ratio is too low - mass1 is too large,
// // treat solid0 and solid1 as static
// // i.e. don't generate a contact joint
// }
// else
// {
// // this block handles two cases which have
// // the same result:
// // 1. ratio is too high - mass0 is too
// // large, treat solid0 as static
// // 2. ratio is good - treat solid0 as
// // static
// dBodyEnable(o1BodyID);
// dJointAttach(contactJoint, 0, o1BodyID);
// }
// }
//}
}
}
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
#if 1
// What is this for??? - Bram
if (!pause)
{
for (int i=0; i<num; i++)
for (int j=0; j < GPB; j++)
if (obj[i].geom[j])
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
setCurrentTransform(obj[i].geom[j]);
setCurrentTransform(TriMesh1);
setCurrentTransform(TriMesh2);
}
#endif
//if (!pause) dWorldStep (world,0.05);
if (!pause) dWorldQuickStep (world,0.05);
for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
dSpaceGetGeom(space, j);
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (obj[i].geom[j]) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
} else {
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
}
dTriIndex* Indices = (dTriIndex*)::Indices;
{const dReal* Pos = dGeomGetPosition(TriMesh1);
const dReal* Rot = dGeomGetRotation(TriMesh1);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
}}}
{const dReal* Pos = dGeomGetPosition(TriMesh2);
const dReal* Rot = dGeomGetRotation(TriMesh2);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 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;
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
dAASSERT(terrainY);
dAASSERT(terrainZ);
dsSetColor (0,1,0);
dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));
dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));
if (show_aabb)
{
dReal aabb[6];
dGeomGetAABB (terrainY,aabb);
dVector3 bbpos;
int i;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
dGeomGetAABB (terrainZ,aabb);
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dsDrawBox (bbpos,RI,bbsides);
}
dsSetColor (1,1,0);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
bool ODE_Particle::Is_enabled()
{
return dBodyIsEnabled (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);
}
bool PhysicsBody::getEnable(void) const
{
return dBodyIsEnabled(_BodyID);
}
static void simLoop (int pause)
{
int i,j;
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
//if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldQuickStep (world,0.05);
if (!pause) dWorldStepFast1 (world,0.05, 5);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
const dReal* pReal = dGeomGetPosition( gheight );
const dReal* RReal = dGeomGetRotation( gheight );
//
// Draw Heightfield
//
// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
int ox = (int) ( -HFIELD_WIDTH/2 );
int oz = (int) ( -HFIELD_DEPTH/2 );
// for ( int tx = -1; tx < 2; ++tx )
// for ( int tz = -1; tz < 2; ++tz )
{
dsSetColorAlpha (0.5,1,0.5,0.5);
dsSetTexture( DS_WOOD );
for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
{
dReal a[3], b[3], c[3], d[3];
a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
a[ 1 ] = heightfield_callback( NULL, i, j );
a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
b[ 1 ] = heightfield_callback( NULL, i + 1, j );
b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
dsDrawTriangle( pReal, RReal, a, c, b, 1 );
dsDrawTriangle( pReal, RReal, b, c, d, 1 );
}
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<num; i++)
{
for (j=0; j < GPB; j++)
{
if (i==selected)
{
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body))
{
dsSetColor (1,0.8,0);
}
else
{
dsSetColor (1,1,0);
}
if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
{
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++)
{
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
// Apply the 'other' matrix which is the oldest.
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
}
else
{
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
if ( show_aabb )
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (gheight,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void CPhysicObject::addForce(const CVector& force){
if ( !dBodyIsEnabled (myBody) )
dBodyEnable(myBody);
dBodyAddForce(myBody, force.v[0], force.v[1], force.v[2]);
};