void TrackedVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
//dMassAdjust(&this->vehicleMass, 2.40);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);
this->leftTrack->create();
this->rightTrack->create();
dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0);
dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0);
this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
dJointSetFixed(this->leftTrackJoint);
dJointSetFixed(this->rightTrackJoint);
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}
void AvatarGameObj::update_geom_offsets() {
// Update the orientation of our physical geom to match _uprightness
dMatrix3 grot;
dRFromAxisAndAngle(grot, 1, 0, 0, _uprightness*M_PI_2);
dGeomSetOffsetRotation(get_entity().get_geom("physical"), grot);
// Update the position of the sticky_attach geom to match _uprightness
dGeomSetOffsetPosition(get_entity().get_geom("sticky_attach"), 0, -sin(_uprightness*M_PI_2) + RUNNING_MAX_DELTA_Y_POS, 0);
}
void SBoxParts::set(dWorldID w, dSpaceID space)
{
Size &sz = m_cmpnt.size();
/*
const dReal hx = sz.x();
const dReal hy = sz.y();
const dReal hz = sz.z();
*/
dReal hx = sz.x();
dReal hy = sz.y();
dReal hz = sz.z();
// konao
DUMP(("[SBoxParts::set] ODE geom created (hx, hy, hz)=(%f, %f, %f) [%s:%d]\n", hx, hy, hz, __FILE__, __LINE__));
if (hz == 0) hz = 0.001;
if (hy == 0) hy = 0.001;
if (hx == 0) hx = 0.001;
dGeomID geom = dCreateBox(0, hx, hy, hz);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
// x-axis and z-axis is swapped between ODE/SIGVerse
dMassSetBox(&m, DENSITY, hz, hy, hx); //TODO: mass of cube should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
// Gap between ODE shape and body
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz);
// Initial orientation
dReal offq[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dGeomSetOffsetQuaternion(geom, offq);
//dMassAdjust(&m, 1.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void PhysicsObject::attachObject(boost::shared_ptr<PhysicsObject> po,
const v3& position,
const qv4& orientation)
{
po->mBodyOffset = position;
GeomMapT::const_iterator it = po->mGeometry.begin();
for (; it != po->mGeometry.end(); ++it)
{
// Calculate new relative position
dGeomID geom = it->second.geomId;
const dReal* offset = dGeomGetOffsetPosition(geom);
v3 newPos(offset);
newPos += position;
// Attach
dGeomSetBody(geom, mOdeBody);
dGeomSetOffsetPosition(geom, newPos.x, newPos.y, newPos.z);
dSpaceRemove(po->mSpaceId, geom);
dSpaceAdd(mSpaceId, geom);
}
// add the two masses
dMass otherMass;
dBodyGetMass(po->mOdeBody, &otherMass);
// dbglog << "OtherMass: " << otherMass.mass;
// dbglog << "OtherCenter: " << odeVectorOut(otherMass.c);
// dbglog << "OtherInertia: " << odeMatrixOut(otherMass.I);
dBodyGetMass(mOdeBody, &mOriginalMass);
// dbglog << "OwnMass: " << mOriginalMass.mass;
// dbglog << "OwnCenter: " << odeVectorOut(mOriginalMass.c);
// dbglog << "OwnInertia: " << odeMatrixOut(mOriginalMass.I);
dMassAdd(&mOriginalMass, &otherMass);
dMassTranslate(&mOriginalMass, -mOriginalMass.c[0], -mOriginalMass.c[1], -mOriginalMass.c[2]);
dBodySetMass(mOdeBody, &mOriginalMass);
// dbglog << "NewMass: " << mOriginalMass.mass;
// dbglog << "NewCenter: " << odeVectorOut(mOriginalMass.c);
// dbglog << "NewInertia: " << odeMatrixOut(mOriginalMass.I);
// Disable old body
dBodyDisable(po->mOdeBody);
notifyControlAboutChangeInMass();
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
void SSphereParts::set(dWorldID w, dSpaceID space)
{
double rad = m_cmpnt.radius();
// konao
DUMP(("[SSphereParts::set] ODE geom created (r=%f) [%s:%d]\n", rad, __FILE__, __LINE__));
dGeomID geom = dCreateSphere(0, rad);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
dMassSetSphere(&m, DENSITY, rad);
//dMassAdjust(&m, 1.0);
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3],
const float r[16])
{
dVector3 v;
dMatrix3 M;
dReal rad;
dReal len;
dMass add;
if (r) set_rotation(M, r);
if (dGeomGetClass(geom) != dPlaneClass)
{
dReal m = get_data(geom)->mass;
/* Create a new mass for the given geom. */
switch (dGeomGetClass(geom))
{
case dBoxClass:
dGeomBoxGetLengths(geom, v);
dMassSetBoxTotal(&add, m, v[0], v[1], v[2]);
break;
case dSphereClass:
rad = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&add, m, rad);
break;
case dCapsuleClass:
dGeomCapsuleGetParams(geom, &rad, &len);
dMassSetCapsuleTotal(&add, m, 3, rad, len);
break;
default:
dMassSetZero(&add);
break;
}
/* Transform the geom and mass to the given position and rotation. */
if(dGeomGetBody(geom))
{
if (p)
{
dGeomSetOffsetPosition(geom, p[0], p[1], p[2]);
dMassTranslate (&add, p[0], p[1], p[2]);
}
if (r)
{
dGeomSetOffsetRotation(geom, M);
dMassRotate (&add, M);
}
}
else
{
if (p) dGeomSetPosition(geom, p[0], p[1], p[2]);
if (r) dGeomSetRotation(geom, M);
}
/* Accumulate the new mass with the body's existing mass. */
dMassAdd(mass, &add);
}
}
int main (int argc, char **argv)
{
dInitODE2(0);
bool fixed = true;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dVector3 offset;
dSetZero (offset, 4);
// Default test case
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
//static int tata = 0;
if (1)
{
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
type = dJointTypeSlider;
if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
{
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
tc = 1;
if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
tc = 2;
if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
tc = 3;
if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
fixed = false;
}
}
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
dMatrix3 R;
// Create the Obstacle
geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
dGeomSetCollideBits (geom[OBS], catBits[ALL]);
//Rotation of 45deg around y
dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
dGeomSetRotation (geom[OBS], R);
dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
//Rotation of 90deg around y
// Will orient the Z axis along X
dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
// Create Body2 (Wiil be attached to the world)
body[BODY2] = dBodyCreate (world);
// Main axis of cylinder is along X=1
dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dMassAdjust (&m, Mass1);
geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dGeomSetBody (geom[BODY2], body[BODY2]);
dGeomSetOffsetRotation (geom[BODY2], R);
dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY2], &m);
// Create Body 1 (Slider on the prismatic axis)
body[BODY1] = dBodyCreate (world);
// Main axis of capsule is along X=1
dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
dMassAdjust (&m, Mass1);
geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
dGeomSetBody (geom[BODY1], body[BODY1]);
dGeomSetOffsetRotation (geom[BODY1], R);
dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
dMass mRect;
dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dMassAdd (&m, &mRect);
// TODO: translate m?
geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dGeomSetBody (geom[RECT], body[BODY1]);
dGeomSetOffsetPosition (geom[RECT],
(BODY1_LENGTH-RECT_SIDES[0]) /2.0,
0.0,
-RADIUS -RECT_SIDES[2]/2.0);
dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY1], &m);
setPositionBodies (tc);
if ( fixed )
{
// Attache external cylinder to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[BODY2]);
dJointSetFixed (fixed );
dWorldSetGravity (world,0,0,-0.8);
}
else
{
dWorldSetGravity (world,0,0,0);
}
// The static is here only to help debugging
switch (type)
{
case dJointTypeSlider :
{
dSliderJoint *sj = new dSliderJoint (world, 0);
sj->attach (body[BODY1], body[BODY2]);
sj->setAxis (1, 0, 0);
joint = sj;
}
break;
case dJointTypePiston : // fall through default
default:
{
dPistonJoint *pj = new dPistonJoint (world, 0);
pj->attach (body[BODY1], body[BODY2]);
pj->setAxis (1, 0, 0);
dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
joint = pj;
}
break;
};
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
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;
}
}
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 SCylinderParts::set(dWorldID w, dSpaceID space)
{
double radius = m_cmpnt.radius();
double length = m_cmpnt.length();
// konao
//LOG_MSG(("[SCylinderParts::set] ODE geom created (r, l)=(%f, %f) [%s:%d]\n", radius, length, __FILE__, __LINE__))
// TODO: Ideally, cylinder should be constructed here. However, collision detection
// between two cylinders could not be realized. So, Capsule is required
// by okamoto@tome on 2011-10-12
//dGeomID geom = dCreateCapsule(0, radius, length);
dGeomID geom = dCreateCylinder(0, radius, length);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
//dMassSetCapsule(&m, DENSITY, 1, radius, length);
dMassSetCylinder(&m, DENSITY, 1, radius, length); //TODO: mass of the cylinder should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body
// set the long axis as y axis
dReal offq[4] = {0.707, 0.707, 0.0, 0.0};
dReal offq2[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dQuaternion qua;
dQMultiply2(qua, offq2, offq);
dGeomSetOffsetQuaternion(geom, qua);
//dGeomSetOffsetQuaternion(geom, offq2);
//dReal tmpq[4] = {0.707, 0.0, 0.0, 0.707};
//dGeomSetQuaternion(geom, tmpq);
//dBodySetQuaternion(body, tmpq);
/*TODO: Consideration is required whether this procedure is needed
* Reflection of orientation of the cylinder
* dMatrix3 R;
* dRFromAxisAndAngle(dMatrix3 R, dReal rx, dReal ry, dReal rz, dReal angle)
* dRFromAxisAndAngle(R,x_axis,y_axis,z_axis,angleData);
* dBodySetRotation(body,R); // Request of actual rotation
*/
// Not used, deleted by inamura
// real part of the quaternion
// double q = cos(angleData/2.0);
// imaginary part of the quaternion
// double i,j,k;
// i = x_axis * sin(angleData/2.0);
// j = y_axis * sin(angleData/2.0);
// k = z_axis * sin(angleData/2.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
dBodyID KinematicNodeDummy::attachToODE(PhysicsEnvironment *environment, dSpaceID visualSpaceID, dSpaceID collisionSpaceID, arma::mat44 parentsCoordinateFrame)
{
// move coordinate frame to current node:
arma::mat44 coordinateFrame = parentsCoordinateFrame * forwardMatrix;
dWorldID worldID = environment->getWorldID();
if (nullptr == m_parent)
{
// the body must coincide with the center of mass of the equivalent mass of all dummy children and this nodes masses
arma::mat44 frame = arma::eye(4, 4);
dMass equivMass = this->getODEMass(frame);
for (KinematicNode *child : m_children)
{
arma::mat44 childFrame = frame * child->getForwardMatrix();
dMass childMass = child->getODEMassToAttachToParent(childFrame);
if (childMass.mass > 0.) {
dMassAdd(&equivMass, &childMass);
}
}
m_odeNodeBodyOffset = arma::eye(4, 4);
m_odeNodeBodyOffset(0, 3) = equivMass.c[0];
m_odeNodeBodyOffset(1, 3) = equivMass.c[1];
m_odeNodeBodyOffset(2, 3) = equivMass.c[2];
dMassTranslate(&equivMass, -m_odeNodeBodyOffset(0, 3), -m_odeNodeBodyOffset(1, 3), -m_odeNodeBodyOffset(2, 3));
// create a body
m_odeBody = dBodyCreate(worldID);
arma::mat44 globalSystemBodyOffset = coordinateFrame * m_odeNodeBodyOffset;
dMatrix3 bodyRotation;
dVector3 bodyPosition;
odeUtils::getRotationMatrixAsDMat(globalSystemBodyOffset, bodyRotation);
odeUtils::getPositionAsDVec(globalSystemBodyOffset, bodyPosition);
dBodySetPosition(m_odeBody, bodyPosition[0], bodyPosition[1], bodyPosition[2]);
dBodySetRotation(m_odeBody, bodyRotation);
if (equivMass.mass > 0.)
{
dBodySetMass(m_odeBody, &equivMass);
} else {
dBodyGetMass(m_odeBody, &equivMass);
equivMass.mass = std::numeric_limits<double>::min();
dBodySetMass(m_odeBody, &equivMass);
}
} else {
// setup the nodeBodyOffset based on the parent's odeBody
dBodyID body = this->getODEBody();
const dReal *bodyPosition = dBodyGetPosition(body);
const dReal *bodyRotation = dBodyGetRotation(body);
arma::mat44 bodyFrame = odeUtils::getMatFromDMatAndDVec(bodyRotation, bodyPosition);
arma::mat44 helperRotation = arma::eye(4, 4);
helperRotation.submat(0, 0, 2, 2) = coordinateFrame.submat(0, 0, 2, 2);
m_odeNodeBodyOffset = coordinateFrame.i() * bodyFrame;
}
// create a "box" to visualize the node
dGeomID geom = dCreateSphere(visualSpaceID, 0.005);
dMatrix3 bodyRotation;
dVector3 bodyPosition;
// the visuals are defined as "local" coordinates but here we need to incorporate the body offset
arma::mat44 visualsFrame = m_odeNodeBodyOffset.i();
odeUtils::getRotationMatrixAsDMat(visualsFrame, bodyRotation); // inverse of rotation part
odeUtils::getPositionAsDVec(visualsFrame, bodyPosition);
dGeomSetBody(geom, getODEBody());
dGeomSetOffsetRotation(geom, bodyRotation);
dGeomSetOffsetPosition(geom, bodyPosition[0], bodyPosition[1], bodyPosition[2]);
// finally add all the visuals
this->attatchODEVisuals(visualsFrame, getODEBody(), collisionSpaceID);
// recurse into everything attached to this node
for (KinematicNode *child : m_children)
{
dBodyID childBody = child->attachToODE(environment, visualSpaceID, collisionSpaceID, coordinateFrame);
UNUSED(childBody);
}
return m_odeBody;
}
void PhysicsGeom::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & BodyFieldMask)
{
if(getBody() != NULL)
{
dGeomSetBody(_GeomID, getBody()->getBodyID());
}
else
{
dGeomSetBody(_GeomID, 0);
}
}
if(whichField & PositionFieldMask)
{
dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
}
if(whichField & RotationFieldMask)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetRotation(_GeomID, rotation);
}
if(whichField & QuaternionFieldMask)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dGeomSetQuaternion(_GeomID,q);
}
if(whichField & OffsetPositionFieldMask &&
getBody() != NULL)
{
dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
}
if(whichField & OffsetRotationFieldMask &&
getBody() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getOffsetRotation()[0];
Vec4f v2 = getOffsetRotation()[1];
Vec4f v3 = getOffsetRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetOffsetRotation(_GeomID, rotation);
}
if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
{
dQuaternion q;
q[0]=getOffsetQuaternion().w();
q[1]=getOffsetQuaternion().x();
q[2]=getOffsetQuaternion().y();
q[3]=getOffsetQuaternion().z();
dGeomSetOffsetQuaternion(_GeomID,q);
}
if(whichField & CategoryBitsFieldMask)
{
dGeomSetCategoryBits(_GeomID, getCategoryBits());
}
if(whichField & CollideBitsFieldMask)
{
dGeomSetCollideBits(_GeomID, getCollideBits());
}
if(whichField & SpaceFieldMask)
{
dSpaceID CurSpace(dGeomGetSpace(_GeomID));
if(CurSpace != 0 &&
(getSpace() == NULL ||
CurSpace != getSpace()->getSpaceID()))
{
dSpaceRemove(CurSpace,_GeomID);
}
if(getSpace() != NULL)
{
dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
}
}
if(whichField & EnableFieldMask)
{
if(getEnable())
{
dGeomEnable(_GeomID);
}
else
{
dGeomDisable(_GeomID);
}
}
}