/* 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);
}
}
