/* adds a torque given by the vector vec to the body b,
* relative to the body reference frame */
void body_add_rel_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 */
dBodyAddRelTorque (b, vec[0], vec[1], vec[2]);
}
void cPhysicsObject::AddTorqueRelativeToObjectNm(const physvec_t& torqueNm)
{
/*// Get our current rotation
breathe::math::cQuaternion qOriginal;
qOriginal.SetFromODEQuaternion(dBodyGetQuaternion(body));
// Find our desired rotation (Current but with x=0, y=0)
breathe::math::cQuaternion qDesired = qOriginal;
qDesired.x = 0.0f;
qDesired.y = 0.0f;
float quat_len = sqrt(qDesired.z * qDesired.z + qDesired.w * qDesired.w);
qDesired.z /= quat_len;
qDesired.w /= quat_len;
// Set our rotation to a rotation in between these two quaternions
breathe::math::cQuaternion qInterpolated;
qInterpolated.Slerp(qOriginal, qDesired, 0.1f);
dReal values[4];
qInterpolated.GetODEQuaternion(values);
dBodySetQuaternion(body, values);
// Get rid of our x,y angular velocity as well
const dReal* rot = dBodyGetAngularVel(body);
dBodySetAngularVel(body, 0.0f, 0.0f, rot[2]);*/
dBodyAddRelTorque(body, torqueNm.x, torqueNm.y, torqueNm.z);
}
void Balaenidae::doDynamics(dBodyID body)
{
Vec3f Ft;
Ft[0]=0;Ft[1]=0;Ft[2]=getThrottle();
dBodyAddRelForce(body,Ft[0],Ft[1],Ft[2]);
dBodyAddRelTorque(body,0.0f,Balaenidae::rudder*1000,0.0f);
if (offshoring == 1) {
offshoring=0;
setStatus(Balaenidae::SAILING);
}
else if (offshoring > 0)
{
// Add a retractive force to keep it out of the island.
Vec3f ap = Balaenidae::ap;
setThrottle(0.0);
Vec3f V = ap*(-10000);
dBodyAddRelForce(body,V[0],V[1],V[2]);
offshoring--;
}
// Buyoncy
//if (pos[1]<0.0f)
// dBodyAddRelForce(me,0.0,9.81*20050.0f,0.0);
dReal *v = (dReal *)dBodyGetLinearVel(body);
dVector3 O;
dBodyGetRelPointPos( body, 0,0,0, O);
dVector3 F;
dBodyGetRelPointPos( body, 0,0,1, F);
F[0] = (F[0]-O[0]);
F[1] = (F[1]-O[1]);
F[2] = (F[2]-O[2]);
Vec3f vec3fF;
vec3fF[0] = F[0];vec3fF[1] = F[1]; vec3fF[2] = F[2];
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
speed = vec3fV.magnitude();
VERIFY(speed, me);
vec3fV = vec3fV * 0.02f;
dBodyAddRelForce(body,vec3fV[0],vec3fV[1],vec3fV[2]);
wrapDynamics(body);
}
IoObject *IoODEBody_addRelTorque(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);
dBodyAddRelTorque(BODYID, x, y, z);
return self;
}
void ODE_ForceHandle::updateActuators() {
if(mHostBody != 0 && mActivateForces->get()) {
dBodyID hostBodyId = mHostBody->getRigidBodyID();
if(hostBodyId != 0) {
if(mApplyRelativeForces->get()) {
dBodyAddForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
else {
dBodyAddRelForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddRelTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
}
}
}
void RigidBody::addRelativeTorque(const ngl::Vec3 &_t)
{
dBodyAddRelTorque(m_id,_t.m_x,_t.m_y,_t.m_z);
}
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 PhysicsBody::addRelTorque(const Vec3f &v)
{
dBodyAddRelTorque(_BodyID,v.x(), v.y(), v.z());
}
void AvatarGameObj::step_impl() {
dBodyID body = get_entity().get_id();
const Channel* chn;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
dBodyAddRelForce(body, -v, 0.0, 0.0);
}
bool pushing_up = false;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateY);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
if (Saving::get().config().invertTranslateY()) {
v = -v;
}
dBodyAddRelForce(body, 0.0, -v, 0.0);
if (v < 0) {
pushing_up = true;
}
}
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ACCEL/MAX_FPS);
dBodyAddRelForce(body, 0.0, 0.0, -v);
}
const dReal* avel = dBodyGetAngularVel(body);
dVector3 rel_avel;
dBodyVectorFromWorld(body, avel[0], avel[1], avel[2], rel_avel);
// X-turn and x-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateY);
if (chn->is_on()) {
float v = -(chn->get_value())*(MAX_TURN/MAX_FPS);
dBodyAddRelTorque(body, 0.0, v, 0.0);
} else {
float cv = rel_avel[1]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, 0.0, cv, 0.0);
}
// Y-turn and y-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_TURN/MAX_FPS);
if (Saving::get().config().invertRotateY()) {
v = -v;
}
dBodyAddRelTorque(body, v, 0.0, 0.0);
} else {
float cv = rel_avel[0]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, cv, 0.0, 0.0);
}
// Roll and counter-roll
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ROLL/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, v);
} else {
float cv = rel_avel[2]*(-CROLL_COEF/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, cv);
}
// Changing stance between superman-style and upright
if (_attached) {
_uprightness += UPRIGHTNESS_STEP_DIFF;
} else {
_uprightness -= UPRIGHTNESS_STEP_DIFF;
}
if (_uprightness > 1.0) { _uprightness = 1.0; } else if (_uprightness < 0.0) { _uprightness = 0.0; }
update_geom_offsets();
_attached = _attached_this_frame;
// If we are attached, work to keep ourselves ideally oriented to the attachment surface
if (_attached) {
Vector sn_rel = vector_from_world(_sn);
Vector lvel = Vector(dBodyGetLinearVel(body));
Vector lvel_rel = vector_from_world(lvel);
Vector avel = Vector(dBodyGetAngularVel(body));
Vector avel_rel = vector_from_world(avel);
// Apply as much of each delta as we can
// X and Z orientation delta
// TODO Maybe should translate body so that the contact point stays in the same spot through rotation
float a = limit_abs(_zrot_delta, RUNNING_ADJ_RATE_Z_ROT/MAX_FPS);
Vector body_x(vector_to_world(Vector(cos(a), sin(a), 0)));
a = limit_abs(-_xrot_delta, RUNNING_ADJ_RATE_X_ROT/MAX_FPS);
Vector body_y(vector_to_world(Vector(0, cos(a), sin(a))));
dMatrix3 matr;
dRFrom2Axes(matr, body_x.x, body_x.y, body_x.z, body_y.x, body_y.y, body_y.z);
dBodySetRotation(body, matr);
// Y position delta
// If the user is pushing up, set the target point high above the ground so we escape sticky attachment
set_pos(get_pos() + _sn*limit_abs(_ypos_delta + (pushing_up ? RUNNING_MAX_DELTA_Y_POS*2 : 0), RUNNING_ADJ_RATE_Y_POS/MAX_FPS));
// Y linear velocity delta
lvel_rel.y += limit_abs(_ylvel_delta, RUNNING_ADJ_RATE_Y_LVEL/MAX_FPS);
lvel = vector_to_world(lvel_rel);
dBodySetLinearVel(body, lvel.x, lvel.y, lvel.z);
// X and Z angular velocity delta
avel_rel.x += limit_abs(_xavel_delta, RUNNING_ADJ_RATE_X_AVEL/MAX_FPS);
avel_rel.z += limit_abs(_zavel_delta, RUNNING_ADJ_RATE_Z_AVEL/MAX_FPS);
avel = vector_to_world(avel_rel);
dBodySetAngularVel(body, avel.x, avel.y, avel.z);
}
if (_attached_this_frame) {
_attached_this_frame = false;
dGeomEnable(get_entity().get_geom("sticky_attach"));
} else {
dGeomDisable(get_entity().get_geom("sticky_attach"));
}
}