/*** ゴールの生成 ***/
static void makeGoal()
{
// 0:right panel, 1:left panel, 2:back panel, 3;bar,
// 4: right wall paper, 5: left wall paper, 6: back wall paper
dReal pos[GOAL_PARTS_NUM][3] = {{-6.318, 1.08, 0.505},{-6.318, -1.08, 0.505},
{-6.59, 0, 0.505},{-6.0625, 0, 1.07},
{-6.25, 1.005, 0.505},{-6.25, -1.005, 0.505}, {-6.5055, 0, 0.505}
};
for (int i = 0; i < GOAL_PARTS_NUM; i++)
{
goal_parts[i] = dCreateBox(space, GOAL_SIDES[i][0],
GOAL_SIDES[i][1], GOAL_SIDES[i][2]);
dGeomSetPosition(goal_parts[i],pos[i][0], pos[i][1], pos[i][2]);
}
dReal pos2[GOAL_PARTS_NUM][3] = {{6.318, 1.08, 0.505},{6.318, -1.08, 0.505},
{6.59, 0, 0.505},{6.0625, 0, 1.07},
{6.25, 1.005, 0.505},{6.25, -1.005, 0.505}, {6.5055, 0, 0.505}
};
for (int i = 0; i < GOAL_PARTS_NUM; i++)
{
goal_parts2[i] = dCreateBox(space, GOAL_SIDES[i][0],
GOAL_SIDES[i][1], GOAL_SIDES[i][2]);
dGeomSetPosition(goal_parts2[i],pos2[i][0], pos2[i][1], pos2[i][2]+offset_z);
}
}
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;
}
void CManipulator::CreateManipulator(dWorldID world,dSpaceID space)
{
if(baseManipulator.paraBase.typeJoint=CYLINDER)
baseManipulator.geomBase=dCreateCylinder(space,baseManipulator.paraBase.paraManipulatorCylinder.r,baseManipulator.paraBase.paraManipulatorCylinder.length);
else
baseManipulator.geomBase=dCreateBox(space,baseManipulator.paraBase.paraManipulatorBox.x,baseManipulator.paraBase.paraManipulatorBox.y,baseManipulator.paraBase.paraManipulatorBox.z);
if(baseManipulator.paraBase.typeJoint=CYLINDER)
dGeomSetPosition(baseManipulator.geomBase,0,0,baseManipulator.paraBase.paraManipulatorCylinder.length/2.0);
else
dGeomSetPosition(baseManipulator.geomBase,0,0,baseManipulator.paraBase.paraManipulatorBox.z/2.0);
int i;
SPoint tempptAnchor;
dBodyID tempbodyLast=NULL;
tempptAnchor.x=tempptAnchor.y=0;
if(baseManipulator.paraBase.typeJoint=CYLINDER)
tempptAnchor.z=baseManipulator.paraBase.paraManipulatorCylinder.length;
else
tempptAnchor.z=baseManipulator.paraBase.paraManipulatorBox.z;
//创建第一轴
CreateJoint(tempptAnchor,jointManipulator[0].paraJoint,BORDER,tempbodyLast,
jointManipulator[0].bodyJoint,jointManipulator[0].geomJoint,jointManipulator[0].joint,world,space);
SetStop(jointManipulator[0].joint,stopparaJoint[0]);
//创建第二轴
if(jointManipulator[0].paraJoint.typeJoint==CYLINDER)
{
tempptAnchor.x=tempptAnchor.x;
tempptAnchor.z=tempptAnchor.z;
tempptAnchor.y=tempptAnchor.y+jointManipulator[0].paraJoint.paraManipulatorCylinder.r;
}
else
{
tempptAnchor.x=tempptAnchor.x;
tempptAnchor.z=tempptAnchor.z;
tempptAnchor.y=tempptAnchor.y+jointManipulator[0].paraJoint.paraManipulatorBox.y/2;
}
CreateJoint(tempptAnchor,jointManipulator[1].paraJoint,BORDER,tempbodyLast,
jointManipulator[1].bodyJoint,jointManipulator[1].geomJoint,jointManipulator[1].joint,world,space);
SetStop(jointManipulator[1].joint,stopparaJoint[1]);
//创建其他轴
for( i=2;i<NUMOFLEGJOINTS;i++)
{
CreateJoint(tempptAnchor,
jointManipulator[i].paraJoint,BORDER,
tempbodyLast,
jointManipulator[i].bodyJoint,
jointManipulator[i].geomJoint,
jointManipulator[i].joint,
world,space);
SetStop(jointManipulator[i].joint,stopparaJoint[i]);
}
return;
}
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 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 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 construirCirculoCentral()
{
// Círculo central (sendo desenhado como um cilindro)
// Para melhor visualização, desabilitar sombras (CTRL+S)
circle = dCreateCylinder(0,CIRCLE_RADIUS,0.01);
dGeomSetPosition(circle,0,0,0.005);
}
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 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 box::create_physical_body(
double x,
double y,
double z,
double size_x,
double size_y,
double size_z,
double mass,
manager& mgr)
{
world_id = mgr.ode_world();
space_id = mgr.ode_space();
//create and position the geom to represent the pysical shape of the rigid body
geom_id = dCreateBox (mgr.ode_space(), size_x, size_y, size_z);
object::set_geom_data(geom_id);
dGeomSetPosition (geom_id, x, y, z);
size[0] = size_x;
size[1] = size_y;
size[2] = size_z;
if(mass > 0)
{
object::create_rigid_body(x, y, z, mgr);
set_mass(mass);
dGeomSetBody (geom_id, body_id);
}
}
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 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 cylinder::create_physical_body(
double x,
double y,
double z,
double radius,
double length,
double mass,
manager& mgr)
{
this->radius = radius;
this->length = length;
world_id = mgr.ode_world();
space_id = mgr.ode_space();
//create and position the geom to represent the physical shape of the rigid body
geom_id = dCreateCylinder(mgr.ode_space(),radius, length);
object::set_geom_data(geom_id);
dGeomSetPosition (geom_id, x, y, z);
if(mass > 0)
{
object::create_rigid_body(x, y, z, mgr);
dGeomSetBody (geom_id, body_id);
set_mass(mass);
}
}
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);
}
}
ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace)
: m_odeWorld(odeWorld), m_odeSpace(odeSpace)
{
m_object = obj;
m_odeGeom = odeGeom;
m_odeBody = NULL;
m_odeBody = dBodyCreate(m_odeWorld);
assert(m_odeGeom!=NULL);
dBodySetPosition(m_odeBody, 0, 0, 0);
dGeomSetPosition(m_odeGeom, 0, 0, 0);
// note: owners must override this by setting the density. can't
// do it here because obj->m_pSpecial is not yet
// initialized.
dMassSetSphere(&m_odeMass, 1, 1);
dBodySetMass(m_odeBody, &m_odeMass);
dGeomSetBody(m_odeGeom, m_odeBody);
dGeomSetData(m_odeGeom, obj);
if (!obj) return;
obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this);
obj->m_position.setSetCallback(ODEObject::on_set_position, this);
obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this);
obj->m_accel.setSetCallback(ODEObject::on_set_accel, this);
obj->m_force.setSetCallback(ODEObject::on_set_force, this);
obj->addHandler("push", "ffffff", ODEObject::push_handler);
}
void Primitive::setPosition(const Pos& pos){
if(body){
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
}else if(geom){ // okay there is just a geom no body
dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
}
update(); // update the scenegraph stuff
}
//===========================================================================
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 collidable_object::set_position(double x, double y, double z)
{
if(body_id)
{
object::set_position(x, y, z);
return;
}
if(!geom_id) return;
dGeomSetPosition (geom_id,x, y, z);
}
void construirParedes()
{
// 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};
int i;
for(i = 0; i < 2; i++)
{
wall[i] = dCreateBox(space,FIELD_WIDTH,THICK,HEIGTH);
dGeomSetPosition(wall[i],0,n[i*2]*FRONT_Y,0);
}
for(i = 2; i < 6; i++)
{
wall[i] = dCreateBox(space,THICK,SIDE_LENGTH,HEIGTH);
dGeomSetPosition(wall[i],n[i-2]*SIDE_X,n[i*2-4]*SIDE_Y,0);
}
}
void CSphereGeom::set_position(const Fvector& ref_point)
{
inherited::set_position(ref_point);
dVector3 local_position={
m_sphere.P.x-ref_point.x,
m_sphere.P.y-ref_point.y,
m_sphere.P.z-ref_point.z
};
dGeomSetPosition(geom(),local_position[0],local_position[1],local_position[2]);
}
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 BaseWidget::setPosition(const QPoint *position)
{
dReal xPos = position->x();
dReal yPos = position->y();
// Update ODE's position
dGeomSetPosition(geometry, RELATIVE(xPos), RELATIVE(yPos), 0);
qDebug("Moved to %f, %f", xPos, yPos);
// Update Qt's position
move(*position);
}
//===========================================================================
void cODEGenericBody::createDynamicBoundingBox(bool a_staticObject)
{
// check if body image exists
if (m_imageModel == NULL) { return; }
// 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;
// computing bounding box of geometry representation
m_imageModel->computeBoundaryBox(true);
// get size and center of box
cVector3d center = m_imageModel->getBoundaryCenter();
// compute dimensions
cVector3d size = m_imageModel->getBoundaryMax() -
m_imageModel->getBoundaryMin();
// build box
m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, size.x, size.y , size.z);
// offset box
dGeomSetPosition (m_ode_geom, center.x, center.y, center.z);
if (!m_static)
{
// set inertia properties
dMassSetBox(&m_ode_mass, 1.0, size.x, size.y, size.z);
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 = size.x;
m_paramDynColModel1 = size.y;
m_paramDynColModel2 = size.z;
//m_posOffsetDynColModel;
//m_rotOffsetDynColModel;
}
void CollidableObject::Update(Ogre::Real timeSinceLastFrame)
{
//In the base case, the geom's orientation and position are synced with
//the referenced scene node
if(isInWorld)
{
Ogre::Quaternion curOrient = mSceneNode->getOrientation();
Ogre::Vector3 curPos = mSceneNode->getPosition() + (curOrient * geomOffset);
float dCurOrient[] = {curOrient.w, curOrient.x, curOrient.y, curOrient.z};
dGeomSetPosition(mGeom, curPos.x, curPos.y, curPos.z);
dGeomSetQuaternion(mGeom, dCurOrient);
}
}
//you should create your own world
void CreateWorld1(Simulator * sim)
{
const double zdim = 2.0;
const double xdim = 20;
const double ydim = 20;
AddBoundaries(sim, xdim, ydim, zdim, 0.2);
dGeomID geom;
dGeomID geom_gara1;
dGeomID geom_gara2;
dQuaternion q; //for rotations
// create a parking garage - 1 st wall
geom_gara1 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara1, -13 + 0.5*(30-xdim), -5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara1, q);
sim->AddGeometryAsObstacle(geom_gara1);
// creat a pakring garage - 2nd wall
geom_gara2 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara2, -13+0.5*(30-xdim), -7.5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara2, q);
sim->AddGeometryAsObstacle(geom_gara2);
geom = dCreateBox(sim->GetSpaceID(), 9.0, 0.2, zdim);
dGeomSetPosition(geom, 6, -4, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, -M_PI * 0.25);
dGeomSetQuaternion(geom, q);
sim->AddGeometryAsObstacle(geom);
geom = dCreateSphere(sim->GetSpaceID(), 0.5 * zdim);
dGeomSetPosition(geom, 0, -4, 0.5 * zdim);
sim->AddGeometryAsObstacle(geom);
}
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 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);
}
void Wheel::disposePhysics(Utils::Xml &x) {
dMatrix3 R;
dRFromAxisAndAngle(R, 0.0, 1.0, 0.0,
Ogre::Degree(x.mustOReal("rotation.y")).valueRadians());
dBodySetRotation(ph.body, R);
Utils::Xml w("../xml/car.xml", "car");
dGeomSetPosition(ph.geom,
w.mustOReal("global-position.x") + x.mustOReal("position.x"),
w.mustOReal("global-position.y") + x.mustOReal("position.y"),
w.mustOReal("global-position.z") + x.mustOReal("position.z")
);
}
