int main (int argc, char **argv)
{
// 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 = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// note: can't share tridata if intending to trimesh-trimesh collide
TriData1 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriData2 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
dGeomSetData(TriMesh1, TriData1);
dGeomSetData(TriMesh2, TriData2);
{dGeomSetPosition(TriMesh1, 0, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh1, Rotation);}
{dGeomSetPosition(TriMesh2, 1, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh2, Rotation);}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int test_dBoxTouchesBox()
{
int k,bt1,bt2;
dVector3 p1,p2,side1,side2;
dMatrix3 R1,R2;
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
draw_all_objects (space);
int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
bt1 = t1 || t2;
bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
if (bt1 != bt2) FAILED();
/*
// some more debugging info if necessary
if (bt1 && bt2) printf ("agree - boxes touch\n");
if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
"says no\n");
if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
"says yes\n");
*/
PASSED();
}
void command (int cmd)
{
// note: 0.0174532925 radians = 1 degree
dQuaternion q;
dMatrix3 m;
switch(cmd)
{
case 'w':
geom1pos[0]+=0.05;
break;
case 'a':
geom1pos[1]-=0.05;
break;
case 's':
geom1pos[0]-=0.05;
break;
case 'd':
geom1pos[1]+=0.05;
break;
case 'e':
geom1pos[2]-=0.05;
break;
case 'q':
geom1pos[2]+=0.05;
break;
case 'i':
dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'j':
dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'k':
dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'l':
dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'm':
(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
break;
case 'n':
(geoms!=convex)? geoms=convex:geoms=boxes;
break;
default:
dsPrint ("received command %d (`%c')\n",cmd,cmd);
}
#if 0
dGeomSetPosition (geoms[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom1quat, m);
dGeomSetRotation (geoms[1],m);
#endif
DumpInfo=true;
}
void Obstacle::on_addToScene()
{
node_ = SceneGraph::addModel(name_, model_);
size_t batchCnt = 0;
TriangleBatch const *triBatch = model_->batches(&batchCnt);
size_t boneCnt = 0;
Bone const *bone = model_->bones(&boneCnt);
size_t vSize = model_->vertexSize();
char const * vertex = (char const *)model_->vertices();
unsigned int const *index = (unsigned int const *)model_->indices();
for (size_t bi = 0; bi != batchCnt; ++bi)
{
dTriMeshDataID tmd = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(tmd, vertex, vSize, triBatch[bi].maxVertexIndex + 1,
index + triBatch[bi].firstTriangle * 3, triBatch[bi].numTriangles * 3, 12);
dGeomID geom = dCreateTriMesh(gStaticSpace, tmd, 0, 0, 0);
tmd_.push_back(tmd);
geom_.push_back(geom);
Matrix bx;
get_bone_transform(bone, triBatch[bi].bone, bx);
Vec3 p(bx.translation());
addTo(p, pos());
dGeomSetPosition(geom, p.x, p.y, p.z);
dGeomSetRotation(geom, bx.rows[0]);
}
}
void Compound::addGeometry(const Pose3<>& parentPose, Geometry& geometry, SimRobotCore2::CollisionCallback* callback)
{
// compute pose
Pose3<> geomPose = parentPose;
if(geometry.translation)
geomPose.translate(*geometry.translation);
if(geometry.rotation)
geomPose.rotate(*geometry.rotation);
// create geometry
dGeomID geom = geometry.createGeometry(Simulation::simulation->staticSpace);
if(geom)
{
dGeomSetData(geom, &geometry);
// set pose
dGeomSetPosition(geom, geomPose.translation.x, geomPose.translation.y, geomPose.translation.z);
dMatrix3 matrix3;
ODETools::convertMatrix(geomPose.rotation, matrix3);
dGeomSetRotation(geom, matrix3);
}
// handle nested geometries
for(std::list< ::PhysicalObject*>::const_iterator iter = geometry.physicalDrawings.begin(), end = geometry.physicalDrawings.end(); iter != end; ++iter)
{
Geometry* geometry = dynamic_cast<Geometry*>(*iter);
if(geometry)
addGeometry(geomPose, *geometry, callback);
}
}
//===========================================================================
void cODEGenericBody::setRotation(cMatrix3d &a_rotation)
{
// apply new rotation to ODE body
dMatrix3 R;
R[0] = a_rotation.m[0][0];
R[1] = a_rotation.m[0][1];
R[2] = a_rotation.m[0][2];
R[4] = a_rotation.m[1][0];
R[5] = a_rotation.m[1][1];
R[6] = a_rotation.m[1][2];
R[8] = a_rotation.m[2][0];
R[9] = a_rotation.m[2][1];
R[10] = a_rotation.m[2][2];
// check if body defined
if (m_ode_body != NULL)
{
// store new rotation matrix
m_localRot = a_rotation;
dBodySetRotation(m_ode_body, R);
}
else if (m_ode_geom != NULL)
{
// store new rotation matrix
m_localRot = a_rotation;
dGeomSetRotation(m_ode_geom, R);
}
}
void CProtoHapticDoc::UpdateDynamics()
{
for(int i= 0; i<m_shapeCount; i++) {
dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dGeomSetPosition (m_geoms[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
dGeomSetRotation (m_geoms[i], dBodyGetRotation(bodies[i]));
dBodySetPosition (bodies[i], m_shapes[i]->getLocationX(),
m_shapes[i]->getLocationY(),
m_shapes[i]->getLocationZ());
float *rotation= m_shapes[i]->getRotation();
const dReal rot[12]=
{ rotation[0], rotation[4], rotation[8], rotation[12],
rotation[1], rotation[5], rotation[9], rotation[13],
rotation[2], rotation[6], rotation[10], rotation[14] };
dBodySetRotation (bodies[i], rot);
dMass mass;
dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
m_shapes[i]->getSizeY(),
m_shapes[i]->getSizeZ());
dBodySetMass (bodies[i], &mass);
}
}
void ApproxDistanceSensor::DistanceSensor::updateValue()
{
pose = physicalObject->pose;
pose.conc(offset);
invertedPose = pose.invert();
Vector3<> boxPos = pose * Vector3<>(max * 0.5f, 0.f, 0.f);
dGeomSetPosition(geom, boxPos.x, boxPos.y, boxPos.z);
dMatrix3 matrix3;
ODETools::convertMatrix(pose.rotation, matrix3);
dGeomSetRotation(geom, matrix3);
closestGeom = 0;
closestSqrDistance = maxSqrDist;
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
if(closestGeom)
{
const dReal* pos = dGeomGetPosition(closestGeom);
Geometry* geometry = (Geometry*)dGeomGetData(closestGeom);
data.floatValue = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - pose.translation).abs() - geometry->innerRadius;
if(data.floatValue < min)
data.floatValue = min;
}
else
data.floatValue = max;
}
// Universal method for all specific ODE geom types, which add the
// geom to the collide space, using an ODE proxy geom to offset the
// geom by the provided transformation matrix. The geom will also
// be attached to the rigid body, if any is set.
void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID)
{
n_assert(GeomId);
n_assert(!IsAttached());
// set the geom's local Transform
const vector3& Pos = Transform.pos_component();
dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z);
dMatrix3 ODERotation;
CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
dGeomSetRotation(GeomId, ODERotation);
// if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body
// else directly set Transform and rotation
if (pRigidBody)
{
ODEGeomID = dCreateGeomTransform(0);
dGeomTransformSetCleanup(ODEGeomID, 1);
dGeomTransformSetGeom(ODEGeomID, GeomId);
dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID());
}
else ODEGeomID = GeomId;
dGeomSetCategoryBits(ODEGeomID, CatBits);
dGeomSetCollideBits(ODEGeomID, CollBits);
dGeomSetData(ODEGeomID, this);
AttachToSpace(SpaceID);
}
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
dMass totalMass;
dBodyGetMass( m_BodyID, &totalMass );
if ( m_GeomIDs.size() == 0 )
{
dMassSetZero( &totalMass );
}
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyToGeomTransform, R, P );
dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
dGeomID encapsulatedGeom = 0;
dMass currMass;
dMassSetZero( &currMass );
encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );
dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
dMassRotate( &currMass, R );
//dMassTranslate(&currMass,P[0],P[1],P[2]);
dMassAdd( &totalMass, &currMass );
dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
dGeomSetRotation( encapsulatedGeom, R );
dGeomTransformSetCleanup( geomTransform, 1 );
dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
dGeomSetBody( geomTransform, m_BodyID );
m_GeomIDs.push_back( geomTransform );
dBodySetMass( m_BodyID, &totalMass );
}
void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
{
math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);
rotation.LoadIdentity();
rotation.SetFromAngles(math::DegreesToRadians(rot));
const math::cMat4 m = rotation.GetMatrix();
dMatrix3 r;
r[0] = m[0]; r[1] = m[4]; r[2] = m[8]; r[3] = 0;
r[4] = m[1]; r[5] = m[5]; r[6] = m[9]; r[7] = 0;
r[8] = m[2]; r[9] = m[6]; r[10] = m[10]; r[11] = 0;
position = pos;
dGeomSetPosition(geom, position.x, position.y, position.z);
dGeomSetRotation(geom, r);
if (bBody) {
body = dBodyCreate(pWorld->GetWorld());
dBodySetPosition(body, position.x, position.y, position.z);
dBodySetRotation(body, r);
dBodySetAutoDisableFlag(body, 1);
dGeomSetBody(geom, body);
pWorld->AddPhysicsObject(shared_from_this());
}
}
//===========================================================================
void cODEGenericBody::createDynamicBox(const double a_lengthX,
const double a_lengthY,
const double a_lengthZ,
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 = dCreateBox(m_ODEWorld->m_ode_space, a_lengthX, a_lengthY, a_lengthZ);
// 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)
{
dMassSetBox(&m_ode_mass, 1.0, a_lengthX, a_lengthY, a_lengthZ);
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_BOX;
// store dynamic model parameters
m_paramDynColModel0 = a_lengthX;
m_paramDynColModel1 = a_lengthY;
m_paramDynColModel2 = a_lengthZ;
m_posOffsetDynColModel = a_offsetPos;
m_rotOffsetDynColModel = a_offsetRot;
}
void CODEGeom::set_static_ref_form(const Fmatrix& form)
{
dGeomSetPosition(geometry_transform(),form.c.x,form.c.y,form.c.z);
Fmatrix33 m33;
m33.set(form);
dMatrix3 R;
PHDynamicData::FMX33toDMX(m33,R);
dGeomSetRotation(geometry_transform(),R);
}
void dmCreateBox0(dmObject *obj, double p[3], double R[12], double m, double side[3], double color[3])
{
obj->body = NULL;
obj->side = side;
obj->color = color;
obj->p = p;
obj->R = R;
obj->geom = dCreateBox(space,obj->side[0], obj->side[1], obj->side[2]); // ボックスジオメトリの生成
dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定
dGeomSetRotation(obj->geom, obj->R);
}
void dmCreateSphere0(dmObject *obj, double p[2], double R[12], double m, double r, double color[3])
{
obj->body = NULL;
obj->m = m;
obj->r = r;
obj->R = R;
obj->p = p;
obj->color = color;
obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成
dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定
dGeomSetRotation(obj->geom, obj->R);
}
void Object::setRotation(float x,float y,float z)
{
dMatrix3 R,R0,R1,R2,R3;
dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(x));
dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(y));
dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(z));
dMultiply0 (R0,R1,R2,3,3,3);
dMultiply0 (R, R0,R3,3,3,3);
dGeomSetRotation(iGeom,R);
dBodySetRotation(iBody,R);
}
void dmCreateCapsule0(dmObject *obj, double p[3], double R[12], double m, double r, double l, double color[3])
{
obj->body = NULL;
obj->r = r;
obj->l = l;
obj->color = color;
obj->p = p;
obj->R = R;
obj->geom = dCreateCapsule(space,obj->r, obj->l); // 円柱ジオメトリの生成
dGeomSetPosition(obj->geom, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定
dGeomSetRotation(obj->geom, obj->R);
}
int CubeBasePiece::selectFace(int& numFaces, dSpaceID space, dGeomID ray, float cameraX, float cameraY, float cameraZ)
{
numFaces = 6;
const dReal* rotationMatrix = dGeomGetRotation(geom);
const dReal* currentPosition = dGeomGetPosition(geom);
dGeomID faces[6];
// create six faces
faces[0] = dCreateBox(space,.01,sides[1],sides[2]);
faces[1] = dCreateBox(space,.01,sides[1],sides[2]);
dGeomSetPosition(faces[0],currentPosition[0]-(sides[0]/2),currentPosition[1],currentPosition[2]);
dGeomSetPosition(faces[1],currentPosition[0]+(sides[0]/2),currentPosition[1],currentPosition[2]);
faces[2] = dCreateBox(space,sides[0],.01,sides[2]);
faces[3] = dCreateBox(space,sides[0],.01,sides[2]);
dGeomSetPosition(faces[2],currentPosition[0],currentPosition[1]+(sides[1]/2),currentPosition[2]);
dGeomSetPosition(faces[3],currentPosition[0],currentPosition[1]-(sides[1]/2),currentPosition[2]);
faces[4] = dCreateBox(space,sides[0],sides[1],.01);
faces[5] = dCreateBox(space,sides[0],sides[1],.01);
dGeomSetPosition(faces[4],currentPosition[0],currentPosition[1],currentPosition[2]+(sides[2]/2));
dGeomSetPosition(faces[5],currentPosition[0],currentPosition[1],currentPosition[2]-(sides[2]/2));
int selectedFace;
int selectedPointDistance = 1000;
dContactGeom contact[1];
// find collision point closest to camera
for(int i = 0 ; i < 6 ; i++)
{
// rotate geom before collision
dGeomSetRotation(faces[i],rotationMatrix);
if(dCollide(faces[i],ray,1,&contact[0],sizeof(dContact)) != 0)
{
float tempDistance = pow(contact[0].pos[0] - cameraX,2) + pow(contact[0].pos[1] - cameraY,2) + pow(contact[0].pos[2] - cameraZ,2);
if(tempDistance <= selectedPointDistance)
{
selectedPointDistance = tempDistance;
selectedFace = i;
}
}
dGeomDestroy(faces[i]); // delete the geoms
}
return selectedFace;
}
void CShape::SetTransform(const matrix44& Tfm)
{
Transform = Tfm;
// if not attached to rigid body, directly update pShape's transformation
if (!pRigidBody && ODEGeomID)
{
//!!!DUPLICATE CODE!
const vector3& Pos = Transform.pos_component();
dGeomSetPosition(ODEGeomID, Pos.x, Pos.y, Pos.z);
dMatrix3 ODERotation;
CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
dGeomSetRotation(ODEGeomID, ODERotation);
}
}
void Object::MakeGeom(dSpaceID space)
{
iGeom = dCreateBox(space,iSize.x,iSize.y,iSize.z);
dMatrix3 R,R0,R1,R2,R3;
dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(iRotate.x));
dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(iRotate.y));
dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(iRotate.z));
dMultiply0 (R0,R1,R2,3,3,3);
dMultiply0 (R, R0,R3,3,3,3);
dGeomSetRotation(iGeom,R);
dGeomSetPosition(iGeom, iPosition.x, iPosition.y, iPosition.z);
};
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 ComponentPhysicsGeom::setOrientation(const mat3 &m)
{
dMatrix3 r;
const vec3 x = m.getAxisX().getNormal();
const vec3 y = m.getAxisY().getNormal();
const vec3 z = m.getAxisZ().getNormal();
r[0] = x.x; r[1] = x.y; r[2] = x.z; r[3] = 0.0f;
r[4] = y.x; r[5] = y.y; r[6] = y.z; r[7] = 0.0f;
r[8] = z.x; r[9] = z.y; r[10]= z.z; r[11]= 0.0f;
dGeomSetRotation(geom, r);
getParentBlackBoard().relayMessage(MessageOrientationHasBeenSet(getOrientation()));
}
void StaticWorldObject::AddLeaf(ssgLeaf *leaf, sgVec3 initialpos)
{
// traverse the triangles
int cnt = leaf->getNumTriangles() ;
int nv = leaf->getNumVertices() ;
// int nn = leaf->getNumNormals() ;
float *vertices = new float[3*nv];
int *indices = new int[3*cnt];
int i;
for (i=0; i<nv; i++)
{
float *v = leaf->getVertex( i ) ;
assert(v);
memcpy(vertices+3*i, v, 3*sizeof(float));
}
for (i=0; i<cnt; i++)
{
short idx0, idx1, idx2 ;
leaf->getTriangle( i, &idx0, &idx1, &idx2 ) ;
indices[3*i+0]=idx0;
indices[3*i+1]=idx1;
indices[3*i+2]=idx2;
}
dTriMeshDataID data = dGeomTriMeshDataCreate();
dataids.push_back(data);
dGeomTriMeshDataBuildSingle
(
data,
vertices,
3*sizeof(float),
nv,
indices,
cnt*3,
3*sizeof(int)
);
//fprintf(stderr,"Adding trimesh with %d verts, %d indices\n", nv, cnt*3);
dGeomID trimesh = dCreateTriMesh(space, data, 0,0,0);
geomids.push_back(trimesh);
dGeomSetPosition(trimesh, initialpos[0], initialpos[1], initialpos[2]);
dMatrix3 R;
dRFromAxisAndAngle (R, 0,1,0, 0.0);
dGeomSetRotation (trimesh, R);
dGeomSetData(trimesh, this);
}
void construirQuinas()
{
// Walls
// 0 front 1 back
// 2 right top 3 right back
// 4 left top 5 left back
int n[7] = {1, 1, -1, -1, 1, 1, -1};
dMatrix3 R;
for(int i=0; i < 4; i++)
{
triangle[i] = dCreateBox(space,TRIANGLE_THICK,TRIANGLE_LENGTH,HEIGHT);
dGeomSetPosition(triangle[i],n[i]*TRIANGLE_X,n[i+1]*TRIANGLE_Y,0);
dRFromAxisAndAngle (R,0,0,1,n[i*2]*M_PI_4);
dGeomSetRotation (triangle[i], R);
}
}
void CollisionMesh::Finalize()
{
//create mesh data structure
meshData = dGeomTriMeshDataCreate();
//create indices
int indexlist = new int[TriangleList.size()*3];
for( int i = 0; i< TriangleList.size()*3; i++)
{
indexlist[i] = i;
}
//copy over vertex data into buffer
dReal* vertices = new dReal[TriangleList.size()*9];
int vi=0;
for ( int i=0; i<TriangleList.size(); i++ )
{
vertices[vi+0] = TriangleList[i].v1.x;
vertices[vi+1] = TriangleList[i].v1.y;
vertices[vi+2] = TriangleList[i].v1.z;
vertices[vi+3] = TriangleList[i].v2.x;
vertices[vi+4] = TriangleList[i].v2.y;
vertices[vi+5] = TriangleList[i].v2.z;
vertices[vi+6] = TriangleList[i].v3.x;
vertices[vi+7] = TriangleList[i].v3.y;
vertices[vi+8] = TriangleList[i].v3.z;
vi += 9;
}
dGeomTriMeshDataBuildSingle(meshData, vertices, sizeof(dReal)*3,
TriangleList.size()*3,
(const int*)indexlist,
TriangleList.size()*3, 3*sizeof( int ));
Geom = dCreateTriMesh(solver->GetSpaceID(true, Location.x, Location.y, Location.z), meshData, 0, 0, 0);
dGeomSetData( Geom, &SurfaceDesc );
dGeomSetPosition( Geom, Location.x, Location.y, Location.z );
dMatrix3 R;
dRFromEulerAngles (R, pitch, -yaw, roll);
dGeomSetRotation( Geom, R );
Initialized = true;
}
void ODEObject::on_set_rotation(void *me, OscMatrix3 &r)
{
// Convert from a CHAI rotation matrix to an ODE rotation matrix
dMatrix3 m;
m[ 0] = r.getCol0().x;
m[ 1] = r.getCol0().y;
m[ 2] = r.getCol0().z;
m[ 3] = 0;
m[ 4] = r.getCol1().x;
m[ 5] = r.getCol1().y;
m[ 6] = r.getCol1().z;
m[ 7] = 0;
m[ 8] = r.getCol2().x;
m[ 9] = r.getCol2().y;
m[10] = r.getCol2().z;
m[11] = 0;
dGeomSetRotation(((ODEObject*)me)->m_odeGeom, m);
}
void CBoxGeom::set_position(const Fvector& ref_point)
{
inherited::set_position(ref_point);
dVector3 local_position={m_box.m_translate.x-ref_point.x,
m_box.m_translate.y-ref_point.y,
m_box.m_translate.z-ref_point.z
};
dGeomSetPosition(geom(),
local_position[0],
local_position[1],
local_position[2]
);
dMatrix3 R;
PHDynamicData::FMX33toDMX(m_box.m_rotate,R);
dGeomSetRotation(geom(),R);
}
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 HarrierSim::makeHarrier()
{
dMass mass;
itsHarrierBody = dBodyCreate(world);
//pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;
dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);
dMatrix3 R;
dRFromAxisAndAngle (R,1,0,0,0);
dBodySetRotation(itsHarrierBody, R);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
//dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
dMassRotate(&mass, R);
dBodySetMass(itsHarrierBody,&mass);
itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
dGeomSetRotation(itsHarrierGeom, R);
dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
void CCylinderGeom::set_position(const Fvector& ref_point)
{
inherited::set_position(ref_point);
dVector3 local_position={
m_cylinder.m_center.x-ref_point.x,
m_cylinder.m_center.y-ref_point.y,
m_cylinder.m_center.z-ref_point.z
};
dGeomSetPosition(
geom(),
local_position[0],
local_position[1],
local_position[2]
);
dMatrix3 R;
Fmatrix33 m33;
m33.j.set(m_cylinder.m_direction);
Fvector::generate_orthonormal_basis(m33.j,m33.k,m33.i);
PHDynamicData::FMX33toDMX(m33,R);
dGeomSetRotation(geom(),R);
}
