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()); } }
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); } }
BarrelBody(Barrel *b) : OdeBody(dBodyCreate(gWorld)), b_(b) { dMass m; dMassSetCylinderTotal(&m, b->mass_, 3, b->radius_, b->height_); dBodySetMass(id_, &m); Vec3 pos(b->pos()); dBodySetPosition(id_, pos.x + b->center_.x, pos.y + b->center_.y, pos.z + b->center_.z); dBodySetData(id_, this); dBodySetAutoDisableFlag(id_, 1); // This will roll forever, very slowly, unless I up the tolerance for sleep. // Compare to a box, which will come to a complete stop easily, and where we // don't want it stopping balancing on an edge. dBodySetAutoDisableAngularThreshold(id_, 0.03f); dBodySetAutoDisableLinearThreshold(id_, 0.03f); dBodySetAutoDisableAverageSamplesCount(id_, 5); }
void YGEBodyAsset::createBody(){ YGETimeSpace::YGESpace* parentSpace = parent->getSpace(); if(parentSpace != NULL && parent->getHasAbsPosition()){ if(hasBody) { dBodyDestroy(bodyId); dGeomDestroy(geomId); } bodyId = dBodyCreate(parentSpace->getWorldId()); mass = new dMass(); dMassSetBox(mass,1,1,1,1); dMassAdjust(mass,bodyMass); dBodySetMass(bodyId, mass); dBodySetAutoDisableFlag(bodyId, 0); YGEMath::Vector3 pos = parent->getAbsPosition(); YGEMath::Quaternion rot = parent->getAbsOrientation(); dBodySetPosition(bodyId,pos.x,pos.y,pos.z); dQuaternion q; q[0]=rot.w; q[1]=rot.x; q[2]=rot.y; q[3]=rot.z; dBodySetQuaternion(bodyId, q); //dBodyAddForce(bodyId, 0.5, 0, 0); // a hull, remove this geomId = dCreateBox(parentSpace->getDSpaceId(), bodySize.x, bodySize.y, bodySize.z); dGeomSetData(geomId, this); dGeomSetCategoryBits(geomId, YGEPhysics::ENTITIES ); dGeomSetCollideBits(geomId, YGEPhysics::ENTITIES | YGEPhysics::STATIC_OBJECTS ); dGeomSetBody(geomId, bodyId); // dBodySetAuto hasBody = true; } }
void Object::MakeBody(dWorldID world) { iBody = dBodyCreate(world); dBodySetQuaternion(iBody,q); dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z); dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z); dMassSetBox(&iMass,iM,iSize.x,iSize.y,iSize.z); dMassAdjust(&iMass,iM/2.0); dBodySetMass(iBody,&iMass); disabledSteps = 0; dBodySetAutoDisableFlag(iBody,1); dBodySetData(iBody,data); dBodyDisable(iBody); };
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)); }
void CBall::Init() { static float dif_orange[4] = {0.75,0.4,0.2,1.0}; static float amb_orange[4] = {0.5,0.2,0.1,1.0}; static float spec[4] = {1.0,1.0,1.0,1.0}; glPushMatrix(); glLoadIdentity(); glTranslatef(0.000, 0.000, 1.100); glGetDoublev(GL_MODELVIEW_MATRIX, position); glPopMatrix(); material.SetDiffuse(dif_orange); material.SetAmbient(amb_orange); material.SetSpecular(spec); material.SetShininess(300); radius = 0.040; //body設定 bBall = dBodyCreate(world); dBodySetAutoDisableFlag(bBall,0); dBodySetPosition(bBall, position[12+0], position[12+1], position[12+2]); dMass mass; dMassSetSphereTotal(&(mass),0.010,0.040); dBodySetMass (bBall ,&(mass)); //geom設定 transBall = dCreateGeomTransform(space); gBall = dCreateSphere (0,0.040); dGeomSetPosition(gBall, 0 ,0, 0.002);//重心が3mmずれてる dGeomTransformSetGeom(transBall, gBall); dGeomSetBody(transBall, bBall); }
State* init() { State* state = new State(); dInitODE(); SDL_Init(SDL_INIT_EVERYTHING); state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL); SDL_WM_SetCaption("Physics", NULL); SDL_Flip(state->screen); SDL_ShowCursor(SDL_DISABLE); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); GLfloat light_ambient[] = { 1, 1, 1, 1 }; glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); GLfloat lightpos[] = {0, 4, 0, 1}; glLightfv(GL_LIGHT0, GL_POSITION, lightpos); glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST); glShadeModel(GL_SMOOTH); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_DEPTH_TEST); glEnable(GL_CULL_FACE); glClearColor(0, 0, 0, 1); state->posx = 0;//21; state->posy = 4;//8; state->posz = 5;//21; state->rotx = 0; state->roty = 0;//-40; state->rotz = 0; state->wkey = false; state->akey = false; state->skey = false; state->dkey = false; state->gkey = false; state->simSpeed = 60; state->carcam = false; state->carbodydrawable = new Drawable("objs/carbody.obj"); state->carwheeldrawable = new Drawable("objs/carwheel.obj"); state->map = new Drawable("objs/jump2.obj"); state->cube = new Drawable("objs/cube.obj"); // state->monkey = new Drawable("objs/monkey.obj"); state->world = dWorldCreate(); dWorldSetGravity(state->world, 0, -9.8, 0); state->worldSpace = dHashSpaceCreate(0); const double carHeight = 0.65; const double carZ = 90; const double carX = 0; const float speed = -1000; const float force = 200; state->carbodybody = dBodyCreate(state->world); dBodySetPosition(state->carbodybody, carX, carHeight, carZ); dMass bodymass; dMassSetBoxTotal(&bodymass, 100, 2, 4, 1); dBodySetMass(state->carbodybody, &bodymass); dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4); dGeomSetBody(carbodygeom, state->carbodybody); state->flcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2); const dMatrix3 m = { 0, 0, 1, 0 , 0, 1, 0, 0 , 0, 0, 1, 0 }; dBodySetRotation(state->flcarwheelbody, m); dMass wheelmass; dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2); dBodySetMass(state->flcarwheelbody, &wheelmass); dJointID joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->flcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(flcarwheelgeom, state->flcarwheelbody); dJointID motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->flcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->frcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2); dBodySetRotation(state->frcarwheelbody, m); dBodySetMass(state->frcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->frcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(frcarwheelgeom, state->frcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->frcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->blcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->blcarwheelbody, m); dBodySetMass(state->blcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->blcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(blcarwheelgeom, state->blcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->blcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->brcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->brcarwheelbody, m); dBodySetMass(state->brcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->brcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(brcarwheelgeom, state->brcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->brcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->var = new double[3]; state->var = dBodyGetPosition(state->carbodybody); // cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl; //TODO check if translation matrix from dBody can be used. dSpaceID cubespace = dHashSpaceCreate(state->worldSpace); for(int i = 0; i < NUM_CUBES/10; i++) { for(int k = 0; k < 10; k++) { state->cubebody[i*10+k] = dBodyCreate(state->world); dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1); dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70); dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2); dGeomSetBody(cubegeom, state->cubebody[i*10+k]); } } { int indexSize = state->map->vertices.size()/3; unsigned int* index = new unsigned int[indexSize]; for(int i = 0; i < indexSize; i++) index[i] = i; dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12); dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); dGeomSetPosition(mapGeom, 0, 0, 0); } // state->monkeyBody = dBodyCreate(state->world); // { // int indexSize = state->monkey->vertices.size()/3; // unsigned int* index = new unsigned int[indexSize]; // for(int i = 0; i < indexSize; i++) // index[i] = i; // dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); // dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12); // dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); // dGeomSetPosition(monkeyGeom, 0, 2, 0); // dGeomSetBody(monkeyGeom, state->monkeyBody); // } state->physicsContactgroup = dJointGroupCreate(0); return state; }
void PhysicsBody::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { Inherited::changed(whichField, origin, details); //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & WorldFieldMask) { if(_BodyID != 0) { dBodyDestroy(_BodyID); } if(getWorld() != NULL) { _BodyID = dBodyCreate(getWorld()->getWorldID()); } } if( ((whichField & PositionFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z()); } if( ((whichField & RotationFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMatrix3 rotation; Vec4f v1 = getRotation()[0]; Vec4f v2 = getRotation()[1]; Vec4f v3 = getRotation()[2]; rotation[0] = v1.x(); rotation[1] = v1.y(); rotation[2] = v1.z(); rotation[3] = 0; rotation[4] = v2.x(); rotation[5] = v2.y(); rotation[6] = v2.z(); rotation[7] = 0; rotation[8] = v3.x(); rotation[9] = v3.y(); rotation[10] = v3.z(); rotation[11] = 0; dBodySetRotation(_BodyID, rotation); } if( ((whichField & QuaternionFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dQuaternion q; q[0]=getQuaternion().w(); q[1]=getQuaternion().x(); q[2]=getQuaternion().y(); q[3]=getQuaternion().z(); dBodySetQuaternion(_BodyID,q); } if( ((whichField & LinearVelFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z()); } if( ((whichField & AngularVelFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z()); } if( ((whichField & ForceFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z()); } if( ((whichField & TorqueFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z()); } if( ((whichField & AutoDisableFlagFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag()); } if( ((whichField & AutoDisableLinearThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold()); } if( ((whichField & AutoDisableAngularThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold()); } if( ((whichField & AutoDisableStepsFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps()); } if( ((whichField & AutoDisableTimeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAutoDisableTime(_BodyID, getAutoDisableTime()); } if( ((whichField & FiniteRotationModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode()); } if( ((whichField & FiniteRotationModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode()); } if( ((whichField & FiniteRotationAxisFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z()); } if( ((whichField & GravityModeFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetGravityMode(_BodyID, getGravityMode()); } if( ((whichField & LinearDampingFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearDamping(_BodyID, getLinearDamping()); } if( ((whichField & AngularDampingFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularDamping(_BodyID, getAngularDamping()); } if( ((whichField & LinearDampingThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold()); } if( ((whichField & AngularDampingThresholdFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold()); } if( ((whichField & MaxAngularSpeedFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { if(getMaxAngularSpeed() >= 0.0) { dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed()); } else { dBodySetMaxAngularSpeed(_BodyID, dInfinity); } } if( ((whichField & MassFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMass TheMass; dBodyGetMass(_BodyID, &TheMass); TheMass.mass = getMass(); dBodySetMass(_BodyID, &TheMass); } if( ((whichField & MassCenterOfGravityFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { //dMass TheMass; //dBodyGetMass(_BodyID, &TheMass); ////TheMass.c[0] = getMassCenterOfGravity().x(); ////TheMass.c[1] = getMassCenterOfGravity().y(); ////TheMass.c[2] = getMassCenterOfGravity().z(); //Vec4f v1 = getMassInertiaTensor()[0]; //Vec4f v2 = getMassInertiaTensor()[1]; //Vec4f v3 = getMassInertiaTensor()[2]; //TheMass.I[0] = v1.x(); //TheMass.I[1] = v1.y(); //TheMass.I[2] = v1.z(); //TheMass.I[3] = getMassCenterOfGravity().x(); //TheMass.I[4] = v2.x(); //TheMass.I[5] = v2.y(); //TheMass.I[6] = v2.z(); //TheMass.I[7] = getMassCenterOfGravity().y(); //TheMass.I[8] = v3.x(); //TheMass.I[9] = v3.y(); //TheMass.I[10] = v3.z(); //TheMass.I[11] = getMassCenterOfGravity().z(); //dBodySetMass(_BodyID, &TheMass); } if( ((whichField & MassInertiaTensorFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { dMass TheMass; dBodyGetMass(_BodyID, &TheMass); Vec4f v1 = getMassInertiaTensor()[0]; Vec4f v2 = getMassInertiaTensor()[1]; Vec4f v3 = getMassInertiaTensor()[2]; TheMass.I[0] = v1.x(); TheMass.I[1] = v1.y(); TheMass.I[2] = v1.z(); TheMass.I[3] = 0; TheMass.I[4] = v2.x(); TheMass.I[5] = v2.y(); TheMass.I[6] = v2.z(); TheMass.I[7] = 0; TheMass.I[8] = v3.x(); TheMass.I[9] = v3.y(); TheMass.I[10] = v3.z(); TheMass.I[11] = 0; dBodySetMass(_BodyID, &TheMass); } if( ((whichField & KinematicFieldMask) || (whichField & WorldFieldMask)) && getWorld() != NULL) { if(getKinematic()) { dBodySetKinematic(_BodyID); } else { dBodySetDynamic(_BodyID); } } }