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;
}
/**
* This method initializes the grabbing of an object.
* It creates an ODE-object with the current position and
* orientation and stores the offset to the grabbed object
* for correct update of the Entity-transformation
* in the update method. It also creates all joints and
* linked objects by calling the attachJoints-method.
* @param entity Entity that should be grabbed
**/
void JointInteraction::grabEntity(Entity* entity, TransformationData offset)
{
unsigned entityID;
unsigned typeBasedID;
Entity* ent;
dQuaternion quat;
std::map< int, ODEObject*>::iterator it;
// TransformationPipe* linkedObjectPipe;
// unsigned short type, id;
std::vector<unsigned> entityIds;
unsigned userId;
JointInteractionBeginEvent* evt;
isGrabbing = true;
// // store position and orientation difference between user and object
deltaTrans = offset;
// Create ODE-representation of grabbed object by simply creating an object with
// user's position and orientation
ODEObject* odeObj = new ODEObject;
odeObj->body = dBodyCreate(world);
odeObj->entity = entity;
dBodySetPosition(odeObj->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
convert(quat, userTrans.orientation);
dBodySetQuaternion(odeObj->body, quat);
// attach all joints to object
entityID = entity->getEnvironmentBasedId();
linkedObjMap[entityID] = odeObj;
attachJoints(entityID);
grabbedObject = odeObj;
entityIds.push_back(entity->getTypeBasedId());
// open Pipes for all linked objects
for (it = linkedObjMap.begin(); it != linkedObjMap.end(); ++it)
{
if (it->second == grabbedObject)
continue;
ent = WorldDatabase::getEntityWithEnvironmentId(it->first);
assert(ent->getEnvironmentBasedId() != entityID);
typeBasedID = ent->getTypeBasedId();
entityIds.push_back(typeBasedID);
/*
split(typeBasedID, type, id);
// TODO: open pipes via Event
linkedObjectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, false);
linkedObjPipes[it->first] = linkedObjectPipe;
*/
} // for
userId = UserDatabase::getLocalUserId();
evt = new JointInteractionBeginEvent(userId, entityIds);
EventManager::sendEvent(evt, EventManager::EXECUTE_GLOBAL);
} // grabEntity
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 createPart(const dWorldID & _world , dPart & part, dReal * pos3){
dMass mass;
part.body = dBodyCreate(_world);
dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position
dMassSetZero(&mass); // Set mass parameter to zero
dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length); // Calculate mass parameter
dBodySetMass(part.body, &mass); // Set mass
}
void ODERigidObject::SetTransform(const RigidTransform& T)
{
Vector3 comPos = T*obj.com;
dBodySetPosition(bodyID,comPos.x,comPos.y,comPos.z);
dMatrix3 rot;
CopyMatrix(rot,T.R);
dBodySetRotation(bodyID,rot);
}
void ODE_Link::setTransform(const hrp::Vector3& pos, const hrp::Matrix33& R){
hrp::Vector3 _pos(R * C + pos);
dBodySetPosition(bodyId, _pos(0), _pos(1), _pos(2));
dMatrix3 _R = {R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0};
dBodySetRotation(bodyId, _R);
}
static void simLoop (int pause)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
/**
* This method returns the ODE-representation of the
* Entity with the passed ID. It searches
* in the map of connected objects for a matching
* ODEObject. If no object is found the method
* creates one and stores it in the map.
* @param entityID ID of the EntityTransform
* @return ODE representation of the EntityTransform
**/
dBodyID JointInteraction::getBodyWithID(int entityID)
{
if (entityID == 0)
return 0;
// TransformationPipe* objectPipe;
Entity* entity;
TransformationData trans;
gmtl::Vec3f pos;
gmtl::Quatf ori;
gmtl::AxisAnglef axAng;
dQuaternion quat;
ODEObject* object = linkedObjMap[entityID];
if (object == NULL)
{
entity = getEntityWithID(entityID);
if (entity == NULL)
{
printd(ERROR, "JointInteraction::getBodyWithID(): ERROR: FOUND BODY WITHOUT MATCHING ENTITY OBJECT!!!\n");
return 0;
} // if
object = new ODEObject;
object->body = dBodyCreate(world);
object->entity = entity;
trans = entity->getEnvironmentTransformation();
pos = trans.position;
ori = trans.orientation;
// get current object position and orientation
// pos[0] = entity->getXTrans();
// pos[1] = entity->getYTrans();
// pos[2] = entity->getZTrans();
// axAng.set(entity->getRotAngle(), entity->getXRot(), entity->getYRot(), entity->getZRot());
// gmtl::set(ori, axAng);
convert(quat, ori);
dBodySetPosition(object->body, pos[0], pos[1], pos[2]);
dBodySetQuaternion(object->body, quat);
linkedObjMap[entityID] = object;
// TODO: open and close pipes via events
// open TransformationPipe for handling transformations of connected entities
// split(entity->getTypeBasedId(), type, id);
// objectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, 0);
// linkedObjPipes[entityID] = objectPipe;
// ingnore request if Entity is not movable
if (!entity->getEntityType()->isFixed())
attachJoints(entityID);
} // if
if (object->entity->getEntityType()->isFixed())
return 0;
return object->body;
} // getBodyWithID
/*** ロボットアームの生成 ***/
void makeArm()
{
dMass mass; // 質量パラメータ
dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x
dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y
dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z
dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ
dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量
dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径
dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x
dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y
dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z
dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x
dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y
dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z
// リンクの生成
for (int i = 0; i < NUM-1; i++) {
rlink[i].body = dBodyCreate(world);
dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
dBodySetMass(rlink[i].body, &mass);
rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
dGeomSetBody(rlink[i].geom,rlink[i].body);
}
rlink[NUM-1].body = dBodyCreate(world);
dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
dBodySetMass(rlink[NUM-1].body, &mass);
rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);
// ジョイントの生成とリンクへの取り付け
joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(joint[0], rlink[0].body, 0);
dJointSetFixed(joint[0]);
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
}
}
void TSRODERigidBody::SetBodyTransform( const TSRMatrix4& _bodyTransform )
{
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyTransform, R, P );
dBodySetPosition( m_BodyID, P[ 0 ], P[ 1 ], P[ 2 ] );
dBodySetRotation( m_BodyID, R );
}
/* sets position and rotation of a body based on position and quaternion values */
void body_set_position_and_quaternion (dBodyID b, t_real x, t_real y, t_real z, t_real q1, t_real q2, t_real q3, t_real q4) {
dQuaternion q;
dBodySetPosition (b, x, y, z);
q [0] = q1;
q [1] = q2;
q [2] = q3;
q [3] = q4;
dBodySetQuaternion (b, q);
}
static void reset_state(void)
{
float sx=-4, sy=-4, sz=2;
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
dBodySetPosition (boxbody, sx, sy+1, sz);
dBodySetLinearVel (boxbody, 0,0,0);
dBodySetAngularVel (boxbody, 0,0,0);
dBodySetQuaternion (boxbody, q);
#endif
#ifdef CYL
dBodySetPosition (cylbody, sx, sy, sz);
dBodySetLinearVel (cylbody, 0,0,0);
dBodySetAngularVel (cylbody, 0,0,0);
dBodySetQuaternion (cylbody, q);
#endif
}
/*** 車輪の生成 ***/
void makeWheel()
{
dMass mass;
dReal r = 0.1;
dReal w = 0.024;
dReal m = 0.15;
dReal d = 0.01;
dReal tmp_z = Z;
dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0};
dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)};
dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0};
dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2};
dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
for (int i = 0; i < WHEEL_NUM; i++)
{
wheel[i].v = 0.0;
// make body, geom and set geom to body.
// The position and orientation of geom is abondoned.
wheel[i].body = dBodyCreate(world);
wheel[i].geom = dCreateCylinder(space,r, w);
dGeomSetBody(wheel[i].geom,wheel[i].body);
// set configure of body
dMassSetZero(&mass);
if (i < 2) {
dMassSetCylinderTotal(&mass,m, 1, r, w);
} else {
dMassSetCylinderTotal(&mass,m, 2, r, w);
}
dBodySetMass(wheel[i].body,&mass);
// set position and orientation of body.
dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]);
dMatrix3 R;
if (i >= 2) {
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
} else {
dRFromAxisAndAngle(R,0,1,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
}
// make joint.
wheel[i].joint = dJointCreateHinge(world,0);
dJointAttach(wheel[i].joint,base.body,wheel[i].body);
if (i < 2) {
dJointSetHingeAxis(wheel[i].joint, 1, 0, 0);
} else {
dJointSetHingeAxis(wheel[i].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]);
}
}
void MyDeformableObjectNode::updatePhysicsBodies(int vertexIndex) {
if (mPrintDebugOutput) {
std::cout << mDebugOutputPrefix << "MyDeformableObjectNode::updatePhysicsBodies(vertexIndex: " << vertexIndex << ")" << std::endl;
}
if (!mPhysicsObjects.empty()) {
unsigned int index, objectCount;
dcollide::Vertex* vertex;
MyODEDeformableTestAppGeom* geom;
if (vertexIndex != -1) {
index = vertexIndex;
objectCount = vertexIndex+1;
} else {
index = 0;
objectCount = mPhysicsObjects.size();
}
const std::vector<dcollide::Vertex*>& vertices
= getProxy()->getShape()->getMesh()->getVertices();
for (/* nop */; index < objectCount; ++index) {
geom = mPhysicsObjects[index];
vertex = vertices[index];
dcollide::Vector3 bodyPosition = vertex->getWorldPosition();
if (mPrintDebugOutput) {
if (index == objectCount-1) {
const dReal* odePosition = 0;
odePosition = dBodyGetPosition(geom->getBody());
std::cout << mDebugOutputPrefix << "----------" << std::endl;
std::cout << mDebugOutputPrefix << "ODE Position (before move): " << odePosition[0] << ", " << odePosition[1] << ", " << odePosition[2] << std::endl;
std::cout << mDebugOutputPrefix << "D-Collide World Position: " << bodyPosition << std::endl << std::endl;
}
}
geom->setMoveNodeOnBodyMoved(false);
dBodySetPosition(geom->getBody(), bodyPosition.getX(),
bodyPosition.getY(),
bodyPosition.getZ());
geom->setMoveNodeOnBodyMoved(true);
if (mPrintDebugOutput) {
if (index == objectCount-1) {
const dReal* odePosition = 0;
odePosition = dBodyGetPosition(geom->getBody());
std::cout << mDebugOutputPrefix << "ODE Position (after move): " << odePosition[0] << ", " << odePosition[1] << ", " << odePosition[2] << std::endl << std::endl;
}
}
}
}
}
void CubeBasePiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
dBodySetPosition(body,x,y,z);
dJointID connectingJoint = dJointCreateSlider(world,jointGroup);
dJointAttach(connectingJoint,otherBody,body);
dJointSetSliderParam(connectingJoint,dParamLoStop,0);
dJointSetSliderParam(connectingJoint,dParamHiStop,0);
}
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
dInitODE2(0);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
IoObject *IoODEBody_setPosition(IoODEBody *self, IoObject *locals, IoMessage *m)
{
const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBody_assertValidBody(self, locals, m);
dBodySetPosition(BODYID, x, y, z);
return self;
}
void ode::newLine(float xp, float yp)
{
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
//dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
lineStrips[lines] = dCreateSphere (space,RADIUS);
dGeomSetBody (lineStrips[lines++],b);
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
static void reset_ball(void)
{
float sx=0.0f, sy=3.40f, sz=7.15;
dQuaternion q;
dQSetIdentity(q);
dBodySetPosition (sphbody, sx, sy, sz);
dBodySetQuaternion(sphbody, q);
dBodySetLinearVel (sphbody, 0,0,0);
dBodySetAngularVel (sphbody, 0,0,0);
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
void ode::newBall(int xp, int yp)
{
dReal z=1,y=1,x=1;
b = dBodyCreate (world);
dBodySetPosition (b,xp,yp,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
/* create world */
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
virtual void PhDataUpdate(dReal step)
{
const float *linear_velocity =dBodyGetLinearVel(m_body);
if(VelocityLimit())
{
dBodySetPosition(m_body,
m_safe_position[0]+linear_velocity[0]*fixed_step,
m_safe_position[1]+linear_velocity[1]*fixed_step,
m_safe_position[2]+linear_velocity[2]*fixed_step);
}
if(!dV_valid(dBodyGetPosition(m_body)))
dBodySetPosition(m_body,m_safe_position[0]-m_safe_velocity[0]*fixed_step,
m_safe_position[1]-m_safe_velocity[1]*fixed_step,
m_safe_position[2]-m_safe_velocity[2]*fixed_step);
dVectorSet(m_safe_position,dBodyGetPosition(m_body));
dVectorSet(m_safe_velocity,linear_velocity);
}
void Rigid::_visual2physic()
{
const Ogre::Vector3& v3 = mNode->getPosition();
dBodySetPosition(mBodyID,v3.x,v3.y,v3.z);
const Ogre::Quaternion& q = mNode->getOrientation();
dQuaternion dq;
dq[0] = q.w;
dq[1] = q.x;
dq[2] = q.y;
dq[3] = q.z;
dBodySetQuaternion(mBodyID,dq);
}
void Sphere::MakeBody(dWorldID world)
{
iBody = dBodyCreate(world);
dMassSetSphereTotal(&iMass,iM,iRadius);
dBodySetMass(iBody,&iMass);
dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z);
dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z);
disabledSteps = 0;
dBodySetData(iBody,data);
}
int main (int argc, char **argv)
{
static dMass m;
dReal mass = 1.0;
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
space = dSimpleSpaceCreate (0);
//@a box
capsule = dBodyCreate (world);
geom = dCreateCapsule (space,radius,length); //create geometry.
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule,&m);
dGeomSetBody(geom,capsule);
dBodySetPosition (capsule,0,4,1);
//Gravedad y cosas de simulacion
dWorldSetGravity(world,0,0,-9.81);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
//-- Destroy the world!!!
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
/*** キーボードコマンドの処理関数 ***/
static void command(int cmd)
{
switch (cmd)
{
case '1':
float xyz1[3],hpr1[3];
dsGetViewpoint(xyz1,hpr1);
printf("xyz=%4.2f %4.2f %4.2f ",xyz1[0],xyz1[1],xyz1[2]);
printf("hpr=%6.2f %6.2f %5.2f \n",hpr1[0],hpr1[1],hpr1[2]);
break;
case 'k':
wheel[3].v -= 0.8;
break; // 後進
case 's':
wheel[0].v = wheel[1].v = wheel[2].v = wheel[3].v = 0.0; // 停止
break;
case 'x': // ゴロシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1], 2));
if (dist < 0.3)
{
dReal vmax = POWER * 0.01 * 8.0;
dBodySetLinearVel(ball.body,vmax * R[0],vmax * R[4], 0.0);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'l': // ループシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1],2));
if (dist < 1.0)
{
dReal vmax45 = POWER * 0.01 * 8.0 / sqrt(2.0);
dBodySetLinearVel(ball.body,vmax45 * R[0],vmax45 * R[4], vmax45);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'b':
dBodySetPosition(ball.body,0,0,0.14+offset_z);
dBodySetLinearVel(ball.body,0,0,0);
dBodySetAngularVel(ball.body,0,0,0);
break;
}
}
void dRigidBodyArraySetPosition(dRigidBodyArrayID bodyArray, dReal x, dReal y, dReal z) {
dBodyID center = bodyArray->center;
const dReal *p0 = dBodyGetPosition(center);
dVector3 ps = {x, y, z};
dVector3 ps_p0;
dOP(ps_p0, -, ps, p0);
for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) {
dBodyID body = dRigidBodyArrayGet(bodyArray, i);
const dReal *pi = dBodyGetPosition(body);
dVector3 p1;
dOP(p1, +, ps_p0, pi);
dBodySetPosition(body, p1[0], p1[1], p1[2]);
}
}
void Car::createBody(const dReal * pos)
{
// create body
std::cout << "debug: in createBody\n";
body_obj =
new BoxObject(world, space, car_design->getBodySides(),
body_mass);
dBodySetPosition(body_obj->body[0], pos[XX], pos[YY], pos[ZZ]);
dBodySetRotation(body_obj->body[0],
car_design->getCenterRotation());
//dBodySetRotation(body_obj->body[0], R);
}
