/*** ゴールの生成 ***/
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 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 RigidBodyEnvironment::createWorld(void)
{
// BEGIN SETTING UP AN OPENDE ENVIRONMENT
// ***********************************
bodyWorld = dWorldCreate();
space = dHashSpaceCreate(0);
dWorldSetGravity(bodyWorld, 0, 0, -0.981);
double lx = 0.2;
double ly = 0.2;
double lz = 0.1;
dMassSetBox(&m, 1, lx, ly, lz);
boxGeom = dCreateBox(space, lx, ly, lz);
boxBody = dBodyCreate(bodyWorld);
dBodySetMass(boxBody, &m);
dGeomSetBody(boxGeom, boxBody);
// *********************************
// END SETTING UP AN OPENDE ENVIRONMENT
setPlanningParameters();
}
void TrackedVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
//dMassAdjust(&this->vehicleMass, 2.40);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);
this->leftTrack->create();
this->rightTrack->create();
dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0);
dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0);
this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
dJointSetFixed(this->leftTrackJoint);
dJointSetFixed(this->rightTrackJoint);
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}
void createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
void Balaenidae::embody(dWorldID world, dSpaceID space)
{
me = dBodyCreate(world);
embody(me);
geom = dCreateBox( space, 100.0f, 40, 500.0f); // scale 50
dGeomSetBody(geom, me);
}
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);
}
void TSRODERigidBody::AddBoxGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, const TSRVector3& _vExtents, 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 = dCreateBox( 0, _vExtents.x, _vExtents.y, _vExtents.z );
dMassSetBox( &currMass, _fDensity, _vExtents.x,_vExtents.y,_vExtents.z);
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 Robots::construirChassi(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->body[i] = dBodyCreate(world);
this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);
// Define a posição do objeto
dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);
// Se o robô for do segundo time, deve ser rotacionado em 180 graus
if ((this->id == 3) || (this->id == 4) || (this->id == 5))
{
dQuaternion q;
dQFromAxisAndAngle(q,0,0,1,M_PI);
dBodySetQuaternion(this->body[i],q);
}
// Define a massa do objeto
dMass m;
dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT); // O segundo bloco é mais curto
dMassAdjust(&m,CMASS*(1+i*2)); // O segundo bloco é mais pesado
dBodySetMass(this->body[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->box[i],this->body[i]);
}
// O chassis é composto por dois blocos que são fixos entre si
dJointID fixed = dJointCreateFixed(world,0);
dJointAttach(fixed,this->body[1],this->body[0]);
dJointSetFixed(fixed);
}
dGeomID BoxGeometry::createGeometry(dSpaceID space)
{
Geometry::createGeometry(space);
approxRadius = std::min(std::min(depth, width), height) * 0.5f;
approxRadiusSqr = approxRadius * approxRadius;
return dCreateBox(space, depth, width, height);
}
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags)
{
VERIFY(ref_obj);
R_ASSERT(_valid( start_pos ) );
R_ASSERT( _valid( start_size ) );
m_body = dBodyCreate (0) ;
dMass m;
dMassSetSphere(&m,1.f,100000.f);
dMassAdjust(&m,1.f);
dBodySetMass(m_body,&m);
switch(_type)
{
case etBox:
m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ;
break;
case etSphere:
m_geom = dCreateSphere (0,start_size.x);
break;
};
dGeomCreateUserData (m_geom) ;
dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ;
dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ;
dGeomSetBody (m_geom,m_body) ;
dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ;
Island() .AddBody (m_body) ;
dBodyEnable (m_body) ;
m_safe_state .create(m_body) ;
spatial_register () ;
m_flags.set(flags,TRUE);
}
void BGcubeD_D::BGSetboxBandG() {
boxGeom_m = dCreateBox (gODESpace, 1,1,1);
}
CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
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);
for(int i = 0 ; i < 6 ; i++)
attachedPieces[i] = NULL; // initialize attached piece array to all null pointers
color[0] = 1;
color[1] = 1;
color[2] = 1;
}
GameObject::GameObject(GameWorld& gw, const ObjectPrototype& proto, double x, double y, double z, int id) :
m_meshName(proto.m_meshName), m_sceneEntity(nullptr), m_sceneNode(nullptr), m_lockRotation(proto.m_lockRotation), m_isKinematic(proto.m_isKinematic),
m_maxTurn(proto.m_maxTurnAngle), m_maxForward(proto.m_maxForward), m_maxBackward(proto.m_maxBackward),
m_hitPoints(proto.m_maxHitPoints), m_collisionAccum(0), m_totalDamage(0), m_hasAgent(proto.m_hasAgent), m_gw(gw), m_id(id), m_oldGS(nullptr), m_newGS(nullptr)
{
m_sceneEntity = gw.GetScene()->createEntity(gw.GetMesh(m_meshName));
m_sceneNode = gw.GetScene()->getRootSceneNode()->createChildSceneNode();
m_sceneNode->attachObject(m_sceneEntity);
Ogre::Vector3 size = m_sceneEntity->getBoundingBox().getSize();
m_body = dBodyCreate(gw.GetPhysicsWorld());
m_geom = dCreateBox(gw.GetPhysicsSpace(), size.x, size.y, size.z);
dMassSetBox(&m_mass, proto.m_density, size.x, size.y, size.z);
dBodySetMass(m_body, &m_mass);
dGeomSetBody(m_geom, m_body);
dBodySetPosition(m_body, x, y, z);
// automagically disable things when the body is still for long enough
dBodySetAutoDisableFlag(m_body, 1);
dBodySetAutoDisableLinearThreshold(m_body, 0.1f);
dBodySetAutoDisableAngularThreshold(m_body, 0.25f);
dBodySetAutoDisableSteps(m_body, 1);
// improve simulation accuracy
dBodySetDamping(m_body, 0.0f, 0.1f);
if (proto.m_registerCollisions) {
gw.RegisterForCollisions(this);
}
if (proto.m_isKinematic) {
dBodySetKinematic(m_body);
}
}
void PhysicsActor::postLoad(){
dMass m;
if (type==CUBESHAPE)
{
geom = dCreateBox(space,shape.x,shape.y,shape.z);
dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
}
if (type==CAPSULESHAPE)
{
geom = dCreateCapsule(space,shape.x,shape.y);
dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y); //the '3' means align on z-axis and density is shape.z
generateCapsuleList();
}
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
//initialise position
if (base){
Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix;
dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]);
}else{
dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]);
}
dBodySetDamping(body, linearDamp, angleDamp);
bInit=true;
}
void cubeD_D::SetboxBandG() {
boxBody_m = dBodyCreate (gODEWorld);
boxGeom_m = dCreateBox (gODESpace, 1,1,1);
}
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);
}
}
OscPrismODE::OscPrismODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase *parent)
: OscPrism(NULL, name, parent)
{
dGeomID odeGeom = dCreateBox(odeSpace, m_size.x, m_size.y, m_size.z);
m_pSpecial = new ODEObject(this, odeGeom, odeWorld, odeSpace);
m_density.set(m_density.m_value);
}
void ComponentPhysicsGeom::createGeomBox()
{
ASSERT(physicsEngine, "\"physicsEngine\" was null");
geom = dCreateBox(physicsEngine->getSpace(),
collisionRadius,
collisionRadius,
desiredHeight);
}
//===========================================================================
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;
}
dGeomID CBoxGeom::create()
{
return dCreateBox(0,
m_box.m_halfsize.x*2.f,
m_box.m_halfsize.y*2.f,
m_box.m_halfsize.z*2.f
);
}
void LaserBeam::embody(dWorldID world, dSpaceID space)
{
me = dBodyCreate(world);
embody(me);
//geom = dCreateSphere( space, 2.64f);
geom = dCreateBox( space, Gunshot::width, Gunshot::height, Gunshot::length);
dBodySetGravityMode(me,0);
dGeomSetBody(geom, me);
}
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);
}
}
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;
}
void SBoxParts::set(dWorldID w, dSpaceID space)
{
Size &sz = m_cmpnt.size();
/*
const dReal hx = sz.x();
const dReal hy = sz.y();
const dReal hz = sz.z();
*/
dReal hx = sz.x();
dReal hy = sz.y();
dReal hz = sz.z();
// konao
DUMP(("[SBoxParts::set] ODE geom created (hx, hy, hz)=(%f, %f, %f) [%s:%d]\n", hx, hy, hz, __FILE__, __LINE__));
if (hz == 0) hz = 0.001;
if (hy == 0) hy = 0.001;
if (hx == 0) hx = 0.001;
dGeomID geom = dCreateBox(0, hx, hy, hz);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
// x-axis and z-axis is swapped between ODE/SIGVerse
dMassSetBox(&m, DENSITY, hz, hy, hx); //TODO: mass of cube should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
// Gap between ODE shape and body
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz);
// Initial orientation
dReal offq[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dGeomSetOffsetQuaternion(geom, offq);
//dMassAdjust(&m, 1.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
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;
// create world
dInitODE();
world = dWorldCreate();
#if 1
space = dHashSpaceCreate (0);
#elif 0
dVector3 center = {0,0,0}, extents = { 100, 100, 100};
space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
space = dSimpleSpaceCreate(0);
#endif
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactSurfaceLayer (world,0.001);
ground = dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// create lift platform
platform = dCreateBox(space, 4, 4, 1);
dGeomSetCategoryBits(ground, 1ul);
dGeomSetCategoryBits(platform, 2ul);
dGeomSetCollideBits(ground, ~2ul);
dGeomSetCollideBits(platform, ~1ul);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 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 LinkBoxDesign::create(dWorldID world, dSpaceID space, dGeomID * geom,
dBodyID * body)
{
body[0] = dBodyCreate(world);
geom[0] = dCreateBox(space, sides[XX], sides[YY], sides[ZZ]);
dGeomSetBody(geom[0], body[0]);
dMass m;
dMassSetBox(&m, 1, sides[XX], sides[YY], sides[ZZ]);
dMassAdjust(&m, LINK_MASS);
dBodySetMass(body[0], &m);
}
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);
}
}
