// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
//===========================================================================
void cODEGenericBody::createDynamicCapsule(const double a_radius,
const double a_length,
bool a_staticObject,
const cVector3d& a_offsetPos,
const cMatrix3d& a_offsetRot)
{
// create ode dynamic body if object is non static
if (!a_staticObject)
{
m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world);
// store pointer to current object
dBodySetData (m_ode_body, this);
}
m_static = a_staticObject;
// build box
m_ode_geom = dCreateCCylinder(m_ODEWorld->m_ode_space, a_radius, a_length);
// adjust position offset
dGeomSetPosition (m_ode_geom, a_offsetPos.x, a_offsetPos.y, a_offsetPos.z);
// adjust orientation offset
dMatrix3 R;
R[0] = a_offsetRot.m[0][0];
R[1] = a_offsetRot.m[0][1];
R[2] = a_offsetRot.m[0][2];
R[4] = a_offsetRot.m[1][0];
R[5] = a_offsetRot.m[1][1];
R[6] = a_offsetRot.m[1][2];
R[8] = a_offsetRot.m[2][0];
R[9] = a_offsetRot.m[2][1];
R[10] = a_offsetRot.m[2][2];
dGeomSetRotation (m_ode_geom, R);
// set inertia properties
if (!m_static)
{
dMassSetCylinder (&m_ode_mass, 1.0, 3, a_radius, a_length); // 3 = x-axis direction
dMassAdjust(&m_ode_mass, m_mass);
dBodySetMass(m_ode_body,&m_ode_mass);
// attach body and geometry together
dGeomSetBody(m_ode_geom, m_ode_body);
}
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_CYLINDER;
// store dynamic model parameters
m_paramDynColModel0 = a_radius;
m_paramDynColModel1 = a_length;
m_paramDynColModel2 = 0.0;
m_posOffsetDynColModel = a_offsetPos;
m_rotOffsetDynColModel = a_offsetRot;
}
void ComponentPhysicsGeom::createGeomCapsule()
{
ASSERT(physicsEngine, "\"physicsEngine\" was null");
const float capsuleHeight = desiredHeight - collisionRadius*2.0f;
geom = dCreateCCylinder(physicsEngine->getSpace(),
collisionRadius,
capsuleHeight);
}
void CappedCylinder::createGeometry(dSpaceID space)
{
if(geometry == 0)
{
setGeometry(dCreateCCylinder(space, dReal(radius), dReal(height)));
dGeomSetPosition(geometry, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
dMatrix3 rotationMatrix;
ODETools::convertMatrix(rotation, rotationMatrix);
dGeomSetRotation(geometry, rotationMatrix);
//set user data pointer of ODE geometry object to this physical object
dGeomSetData(geometry, this);
//set collide bitfield
PhysicalObject::setCollideBitfield();
}
}
void Capsule::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
char mode) {
assert(mode & Body || mode & Geom);
if (!substanceManuallySet)
substance = odeHandle.substance;
this->mode=mode;
QMP_CRITICAL(3);
if (mode & Body){
body = dBodyCreate (odeHandle.world);
setMass(mass, mode & Density);
}
if (mode & Geom){
geom = dCreateCCylinder ( odeHandle.space , osgcapsule->getRadius(), osgcapsule->getHeight());
attachGeomAndSetColliderFlags();
}
if (mode & Draw){
osgcapsule->init(osgHandle);
}
QMP_END_CRITICAL(3);
}
CRigidCapsule::CRigidCapsule(S32 parent, CSceneObject *so, const CVector3 &d ) : CRigidBody(parent, so) {
if(parent == 0)parent = (S32)space;
#ifdef SIM
mGeomID = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID, 1);
F32 radius = sqrt(d.x * d.x + d.z * d.z) / 2.0;
//F32 radius = d.x / 2;
dGeomID mGeomSphere = dCreateSphere(0, TO_PHYSICS(radius));
dGeomTransformSetGeom (mGeomID, mGeomSphere);
dGeomSetPosition (mGeomSphere, 0, TO_PHYSICS(-d.y/2 + radius), 0);
mGeomID2 = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID2, 1);
dGeomID mGeomBox = dCreateBox(0, TO_PHYSICS(d.x), TO_PHYSICS(d.y - radius), TO_PHYSICS(d.z));
dGeomTransformSetGeom (mGeomID2, mGeomBox);
dGeomSetPosition (mGeomBox, 0, TO_PHYSICS(radius / 2), 0);
#else
F32 length = d.y - d.x * 2;
mGeomID = dCreateCCylinder((dSpaceID)parent, TO_PHYSICS(d.x), TO_PHYSICS(length));
#endif
mBodyID = dBodyCreate(world);
dGeomSetBody(mGeomID, mBodyID);
#ifdef SIM
dGeomSetBody(mGeomID2, mBodyID);
#endif
dGeomSetData(mGeomID, static_cast<void *>(this));
#ifdef SIM
dGeomSetData(mGeomID2, static_cast<void *>(this));
#endif
mDimentions = d;
mDimentions.y = length;
setDensity(0.0001);
dBodySetAutoDisableFlag(mBodyID, 0);
mGroundBox.setBounds(CVector3(-d.x, -d.y / 2 - 5.0, -d.z), CVector3(d.x, -d.y / 2 + 20.0, d.z));
}
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;
}
}
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
}
