BarrelGeom2(Barrel *b) :
OdeGeom(dCreateBox(gDynamicSpace, b->radius_ * 1.4f, b->radius_ * 1.4f, b->height_)),
b_(b)
{
dGeomSetBody(id_, b->body_->id_);
dGeomSetData(id_, this);
}
BarrelGeom(Barrel *b) :
OdeGeom(dCreateCylinder(gDynamicSpace, b->radius_, b->height_)),
b_(b)
{
dGeomSetBody(id_, b->body_->id_);
dGeomSetData(id_, this);
}
WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
dBodyCreate(world);
body = dBodyCreate(world);
radius = .35;
thickness = .25;
activationDirection = 0;
geom = dCreateCylinder(space, radius, thickness);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setCylinder(.5, 1, radius, thickness);
dBodySetMass(body, &mass);
const dMatrix3 rotationMatrix = {1,0,0,0,
0,1,0,0,
0,0,1,0
};
dBodySetRotation(body, rotationMatrix);
dBodySetPosition(body,x,y,z);
attachmentOffset = thickness + .25;
color[0] = .5;
color[1] = 1;
color[2] = 1;
}
// 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);
}
IoODEPlane *IoODEPlane_new(void *state, dGeomID geomId)
{
IoODEPlane *proto = IoState_protoWithInitFunction_(state, IoODEPlane_proto);
IoODEPlane *self = IOCLONE(proto);
GEOMID = geomId;
dGeomSetData(GEOMID, self);
return self;
}
IoODEBox *IoODEBox_new(void *state, dGeomID geomId)
{
IoODEBox *proto = IoState_protoWithId_(state, protoId);
IoODEBox *self = IOCLONE(proto);
GEOMID = geomId;
dGeomSetData(GEOMID, self);
return self;
}
void ODESimulator::AddObject(const RigidObject& object)
{
objects.push_back(new ODERigidObject(object));
objects.back()->Create(worldID,envSpaceID);
dGeomSetData(objects.back()->geom(),(void*)(objects.size()-1));
dGeomSetCategoryBits(objects.back()->geom(),0x2);
dGeomSetCollideBits(objects.back()->geom(),0xffffffff);
}
dGeomID wrl::atom::init_edit_geom(dSpaceID space)
{
if ((edit_geom = new_edit_geom(space)))
{
dGeomSetData (edit_geom, this);
bGeomSetTransform(edit_geom, current_M);
}
return edit_geom;
}
CRigidBox::CRigidBox(S32 parent, CSceneObject *so, const CVector3 &d ) : CRigidBody(parent, so) {
if(parent == 0) parent = (S32)space;
mGeomID = dCreateBox((dSpaceID)parent, TO_PHYSICS(d.x), TO_PHYSICS(d.y), TO_PHYSICS(d.z));
mBodyID = dBodyCreate(world);
dGeomSetBody(mGeomID, mBodyID);
dGeomSetData(mGeomID, static_cast<void *>(this));
mDimentions = d;
setDensity(0.0001);
}
CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
CBoxEntity& c_box) :
CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
m_cBoxEntity(c_box),
m_sGeomData(GEOM_NORMAL) {
/* Check whether the box is movable or not */
if(c_box.GetEmbodiedEntity().IsMovable()) {
/* Movable box */
/* Set the body to its initial position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Create the geometry and the mass */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Set Geom gripping properties. */
m_sGeomData.Type = GEOM_GRIPPABLE;
dGeomSetData(m_tGeom, &m_sGeomData);
/* Create its mass */
dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Associate the body to the geom */
dGeomSetBody(m_tGeom, m_tBody);
/* Set the parent body total mass */
dBodySetMass(m_tBody, &m_tMass);
}
else {
/* Unmovable box, get rid of the body and add only the geometry */
dBodyDestroy(m_tBody);
/* Create the geometry */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
dGeomSetData(m_tGeom, &m_sGeomData);
/* Set the geom to its position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dGeomSetQuaternion(m_tGeom, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Associate the geom to null body (this makes it static) */
dGeomSetBody(m_tGeom, 0);
}
}
Card()
{
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(1, sides[0], sides[1], sides[2]);
dBodySetMass(body, &mass);
}
void CollidableObject::SetCollideShapeBox(dReal lx, dReal ly, dReal lz)
{
//Register a Box geom with ODE and link it to the collidable object
if(isInWorld)
dGeomDestroy(mGeom);
mGeom = PhysWorld::GetSingletonPtr()->AddBox(lx, ly, lz);
dGeomSetData(mGeom, (void*)this);
isInWorld = true;
Update(0.0f); //Ensure the geom's position and orientation match the attached scene node
}
void CollidableObject::SetCollideShapeCapsule(dReal radius, dReal length)
{
//Register a Capsule geom with ODE and link it to the collidable object
if(isInWorld)
dGeomDestroy(mGeom);
mGeom = PhysWorld::GetSingletonPtr()->AddCapsule(radius, length);
dGeomSetData(mGeom, (void*)this);
isInWorld = true;
Update(0.0f); //Ensure the geom's position and orientation match the attached scene node
}
void ComponentPhysicsGeom::createGeom( const string &type )
{
if(type == "Box") createGeomBox();
else if(type == "Cylinder") createGeomCylinder();
else if(type == "Capsule") createGeomCapsule();
else if(type == "Sphere") createGeomSphere();
else FAIL("Unknown geom type: " + type);
dGeomSetData(geom, getActorPtr());
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
CollisionShape::CollisionShape(dGeomID _geometryId)
: m_geometryId(_geometryId)
, m_pModule(Zen::Engine::Physics::I_PhysicsManager::getSingleton().getDefaultScriptModule())
, m_pScriptObject(NULL)
{
if( m_geometryId != NULL )
{
dGeomSetData(m_geometryId, this);
}
}
PhyCylinder::PhyCylinder(eType type)
: PhyEntity(type)
{
mGeomId = dCreateCylinder(g_OdeSpace, 1, 1);
if (mBodyId != NULL)
dGeomSetBody(mOdeGeom, mOdeBody);
dGeomSetData(mOdeGeom, this);
}
PhySphere::PhySphere(eType type)
: PhyEntity(type)
{
mGeomId = dCreateSphere(g_OdeSpace, 1);
if (mBodyId != NULL)
dGeomSetBody(mOdeGeom, mOdeBody);
dGeomSetData(mOdeGeom, this);
}
void CODEGeom::init()
{
dGeomID geom=create();
m_geom_transform=dCreateGeomTransform(0);
dGeomTransformSetCleanup(m_geom_transform,0);
dGeomSetData(m_geom_transform,0);
dGeomTransformSetGeom(m_geom_transform,geom);
dGeomTransformSetInfo(m_geom_transform,1);
dGeomCreateUserData(geom);
dGeomUserDataSetBoneId(geom,m_bone_id);
}
void object::set_geom_data(dGeomID geom)
{
if(geom)
{
//for the materials to work correctly when two objects collide we must
//store a pointer to the class itself in the geom. This alows us to cast back
//in the collision near callback function used by ode_manager and get the
//bouncyness, friction, etc, of the object.
dGeomSetData(geom,reinterpret_cast<void*>(this));
}
}
void PWorld::addObject(PObject* o)
{
int id = objects.count();
o->id = id;
if (o->world==NULL) o->world = world;
if (o->space==NULL) o->space = space;
o->g = g;
o->init();
dGeomSetData(o->geom,(void*)(&(o->id)));
objects.append(o);
}
CRigidCapsule::CRigidCapsule(S32 parent, CSceneObject *so, const CVector3 &d ) : CRigidBody(parent, so) {
if(parent == 0)parent = (S32)space;
#ifdef SIM
mGeomID = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID, 1);
F32 radius = sqrt(d.x * d.x + d.z * d.z) / 2.0;
//F32 radius = d.x / 2;
dGeomID mGeomSphere = dCreateSphere(0, TO_PHYSICS(radius));
dGeomTransformSetGeom (mGeomID, mGeomSphere);
dGeomSetPosition (mGeomSphere, 0, TO_PHYSICS(-d.y/2 + radius), 0);
mGeomID2 = dCreateGeomTransform ((dSpaceID)parent);
dGeomTransformSetCleanup (mGeomID2, 1);
dGeomID mGeomBox = dCreateBox(0, TO_PHYSICS(d.x), TO_PHYSICS(d.y - radius), TO_PHYSICS(d.z));
dGeomTransformSetGeom (mGeomID2, mGeomBox);
dGeomSetPosition (mGeomBox, 0, TO_PHYSICS(radius / 2), 0);
#else
F32 length = d.y - d.x * 2;
mGeomID = dCreateCCylinder((dSpaceID)parent, TO_PHYSICS(d.x), TO_PHYSICS(length));
#endif
mBodyID = dBodyCreate(world);
dGeomSetBody(mGeomID, mBodyID);
#ifdef SIM
dGeomSetBody(mGeomID2, mBodyID);
#endif
dGeomSetData(mGeomID, static_cast<void *>(this));
#ifdef SIM
dGeomSetData(mGeomID2, static_cast<void *>(this));
#endif
mDimentions = d;
mDimentions.y = length;
setDensity(0.0001);
dBodySetAutoDisableFlag(mBodyID, 0);
mGroundBox.setBounds(CVector3(-d.x, -d.y / 2 - 5.0, -d.z), CVector3(d.x, -d.y / 2 + 20.0, d.z));
}
static void init_test()
{
int i,j;
const dReal scale = 0.5;
// set random boxes
dRandSetSeed (seed);
for (i=0; i < NUM; i++) {
bounds[i][0] = dRandReal()*2-1;
bounds[i][1] = bounds[i][0] + dRandReal()*scale;
bounds[i][2] = dRandReal()*2-1;
bounds[i][3] = bounds[i][2] + dRandReal()*scale;
bounds[i][4] = dRandReal()*2;
bounds[i][5] = bounds[i][4] + dRandReal()*scale;
if (geom[i]) dGeomDestroy (geom[i]);
geom[i] = dCreateBox (space,
bounds[i][1] - bounds[i][0],
bounds[i][3] - bounds[i][2],
bounds[i][5] - bounds[i][4]);
dGeomSetPosition (geom[i],
(bounds[i][0] + bounds[i][1])*0.5,
(bounds[i][2] + bounds[i][3])*0.5,
(bounds[i][4] + bounds[i][5])*0.5);
dGeomSetData (geom[i],(void*)(size_t)(i));
}
// compute all intersections and put the results in "good_matrix"
for (i=0; i < NUM; i++) {
for (j=0; j < NUM; j++) good_matrix[i][j] = 0;
}
for (i=0; i < NUM; i++) hits[i] = 0;
for (i=0; i < NUM; i++) {
for (j=i+1; j < NUM; j++) {
dReal *bounds1 = &bounds[i][0];
dReal *bounds2 = &bounds[j][0];
if (bounds1[0] > bounds2[1] ||
bounds1[1] < bounds2[0] ||
bounds1[2] > bounds2[3] ||
bounds1[3] < bounds2[2] ||
bounds1[4] > bounds2[5] ||
bounds1[5] < bounds2[4]) continue;
good_matrix[i][j] = 1;
good_matrix[j][i] = 1;
hits[i]++;
hits[j]++;
}
}
}
void Solid::createGeometry(const dSpaceID& i_space)
{
/// need to get the correct geometry from volume.
std::vector< ::Ogre::Vector3> vertices ;
std::vector<unsigned long> indices ;
Ogre::Vector3 scale(1,1,1) ;
m_vertices = NULL ;
m_indices = NULL ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#0") ;
getObject()
->getTrait<Model::Solid>()
->getMesh().getMeshInformation(vertices,indices,scale) ;
if (vertices.size()>0 && indices.size() > 0)
{
m_vertices = new dVector3[vertices.size()];
m_indices = new dTriIndex[indices.size()];
for(unsigned int vertex_index = 0 ;
vertex_index < vertices.size() ;
++vertex_index)
{
m_vertices[vertex_index][0] = (dReal)(vertices[vertex_index].x) ;
m_vertices[vertex_index][1] = (dReal)(vertices[vertex_index].y) ;
m_vertices[vertex_index][2] = (dReal)(vertices[vertex_index].z) ;
m_vertices[vertex_index][3] = 0 ;
}
for(unsigned int index = 0 ;
index < indices.size() ;
++index)
{
m_indices[index] = (int)indices[index] ;
}
m_data = dGeomTriMeshDataCreate() ;
dGeomTriMeshDataBuildSimple(m_data,
(dReal*)m_vertices,(int)vertices.size(),
m_indices,(int)indices.size()) ;
m_geometry1 = dCreateTriMesh(i_space,m_data,0,0,0);
dGeomSetData(m_geometry1,m_data) ;
dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Solid) ;
createApproximatedGeometry(i_space) ;
InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#5") ;
}
}
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
CollisionShape::setGeometryId(dGeomID _geometryId)
{
if( m_geometryId != NULL )
{
dGeomDestroy( m_geometryId );
}
m_geometryId = _geometryId;
if( m_geometryId != NULL )
{
dGeomSetData(m_geometryId, this);
}
}
void ODESimulator::AddEnvironment(const Environment& env)
{
envs.push_back(&env);
envMeshes.resize(envMeshes.size()+1);
envMeshes.back() = new ODETriMesh;
envMeshes.back()->Create(env.mesh,envSpaceID);
envMeshes.back()->surf() = settings.defaultEnvSurface;
envMeshes.back()->SetPadding(settings.defaultEnvPadding);
if(!env.kFriction.empty())
envMeshes.back()->surf().kFriction = env.kFriction[0];
//the index of the environment is encoded as -1-index
dGeomSetData(envMeshes.back()->geom(),(void*)(-(int)envs.size()));
dGeomSetCategoryBits(envMeshes.back()->geom(),0x1);
dGeomSetCollideBits(envMeshes.back()->geom(),0xffffffff ^ 0x1);
}
//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 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);
}
dGeomID set_phys_geom_type(dGeomID geom, dBodyID body,
int i, int t, const float *v)
{
/* Destroy the old geom and its data. */
if (geom)
{
free(dGeomGetData(geom));
dGeomDestroy(geom);
geom = 0;
}
/* Create a new geom of the required type. */
switch (t)
{
case dSphereClass:
geom = dCreateSphere(space, v[0]);
break;
case dCapsuleClass:
geom = dCreateCapsule(space, v[0], v[1]);
break;
case dBoxClass:
geom = dCreateBox(space, v[0], v[1], v[2]);
break;
case dPlaneClass:
geom = dCreatePlane(space, v[0], v[1], v[2], v[3]);
break;
case dRayClass:
geom = dCreateRay(space, (dReal) sqrt(v[3] * v[3] +
v[4] * v[4] +
v[5] * v[5]));
dGeomRaySet(geom, v[0], v[1], v[2], v[3], v[4], v[5]);
break;
}
/* Assign geom data and attach it to the body. */
if (geom)
{
dGeomSetData(geom, create_data(i));
dGeomSetBody(geom, body);
}
return geom;
}
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;
}
