Space* ODESimulator::createSpace()
{
ODESpace * newSpace = new ODESpace();
// Add this new Space as a child of the Simulator's root Space.
dSpaceAdd(mRootSpaceID, (dGeomID) newSpace->internal_getSpaceID());
addSpace(newSpace);
return newSpace;
}
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();
}
//since this is just the base level construction, we'll just make a sphere (WHOO, EXCITING!!!!)
bool Construction::Construct( char* descriptionFile, DynamicsSolver* solver, Screen3D& Screen,MeshManager& MM, Position& Location, ICollisionHandler* ch )
{
//deconstruct any old stuff
Deconstruct();
//save the solver pointer
mySolver = solver;
CollisionHandler = ch;
//create the body list
ObjectList = new DynamicsObject[1];
this->nObjects = 1;
//Create the geom group
GeomGroup = dSimpleSpaceCreate (solver->GetSpaceID(false)); //dCreateGeomGroup (solver->GetSpaceID(false));
//Create the actual body ( a sphere! )
ObjectList[0].CreateBody( solver );
dBodySetPosition ( ObjectList[0].Body, Location.x, Location.y, Location.z);
dMassSetSphere ( &ObjectList[0].Mass, 1.0, 5.0 );
dMassAdjust (&ObjectList[0].Mass, 1.0 );
dBodySetMass( ObjectList[0].Body, &ObjectList[0].Mass);
ObjectList[0].Geom = dCreateSphere (0,5.0);
dGeomSetData( ObjectList[0].Geom, &ObjectList[0].SurfaceDesc );
dGeomSetBody (ObjectList[0].Geom,ObjectList[0].Body);
dSpaceAdd (GeomGroup,ObjectList[0].Geom);
ObjectList[0].HasGeom = true;
//set owner
for(int i=0; i<nObjects; i++)
{
ObjectList[i].SurfaceDesc.Owner = &ObjectList[i];
ObjectList[i].SurfaceDesc.ParentConstruction = this;
ObjectList[i].Owner = this;
//ObjectList[i].HasGeom = true;
}
LinearDisableEpsilon = .1;
AngularDisableEpsilon = .01f;
//create the mesh for drawing
D3DXCreateSphere( Screen.D3DDevice, 5.5f, 10, 10, &DrawMesh, NULL );
return true;
}
void ODESpace::setParentSpace(Space* parentSpace)
{
dSpaceID tempSpaceID =
((ODESpace*)parentSpace)->internal_getSpaceID();
// First remove this Space from its current parent Space, if one
// exists.
if (NULL != mParentSpaceID)
{
dSpaceRemove(mParentSpaceID, (dGeomID)mSpaceID);
}
// Now add this Space into the new Space.
dSpaceAdd(tempSpaceID, (dGeomID)mSpaceID);
mParentSpaceID = tempSpaceID;
}
int test_plane_point_depth()
{
int j;
dVector3 n,p,q,a,b; // n = plane normal
dReal d;
dSimpleSpace space(0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test point on plane has depth 0
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = 0;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
// ********** test arbitrary depth point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = dRandReal() - 0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
FAILED();
// ********** test depth-1 point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = -1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
PASSED();
}
dxGeom* dCreateTriList(dSpaceID space, dTriCallback* Callback, dTriArrayCallback* ArrayCallback){
if (dTriListClass == -1){
dGeomClass c;
c.bytes = sizeof(dxTriList);
c.collider = &dTriListColliderFn;
c.aabb = &dInfiniteAABB;
c.aabb_test = &dAABBTestTL;
// c.aabb_test=NULL;
c.dtor = &dDestroyTriList;
dTriListClass = dCreateGeomClass(&c);
}
dxGeom* g = dCreateGeom(dTriListClass);
if (space) dSpaceAdd(space, g);
dxTriList* Data = (dxTriList*)dGeomGetClassData(g);
Data->Callback = Callback;
Data->ArrayCallback = ArrayCallback;
Data->Collider = xr_new<dcTriListCollider>(g);
return g;
}
dxGeom *dCreateRayMotions (dSpaceID space)
{
if (dRayMotionsClassUser == -1)
{
dGeomClass c;
c.bytes = sizeof (dxRayMotions);
c.collider = &dRayMotionsColliderFn;
c.aabb = &dRayMotionsAABB;
c.aabb_test = 0;
c.dtor = &dGeomRayMotionDestroy;
dRayMotionsClassUser =dCreateGeomClass (&c);
}
dGeomID g = dCreateGeom (dRayMotionsClassUser);
if (space) dSpaceAdd (space,g);
dxRayMotions *c = (dxRayMotions*) dGeomGetClassData(g);
c->ray=dCreateRay(space,REAL(1.));
return g;
}
int test_sphere_point_depth()
{
int j;
dVector3 p,q;
dMatrix3 R;
dReal r,d;
dSimpleSpace space(0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test center point has depth r
if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
// ********** test point at random depth
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
PASSED();
}
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 Bola::construir(dWorldID world, dSpaceID space)
{
// Cria objeto e geometria
this->corpo = dBodyCreate(world);
this->geometria = dCreateSphere(0,BOLA_RAIO);
// Define a posição do objeto
dBodySetPosition(this->corpo, this->pegarX(), this->pegarY(), STARTZ);
// Define a massa do objeto
dMass m;
dMassSetSphere(&m,1,BOLA_RAIO);
dMassAdjust(&m,BOLA_MASSA);
dBodySetMass(this->corpo,&m);
// Associa o objeto à geometria
dGeomSetBody(this->geometria,this->corpo);
// Cria um espaço para a bola e a adiciona
this->espaco = dSimpleSpaceCreate(space);
dSpaceSetCleanup(this->espaco,0);
dSpaceAdd(this->espaco,this->geometria);
}
dGeomID Space::add(dGeomID g){
dSpaceAdd(space,g);
return g;
}
int test_ray_and_ccylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,ccyl);
// ********** make a random capped cylinder
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test ray completely within ccyl
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
dNormalize3 (b);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
for (j=0; j<3; j++) b[j] -= a[j];
dNormalize3 (b);
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just misses (between caps)
k = dRandReal()*2*M_PI;
x = sin(k);
y = cos(k);
for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (between caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test ray outside ccyl that just misses (caps)
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) < 0) {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
}
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test random rays
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
dNormalize3 (n);
dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ccylinder_point_depth()
{
int j;
dVector3 p,a;
dMatrix3 R;
dReal r,l,beta,x,y,d;
dSimpleSpace space(0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ccyl);
// ********** make a random ccyl
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test point on axis has depth of 'radius'
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
FAILED();
// ********** test point on surface (excluding caps) has depth 0
beta = dRandReal()*2*M_PI;
x = r*sin(beta);
y = r*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point on surface of caps has depth 0
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point inside ccyl has positive depth
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
// ********** test point depth (1)
d = (dRandReal()*2-1) * r;
beta = dRandReal()*2*M_PI;
x = (r-d)*sin(beta);
y = (r-d)*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
// ********** test point depth (2)
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
PASSED();
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// 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/textures";
if(argc==2)
fn.path_to_textures = argv[1];
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 64);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(int);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
// fprintf(stderr,"Building Single Precision Mesh\n");
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(int)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRSetIdentity(R);
//dIASSERT(dVALIDMAT3(R));
dGeomSetRotation (world_mesh, R);
float sx=0.0, sy=3.40, sz=6.80;
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, RADIUS);
dGeomSetBody (sphgeom,sphbody);
reset_ball();
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// Causes segm violation? Why?
// (because dWorldDestroy() destroys body connected to geom; must call first!)
dGeomDestroy(sphgeom);
dGeomDestroy (world_mesh);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
void PCylinder::init()
{
body = dBodyCreate (world);
initPosBody();
setMass(m_mass);
/* if (m_texid!=-1)
{
dTriMeshDataID g = dGeomTriMeshDataCreate();
const int vertexcount = 19*3+1;
const int indexcount = 19*9;
dVector3* vertices = new dVector3[vertexcount];
dTriIndex* indices = new dTriIndex[indexcount];
const int n = 24; // number of sides to the cylinder (divisible by 4)
double l = m_length*0.5;
double a = float(M_PI*2.0)/float(n);
double sa = (float) sin(a);
double ca = (float) cos(a);
double r = m_radius;
double ny=1,nz=0,tmp;
int v=0,f=0;
int i;
for (i=0; i<=n; i++) {
if (i>2 && i<n-2)
{
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = l;v++;
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = -l;v++;
indices[f] = v;f++;
indices[f] = v-2;f++;
indices[f] = v-1;f++;
indices[f] = v-1;f++;
indices[f] = v;f++;
indices[f] = v+1;f++;
}
}
//v=38;f=38*3
// rotate ny,nz
tmp = ca*ny - sa*nz;
nz = sa*ny + ca*nz;
ny = tmp;
ny=1; nz=0; // normal vector = (0,ny,nz)
glBegin (GL_TRIANGLE_FAN);
vertices[v][0] = 0;
vertices[v][1] = 0;
vertices[v][2] = l;v++;
for (i=0; i<n; i++) {
if (i>2 && i<n-2)
{
vertices[v][0] = ny*r;
vertices[v][1] = nz*r;
vertices[v][2] = l;v++;
indices[f] = 38;f++;
indices[f] = v;f++;
indices[f] = v-1;f++;
}
// rotate ny,nz
tmp = ca*ny - sa*nz;
nz = sa*ny + ca*nz;
ny = tmp;
}
// draw bottom cap
// ny=1; nz=0; // normal vector = (0,ny,nz)
// glBegin (GL_TRIANGLE_FAN);
// glNormal3d (0,0,-1);
// glVertex3d (0,0,-l+zoffset);
// for (i=0; i<=n; i++) {
// if (i>=2 && i<n-2)
// {
//
// glNormal3d (0,0,-1);
// glVertex3d (ny*r,nz*r,-l+zoffset);
// }
// // rotate ny,nz
// tmp = ca*ny + sa*nz;
// nz = -sa*ny + ca*nz;
// ny = tmp;
// }
dGeomTriMeshDataBuildSimple(g, (dReal*) vertices, vertexcount, indices, indexcount);
geom = dCreateTriMesh(0,g,NULL,NULL,NULL);
}
else */
{
geom = dCreateCylinder(0,m_radius,m_length);
}
dGeomSetBody (geom,body);
dSpaceAdd (space,geom);
}
void PhysicsSpace::AddSpace( dSpaceID s)
{
dSpaceAdd(_SpaceID, (dGeomID)s);
}
void CODEGeom::add_to_space(dSpaceID space)
{
if(m_geom_transform) dSpaceAdd(space,m_geom_transform);
}
int test_ray_and_plane()
{
int j;
dContactGeom contact;
dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
dMatrix3 R;
dReal d;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,ray);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test finite length ray below plane
dGeomRaySetLength (ray,0.09);
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = -dRandReal()*0.5 - 0.1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
dGeomSetPosition (ray,b[0],b[1],b[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray above plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()*0.5 + 0.01;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal() + 0.01;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray that intersects plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()-0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal()-0.5;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
// test that contact is on plane surface
if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED();
// also check contact point depth
if (dFabs (dGeomPlanePointDepth
(plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test ray that just misses
for (j=0; j<3; j++) b[j] = (1+d)*n[j];
for (j=0; j<3; j++) h[j] = -n[j];
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,0.99);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray that just hits
dGeomRaySetLength (ray,1.01);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test polarity with typical ground plane
dGeomPlaneSetParams (plane,0,0,1,0);
for (j=0; j<3; j++) a[j] = 0.1;
for (j=0; j<3; j++) b[j] = 0;
a[2] = 1;
b[2] = -1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
a[2] = -1;
b[2] = 1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
PASSED();
}
void PhysicsSpace::AddGeom( dGeomID g)
{
dSpaceAdd(_SpaceID, g);
}
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);
}
}
}
int test_ray_and_box()
{
int i,j;
dContactGeom contact;
dVector3 s,p,q,n,q2,q3,q4; // s = box sides
dMatrix3 R;
dReal k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test zero length ray just inside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
for (j=0; j<3; j++) q4[j] += p[j];
for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
dNormalize3 (n);
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
dNormalize3 (q2);
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
for (j=0; j<3; j++) q2[j] = -q2[j];
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,k*0.99);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,k*1.01);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test contact point position for random rays
for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
dNormalize3 (q3);
dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomBoxPointDepth
(box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check position of contact point
for (j=0; j<3; j++) contact.pos[j] -= p[j];
dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
FAILED();
}
// also check normal signs
if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int main (int argc, char **argv)
{
dMass m;
// 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;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void Buggy::embody(dWorldID world, dSpaceID space)
{
dMass m;
float LENGTH = 10;
float WIDTH = 5;
float HEIGHT = 5;
float RADIUS=4; // wheel radius
float STARTZ=1; // starting height of chassis
me = dBodyCreate(world);
dBodySetPosition (me,pos[0],pos[1],pos[2]);
dMassSetBox (&m,1,WIDTH,HEIGHT,LENGTH);
dMassAdjust (&m,CMASS);
dBodySetMass (me,&m);
box[0] = dCreateBox (0,WIDTH,HEIGHT,LENGTH);
dGeomSetBody(box[0], me);
// wheel bodies
for (int i=1; i<=4; i++) {
carbody[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,0);
dBodySetQuaternion (carbody[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (carbody[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],carbody[i]);
}
//
dBodySetPosition (carbody[1],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH);
dBodySetPosition (carbody[2],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH);
dBodySetPosition (carbody[3],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH);
dBodySetPosition (carbody[4],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH);
//
//
// // front and back wheel hinges
for (int i=0; i<4; i++) {
carjoint[i] = dJointCreateHinge2 (world,0);
dJointAttach (carjoint[i],me,carbody[i+1]);
const dReal *a = dBodyGetPosition (carbody[i+1]);
dJointSetHinge2Anchor (carjoint[i],a[0],a[1],a[2]);
dJointSetHinge2Axes (carjoint[i], zunit, yunit);
//dJointSetHinge2Axis1 (carjoint[i],0,1,0); // Axis 1 that comes from the structure
//dJointSetHinge2Axis2 (carjoint[i],0,0,1); // Axis 2 where the wheels spin
}
//
// // set joint suspension
for (int i=0; i<4; i++) {
dJointSetHinge2Param (carjoint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (carjoint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (int i=1; i<4; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (carjoint[i],dParamLoStop,0);
dJointSetHinge2Param (carjoint[i],dParamHiStop,0);
// // the following alternative method is no good as the wheels may get out
// // of alignment:
// // dJointSetHinge2Param (joint[i],dParamVel,0);
// // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
dSpaceAdd (car_space,sphere[3]);
}
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);
}
void wrl::atom::live(dSpaceID space) const
{
dSpaceAdd(space, edit_geom);
}
int main (int argc, char **argv)
{
int i;
dMass m;
// 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/textures";
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
ground = dCreatePlane (space,0,0,1,0);
// chassis body
body[0] = dBodyCreate (world);
dBodySetPosition (body[0],0,0,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[0],body[0]);
// wheel bodies
for (i=1; i<=3; i++) {
body[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front wheel hinge
/*
joint[0] = dJointCreateHinge2 (world,0);
dJointAttach (joint[0],body[0],body[1]);
const dReal *a = dBodyGetPosition (body[1]);
dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[0],0,0,1);
dJointSetHinge2Axis2 (joint[0],0,1,0);
*/
// front and back wheel hinges
for (i=0; i<3; i++) {
joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[i],0,0,1);
dJointSetHinge2Axis2 (joint[i],0,1,0);
// breakable joints contribution
// the wheels can break
dJointSetBreakable (joint[i], 1);
// the wheels wil break at a specific force
dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE);
// specify the force for the first body connected to the joint ...
dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5);
// and for the second body
dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5);
}
// set joint suspension
for (i=0; i<3; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (i=1; i<3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// dJointSetHinge2Param (joint[i],dParamVel,0);
// dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
// environment
ground_box = dCreateBox (space,2,1.5,1);
dMatrix3 R;
dRFromAxisAndAngle (R,0,1,0,-0.15);
dGeomSetPosition (ground_box,2,0,-0.34);
dGeomSetRotation (ground_box,R);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
return 0;
}
//since this is just the base level construction, we'll just make a sphere
bool AvatarConstruction::Construct( char* descriptionFile, DynamicsSolver* solver, Screen3D& Screen,MeshManager& MM, Position& Location, ICollisionHandler* aCollisionHandler )
{
//deconstruct any old stuff
Deconstruct();
//save the solver pointer
mySolver = solver;
this->CollisionHandler = aCollisionHandler;
//create the body list
ObjectList = new DynamicsObject[1];
this->nObjects = 1;
//Create the geom group
GeomGroup = dSimpleSpaceCreate (solver->GetSpaceID(false));
//set up surface properties
ObjectList[0].SurfaceDesc.ContactSlip1 = 0.01f;
ObjectList[0].SurfaceDesc.ContactSlip2 = 0.01f;
//set some other properties
for(int i=0; i<nObjects; i++)
{
ObjectList[i].SurfaceDesc.Owner = &ObjectList[i];
ObjectList[i].SurfaceDesc.ParentConstruction = this;
ObjectList[i].SurfaceDesc.SoftCFM = .01f;
ObjectList[i].SurfaceDesc.SoftERP = .1f;
ObjectList[i].SurfaceDesc.mu = 10.0f;
ObjectList[i].isAvatar = true;
ObjectList[i].Owner = this;
}
//Create the actual body ( a sphere! )
ObjectList[0].CreateBody( solver );
dBodySetPosition ( ObjectList[0].Body, Location.x, Location.y, Location.z);
dMassSetSphere ( &ObjectList[0].Mass, 1.0, 6.0 );
dMassAdjust (&ObjectList[0].Mass, 1.0 );
dBodySetMass( ObjectList[0].Body, &ObjectList[0].Mass);
ObjectList[0].Geom = dCreateSphere (0,6.0);
dGeomSetData( ObjectList[0].Geom, &ObjectList[0].SurfaceDesc );
dGeomSetBody (ObjectList[0].Geom,ObjectList[0].Body);
dSpaceAdd (GeomGroup,ObjectList[0].Geom);
ObjectList[0].HasGeom = true;
//create the angular motor
this->nJoints = 1;
JointList = new dJointID[nJoints];
JointList[0] = dJointCreateAMotor( solver->GetWorldID(), 0 );
dJointSetAMotorMode ( JointList[0], dAMotorUser );
dJointSetAMotorNumAxes( JointList[0], 3 );
dJointSetAMotorAxis(JointList[0], 0, 0, 1, 0, 0); //x axis
dJointSetAMotorAxis(JointList[0], 1, 0, 0, 1, 0); //y axis
dJointSetAMotorAxis(JointList[0], 2, 0, 0, 0, 1); //z axis
dJointAttach( JointList[0], ObjectList[0].Body, NULL); //attach to world
//set some properties
LinearDisableEpsilon = .2;
AngularDisableEpsilon = .01f;
JumpDelay = 1000;
LastJumpTime = 0;
ForceEnable = false;
//create the mesh for drawing
D3DXCreateSphere( Screen.D3DDevice, 6.0f, 20, 20, &DrawMesh, NULL );
return true;
}
int test_ray_and_sphere()
{
int j;
dContactGeom contact;
dVector3 p,q,q2,n,v1;
dMatrix3 R;
dReal r,k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test zero length ray just inside sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside that sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal();
for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
dMakeRandomVector (q2,3,1.0);
dNormalize3 (q2);
k = dRandReal();
for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
for (j=0; j<3; j++) n[j] = q2[j] - q[j];
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2));
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
do {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
}
while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) n[j] = -q[j];
for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,0.99*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,1.01*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED();
// ********** test contact point distance for random rays
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal()+0.5;
for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere));
if (dFabs(k - r) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
// also check depth of contact point
if (dFabs (dGeomSpherePointDepth
(sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test tangential grazing - miss
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test tangential grazing - hit
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
PASSED();
}
int main (int argc, char **argv)
{
dMass m;
// 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;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 32);
dCreatePlane (space,0,0,1, 0.0);
cylbody = dBodyCreate (world);
dQuaternion q;
#if 0
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#else
// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
#endif
dBodySetQuaternion (cylbody,q);
dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
dGeomSetBody (cylgeom,cylbody);
dBodySetPosition (cylbody, 0, 0, 3);
dSpaceAdd (space, cylgeom);
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,SPHERERADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, SPHERERADIUS);
dGeomSetBody (sphgeom,sphbody);
dBodySetPosition (sphbody, 0, 0, 5.5);
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dGeomDestroy(sphgeom);
dGeomDestroy (cylgeom);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// 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;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 12);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(dTriIndex);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(dTriIndex)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRFromAxisAndAngle (R, 0,1,0, 0.0);
dGeomSetRotation (world_mesh, R);
#ifdef BOX
boxbody = dBodyCreate (world);
dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
dMassAdjust (&m, 1);
dBodySetMass (boxbody,&m);
boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
dGeomSetBody (boxgeom,boxbody);
dSpaceAdd (space, boxgeom);
#endif
#ifdef CYL
cylbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
dGeomSetBody (cylgeom,cylbody);
#if defined(CYL_GEOM_OFFSET)
dMatrix3 mat;
dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0);
dGeomSetOffsetRotation(cylgeom,mat);
#endif
dSpaceAdd (space, cylgeom);
#endif
reset_state();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy geoms, then space, then the world.
#ifdef CYL
dGeomDestroy (cylgeom);
#endif
#ifdef BOX
dGeomDestroy (boxgeom);
#endif
dGeomDestroy (world_mesh);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
(void)world_normals; // get rid of compiler warnings
}
