/* sets a torque given by the vector vec to the body b */ void body_set_torque (dBodyID b, const t_real *vec) { /* let's check if we have the surface, in which case we exit doing nothing */ if (b == SURFACE_BODY_ID) return; /* in all other cases, we add the torque to the body */ dBodySetTorque (b, vec[0], vec[1], vec[2]); }
void BodyCutForce(dBodyID body,float l_limit,float w_limit) { const dReal wa_limit=w_limit/fixed_step; const dReal* force= dBodyGetForce(body); dReal force_mag=dSqrt(dDOT(force,force)); //body mass dMass m; dBodyGetMass(body,&m); dReal force_limit =l_limit/fixed_step*m.mass; if(force_mag>force_limit) { dBodySetForce(body, force[0]/force_mag*force_limit, force[1]/force_mag*force_limit, force[2]/force_mag*force_limit ); } const dReal* torque=dBodyGetTorque(body); dReal torque_mag=dSqrt(dDOT(torque,torque)); if(torque_mag<0.001f) return; dMatrix3 tmp,invI,I; // compute inertia tensor in global frame dMULTIPLY2_333 (tmp,m.I,body->R); dMULTIPLY0_333 (I,body->R,tmp); // compute inverse inertia tensor in global frame dMULTIPLY2_333 (tmp,body->invI,body->R); dMULTIPLY0_333 (invI,body->R,tmp); //angular accel dVector3 wa; dMULTIPLY0_331(wa,invI,torque); dReal wa_mag=dSqrt(dDOT(wa,wa)); if(wa_mag>wa_limit) { //scale w for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag; dVector3 new_torqu; dMULTIPLY0_331(new_torqu,I,wa); dBodySetTorque ( body, new_torqu[0], new_torqu[1], new_torqu[2] ); } }
IoObject *IoODEBody_setTorque(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); dBodySetTorque(BODYID, x, y, z); return self; }
void FixBody(dBodyID body,float ext_param,float mass_param) { dMass m; dMassSetSphere(&m,1.f,ext_param); dMassAdjust(&m,mass_param); dBodySetMass(body,&m); dBodySetGravityMode(body,0); dBodySetLinearVel(body,0,0,0); dBodySetAngularVel(body,0,0,0); dBodySetForce(body,0,0,0); dBodySetTorque(body,0,0,0); }
bool ODERigidObject::ReadState(File& f) { Vector3 w,v; dReal pos[3]; dReal q[4]; dReal frc[3]; dReal trq[3]; if(!ReadArrayFile(f,pos,3)) return false; if(!ReadArrayFile(f,q,4)) return false; if(!ReadFile(f,w)) return false; if(!ReadFile(f,v)) return false; if(!ReadArrayFile(f,frc,3)) return false; if(!ReadArrayFile(f,trq,3)) return false; dBodySetPosition(bodyID,pos[0],pos[1],pos[2]); dBodySetQuaternion(bodyID,q); dBodySetForce(bodyID,frc[0],frc[1],frc[2]); dBodySetTorque(bodyID,trq[0],trq[1],trq[2]); SetVelocity(w,v); return true; }
void CDynamics3DEntity::Reset() { /* Clear force and torque on the body */ dBodySetForce(m_tBody, 0.0f, 0.0f, 0.0f); dBodySetTorque(m_tBody, 0.0f, 0.0f, 0.0f); /* Clear speeds */ dBodySetLinearVel(m_tBody, 0.0f, 0.0f, 0.0f); dBodySetAngularVel(m_tBody, 0.0f, 0.0f, 0.0f); /* Reset position */ const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition(); dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); /* Reset orientation */ const CQuaternion& cQuaternion = GetEmbodiedEntity().GetInitOrientation(); dQuaternion tQuat = { cQuaternion.GetW(), cQuaternion.GetX(), cQuaternion.GetY(), cQuaternion.GetZ() }; dBodySetQuaternion(m_tBody, tQuat); }
void Player::Update(scalar dt) { const dReal * pos = dBodyGetPosition(bodyId); position.x = pos[0]; position.y = pos[1]; position.z = pos[2]; if (moveForward) { TGen::Vector3 hej = direction * 2.6f * dt; dBodyEnable(bodyId); dBodySetForce(bodyId, hej.x, hej.y, hej.z); //dBodySetLinearVel(bodyId, hej.x, hej.y, hej.z); } else { // dBodySetLinearVel(bodyId, 0.0f, 0.0f, 0.0f); } if (spinLeft) lookDir += dt * 2.0f; if (spinRight) lookDir -= dt * 2.0f; direction.x = sin(lookDir); direction.z = cos(lookDir); dMatrix3 ident; dRSetIdentity(ident); dBodySetRotation(bodyId, ident); dBodySetAngularVel(bodyId, 0.0f, 0.0f, 0.0f); dBodySetTorque(bodyId, 0.0f, 0.0f, 0.0f); //std::cout << "player: " << std::string(position) << std::endl; }
void RigidBody::setTorque(const ngl::Vec3 &_p) { dBodySetTorque(m_id,_p.m_x,_p.m_y,_p.m_z); }
void StickyObj::resetAll(){ dBodySetForce(this->bodyID,0,0,0); dBodySetTorque(this->bodyID,0,0,0); dBodySetLinearVel(this->bodyID,0,0,0); dBodySetAngularVel(this->bodyID,0,0,0); }
void ODESimulator::stepPhysics() { // Apply linear and angular damping; if using the "add opposing // forces" method, be sure to do this before calling ODE step // function. std::vector<Solid*>::iterator iter; for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter) { ODESolid* solid = (ODESolid*) (*iter); if (!solid->isStatic()) { if (solid->isSleeping()) { // Reset velocities, force, & torques of objects that go // to sleep; ideally, this should happen in the ODE // source only once - when the object initially goes to // sleep. dBodyID bodyID = ((ODESolid*) solid)->internal_getBodyID(); dBodySetLinearVel(bodyID, 0, 0, 0); dBodySetAngularVel(bodyID, 0, 0, 0); dBodySetForce(bodyID, 0, 0, 0); dBodySetTorque(bodyID, 0, 0, 0); } else { // Dampen Solid motion. 3 possible methods: // 1) apply a force/torque in the opposite direction of // motion scaled by the body's velocity // 2) same as 1, but scale the opposing force by // the body's momentum (this automatically handles // different mass values) // 3) reduce the body's linear and angular velocity by // scaling them by 1 - damping * stepsize dBodyID bodyID = solid->internal_getBodyID(); dMass mass; dBodyGetMass(bodyID, &mass); // Method 1 //const dReal* l = dBodyGetLinearVel(bodyID); //dReal damping = -solid->getLinearDamping(); //dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]); //const dReal* a = dBodyGetAngularVel(bodyID); //damping = -solid->getAngularDamping(); //dBodyAddTorque(bodyID, damping*a[0], damping*a[1], damping*a[2]); // Method 2 // Since the ODE mass.I inertia matrix is local, angular // velocity and torque also need to be local. // Linear damping real linearDamping = solid->getLinearDamping(); if (0 != linearDamping) { const dReal * lVelGlobal = dBodyGetLinearVel(bodyID); // The damping force depends on the damping amount, // mass, and velocity (i.e. damping amount and // momentum). dReal dampingFactor = -linearDamping * mass.mass; dVector3 dampingForce = { dampingFactor * lVelGlobal[0], dampingFactor * lVelGlobal[1], dampingFactor * lVelGlobal[2] }; // Add a global force opposite to the global linear // velocity. dBodyAddForce(bodyID, dampingForce[0], dampingForce[1], dampingForce[2]); } // Angular damping real angularDamping = solid->getAngularDamping(); if (0 != angularDamping) { const dReal * aVelGlobal = dBodyGetAngularVel(bodyID); dVector3 aVelLocal; dBodyVectorFromWorld(bodyID, aVelGlobal[0], aVelGlobal[1], aVelGlobal[2], aVelLocal); // The damping force depends on the damping amount, // mass, and velocity (i.e. damping amount and // momentum). //dReal dampingFactor = -angularDamping * mass.mass; dReal dampingFactor = -angularDamping; dVector3 aMomentum; // Make adjustments for inertia tensor. dMULTIPLYOP0_331(aMomentum, = , mass.I, aVelLocal); dVector3 dampingTorque = { dampingFactor * aMomentum[0], dampingFactor * aMomentum[1], dampingFactor * aMomentum[2] }; // Add a local torque opposite to the local angular // velocity. dBodyAddRelTorque(bodyID, dampingTorque[0], dampingTorque[1], dampingTorque[2]); } //dMass mass; //dBodyGetMass(bodyID, &mass); //const dReal* l = dBodyGetLinearVel(bodyID); //dReal damping = -solid->getLinearDamping() * mass.mass; //dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]); //const dReal* aVelLocal = dBodyGetAngularVel(bodyID); ////dVector3 aVelLocal; ////dBodyVectorFromWorld(bodyID, aVelGlobal[0], aVelGlobal[1], aVelGlobal[2], aVelLocal); //damping = -solid->getAngularDamping(); //dVector3 aMomentum; //dMULTIPLYOP0_331(aMomentum, =, aVelLocal, mass.I); //dBodyAddTorque(bodyID, damping*aMomentum[0], damping*aMomentum[1], // damping*aMomentum[2]); // Method 3 //const dReal* l = dBodyGetLinearVel(bodyID); //dReal damping = (real)1.0 - solid->getLinearDamping() * mStepSize; //dBodySetLinearVel(bodyID, damping*l[0], damping*l[1], damping*l[2]); //const dReal* a = dBodyGetAngularVel(bodyID); //damping = (real)1.0 - solid->getAngularDamping() * mStepSize; //dBodySetAngularVel(bodyID, damping*a[0], damping*a[1], damping*a[2]); } } } // Do collision detection; add contacts to the contact joint group. dSpaceCollide(mRootSpaceID, this, &ode_hidden::internal_collisionCallback); // Take a simulation step. if (SOLVER_QUICKSTEP == mSolverType) { dWorldQuickStep(mWorldID, mStepSize); } else { dWorldStep(mWorldID, mStepSize); } // Remove all joints from the contact group. dJointGroupEmpty(mContactJointGroupID); // Fix angular velocities for freely-spinning bodies that have // gained angular velocity through explicit integrator inaccuracy. for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter) { ODESolid* s = (ODESolid*) (*iter); if (!s->isSleeping() && !s->isStatic()) { s->internal_doAngularVelFix(); } } }
void SParts::setTorque(dReal tx, dReal ty, dReal tz) { dBodySetTorque(m_odeobj->body(), tx, ty, tz); }
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); } } }
/** * This method updates the position and orientation of the grabbed * object and all connected objects. If the grabbed object has * no connections to other objects (via joints) the method simply * applies the passed position and orientation to the grabbed object. * Otherwise the calculation of the new transformation takes place in * two steps: * First the orientational difference between the grabbed object and * and the passed orientation is calculated and applied to the object * as torque. Then a simulation-step takes place, where the timestep * is set to 0.5, what should leed to the half orientation correction. * After the simulation step the objects velocities are set to zero. * In the second step the difference of the objects position and the * passed one are calculated and applied to the object as force. Like * in step one a simulation-step with dt=0.5 takes place and the * velocities are set to zero again. * After the computation of the new transformations, the method iterates * over all objects, which are grabbed or are linked with the grabbed * object and copies their transformation from the ODE-representation * to the Entity-members. * At last the method calls the checkConstraints-method of all currently * used joints, so that the joints can be activated or deactivated. * @param position: position of the manipulating object * @param orientation: orientation of the manipulating object */ TransformationData JointInteraction::update(TransformationData trans) { if (!isInitialized) init(); gmtl::AxisAnglef AxAng; gmtl::Vec3f diff; dQuaternion quat; userTrans.position = trans.position; userTrans.orientation = trans.orientation; if (!isGrabbing) return identityTransformation(); int numJoints = dBodyGetNumJoints(grabbedObject->body); if (!numJoints) { // Simple code for objects without Joints convert(quat, userTrans.orientation); dBodySetPosition(grabbedObject->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]); dBodySetQuaternion(grabbedObject->body, quat); } else { // Code for object with Joints TransformationData objectTrans; gmtl::Quatf rotation; gmtl::Vec3f force, torque; const dReal* pos = dBodyGetPosition(grabbedObject->body); const dReal* rot = dBodyGetQuaternion(grabbedObject->body); convert(objectTrans.position, pos); convert(objectTrans.orientation, rot); // TODO: move this code to manipulationTerminationModel // ungrab object if distance is too high // diff = userTrans.position - objectTrans.position; // if (gmtl::length(diff) > maxDist) // { // printf("JointInteraction::update(): distance %f too high!\n", gmtl::length(diff)); // ungrab(); // return identityTransformation(); // } // if // STEP 1: apply Torque // calculate Torque = angular offset from object orientation to current orientation rotation = userTrans.orientation; rotation *= invert(objectTrans.orientation); gmtl::set(AxAng, rotation); torque[0] = AxAng[0]*AxAng[1]; // rotAngle*rotX torque[1] = AxAng[0]*AxAng[2]; // rotAngle*rotY torque[2] = AxAng[0]*AxAng[3]; // rotAngle*rotZ // TODO: move this code to manipulationTerminationModel // ungrab object if angular distance is too high // if (gmtl::length(torque) > maxRotDist) // { // printf("JointInteraction::update(): rotation %f too high!\n", gmtl::length(torque)); // ungrab(); // return identityTransformation(); // } // if // create a Ball Joint to adjust orientation without changing the position dJointID universalJoint = dJointCreateBall(world, 0); dJointAttach(universalJoint, grabbedObject->body, 0); dJointSetBallAnchor(universalJoint, pos[0], pos[1], pos[2]); // printf("Torque = %f %f %f\n", torque[0], torque[1], torque[2]); /*** * old way --> is very instable due to high timestep!!! *** * dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]); * // Half way to destination * dWorldStep(world, 0.5); **/ // take some simulation-steps for adjusting object's orientation for (int i=0; i < stepsPerFrame; i++) { dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]); dWorldStep(world, 1.0f/stepsPerFrame); } // for dJointDestroy(universalJoint); // Reset speed and angular velocity to 0 dBodySetLinearVel(grabbedObject->body, 0, 0, 0); dBodySetAngularVel(grabbedObject->body, 0, 0, 0); // STEP 2: apply force into wanted direction // calculate Force = vector from object position to current position force = (userTrans.position - objectTrans.position); // printf("Force = %f %f %f\n", force[0], force[1], force[2]); /*** * old way --> is very instable due to high timestep!!! *** * dBodySetForce(grabbedObject->body, force[0], force[1], force[2]); * // Half way to destination * dWorldStep(world, 0.5); **/ // take some simulation-steps for adjusting object's position for (int i=0; i < stepsPerFrame; i++) { dBodySetForce(grabbedObject->body, force[0], force[1], force[2]); dWorldStep(world, 1.0f/stepsPerFrame); } // for // Reset speed and angular velocity to 0 dBodySetLinearVel(grabbedObject->body, 0, 0, 0); dBodySetAngularVel(grabbedObject->body, 0, 0, 0); } // Get position and Orientation from simulated object TransformationData newTransform = identityTransformation(); const dReal* pos = dBodyGetPosition(grabbedObject->body); const dReal* rot = dBodyGetQuaternion(grabbedObject->body); convert(newTransform.orientation, rot); convert(newTransform.position, pos); // Get position and Orientation from simulated objects std::map<int, ODEObject*>::iterator it = linkedObjMap.begin(); ODEObject* object; TransformationData linkedObjectTrans = identityTransformation(); while (it != linkedObjMap.end()) { object = linkedObjMap[(*it).first]; if (object != grabbedObject) { linkedObjectTrans = object->entity->getWorldTransformation(); const dReal* pos1 = dBodyGetPosition(object->body); const dReal* rot1 = dBodyGetQuaternion(object->body); convert (linkedObjectTrans.orientation, rot1); convert (linkedObjectTrans.position, pos1); linkedObjPipes[it->first]->push_back(linkedObjectTrans); // gmtl::set(AxAng, newRot); /* object->entity->setTranslation(pos1[0], pos1[1], pos1[2]); object->entity->setRotation(AxAng[1], AxAng[2], AxAng[3], AxAng[0]);*/ // assert(false); // object->entity->setEnvironmentTransformation(linkedObjectTrans); // object->entity->update(); } // if ++it; } // while // Update angles in HingeJoints and check joint-constraints HingeJoint* hinge; for (int i=0; i < (int)attachedJoints.size(); i++) { hinge = dynamic_cast<HingeJoint*>(attachedJoints[i]); if (hinge) hinge->checkAngles(); assert(attachedJoints[i]); attachedJoints[i]->checkConstraints(); } // for TransformationData result; multiply(result, newTransform, deltaTrans); TransformationPipe* pipe = linkedObjPipes[grabbedObject->entity->getEnvironmentBasedId()]; if (pipe) pipe->push_back(result); else printd(WARNING, "JointInteraction::update(): Could not find Pipe for manipulated Object!\n"); return newTransform; } // update
void base::hack_2d(void) { const dReal *rot = dBodyGetAngularVel (body); const dReal *quat_ptr; dReal quat[4], quat_len; quat_ptr = dBodyGetQuaternion (body); quat[0] = quat_ptr[0]; quat[1] = 0; quat[2] = 0; quat[3] = quat_ptr[3]; quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); quat[0] /= quat_len; quat[3] /= quat_len; dBodySetQuaternion (body, quat); dBodySetAngularVel (body, 0, 0, rot[2]); //Restricting Rotation To One Axis //The plane2D stops objects rotating along non-2d axes, //but it does not compensate for drift const dReal *rot1 = dBodyGetAngularVel( body ); const dReal *quat_ptr1; dReal quat1[4], quat_len1; quat_ptr1 = dBodyGetQuaternion( body ); quat1[0] = quat_ptr1[0]; quat1[1] = 0; quat1[2] = 0; quat1[3] = quat_ptr1[3]; quat_len1 = sqrt( quat1[0] * quat1[0] + quat1[3] * quat1[3] ); quat1[0] /= quat_len1; quat1[3] /= quat_len1; dBodySetQuaternion( body, quat1 ); dBodySetAngularVel( body, 0, 0, rot1[2] ); dMatrix3 R = { 0, 0, 0, 0, 0, 1}; // 0 0 y if(!rotatebit) //if there is no set rotatebit then don't make rotate in ode { quat1[0] = 0; quat1[1] = 0; quat1[2] = 0; quat1[3] = 1; dBodySetQuaternion( body, quat1 ); dBodySetAngularVel( body, 0, 0, 0 ); } static dVector3 pointrelvel; const dReal *odepos; const dReal *force; dBodyGetRelPointVel(body,0,0,0,pointrelvel); odepos=dBodyGetPosition(body); force = dBodyGetForce (body); dBodySetForce(body,force[0],force[1],0.000); if (odepos[2]>=0.001 || odepos[2]<=-0.001 ){ dBodySetPosition (body,odepos[0],odepos[1], 0.000); dBodyAddRelForce (body,0,0, -(pointrelvel[2])); dBodySetTorque (body, 0.00, 0.00, 0.000); } }