/*
* This function computes the force and torque applied by the object on the ground with respect to the center of the contacting surface
* It tehn updates the values stored in locally in the object
*
*/
void StickyObj::computeAndUpdateFeedback(){
dVector3 jointTempForce;
dVector3 jointTempTorque;
dBodyVectorFromWorld(bodyID,linkJointFeedback.f1[0],linkJointFeedback.f1[1],linkJointFeedback.f1[2],jointTempForce);
dBodyVectorFromWorld(bodyID,linkJointFeedback.t1[0],linkJointFeedback.t1[1],linkJointFeedback.t1[2],jointTempTorque);
// Update objects vectors
linkJointForce = Vector3D(jointTempForce[0],jointTempForce[1],jointTempForce[2]);
linkJointTorque = Vector3D(jointTempTorque[0],jointTempTorque[1],jointTempTorque[2]);
}
void Gyroscope::GyroscopeSensor::updateValue()
{
const dReal* angularVelInWorld = dBodyGetAngularVel(body->body);
dVector3 result;
dBodyVectorFromWorld(body->body, angularVelInWorld[0], angularVelInWorld[1], angularVelInWorld[2], result);
ODETools::convertVector(result, angularVel);
}
/* returns the total kinetic energy of a body */
t_real body_total_kinetic_energy (dBodyID b) {
const t_real *v, *omega;
dVector3 wb;
t_real trans_energy, rot_energy, energy;
dMass m;
/* get the linear and angular velocities */
dBodyGetMass (b, &m);
v = dBodyGetLinearVel (b);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
/* and assign the energy */
/* TODO: restore */
/* energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
energy += 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]); */
trans_energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
rot_energy = 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]);
energy = trans_energy + rot_energy;
/* printf ("trans: %.3f rot: %.3f\n", trans_energy, rot_energy); */
/* and return */
return energy;
}
void dJointSetGearboxAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
dNormalize3(joint->axis2);
}
/* returns the rotational z kinetic energy of a body */
t_real body_zrot_kinetic_energy (dBodyID b) {
dMass m;
const t_real *omega;
dVector3 wb;
dBodyGetMass (b, &m);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
return 0.5*m.I[10]*wb[2]*wb[2];
}
void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT(joint->mode = dTransmissionIntersectingAxes,
"can't set individual axes in current mode" );
if (joint->node[1].body) {
dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]);
dNormalize3(joint->axes[1]);
}
joint->update = 1;
}
void dxPlanarJoint::updatePlane() {
dBodyID body1 = this->node[0].body;
if (body1)
{
// compute plane normal and anchor coordinates in the local coordinates of the first body
dBodyVectorFromWorld(body1, planeNormal[0], planeNormal[1], planeNormal[2], axis1);
dBodyGetPosRelPoint(body1, anchor[0], anchor[1], anchor[2], anchor1);
dBodyID body2 = this->node[1].body;
if (body2) // second body given, attach the plane to it
{
// compute plane normal and anchor coordinates in the local coordinates of the second body
dBodyVectorFromWorld(body2, planeNormal[0], planeNormal[1], planeNormal[2], axis2);
dBodyGetPosRelPoint(body2, anchor[0], anchor[1], anchor[2], anchor2);
}
else // second body not given, attach the plane to the world
{
// plane normal and anchor coordinates in the world world frame are the same as global coordinates
dCopyVector3(axis2, planeNormal);
dCopyVector3(anchor2, anchor);
}
}
}
void dJointSetTransmissionAxis( dJointID j, dReal x, dReal y, dReal z )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
int i;
dUASSERT( joint, "bad joint argument" );
dUASSERT(joint->mode == dTransmissionParallelAxes ||
joint->mode == dTransmissionChainDrive ,
"axes must be set individualy in current mode" );
for (i = 0 ; i < 2 ; i += 1) {
if (joint->node[i].body) {
dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]);
dNormalize3(joint->axes[i]);
}
}
joint->update = 1;
}
void Accelerometer::AccelerometerSensor::updateValue()
{
const dReal* linearVelInWorld = dBodyGetLinearVel(body->body);
Scene* scene = Simulation::simulation->scene;
const float timeScale = 1.f / (scene->stepLength * float(Simulation::simulation->simulationStep - lastSimulationStep));
float linearAccInWorld[3];
linearAccInWorld[0] = (float) ((linearVelInWorld[0] - this->linearVelInWorld[0]) * timeScale);
linearAccInWorld[1] = (float) ((linearVelInWorld[1] - this->linearVelInWorld[1]) * timeScale);
linearAccInWorld[2] = (float) ((linearVelInWorld[2] - this->linearVelInWorld[2]) * timeScale - scene->gravity);
dVector3 result;
dBodyVectorFromWorld(body->body, linearAccInWorld[0], linearAccInWorld[1], linearAccInWorld[2], result);
ODETools::convertVector(result, linearAcc);
this->linearVelInWorld[0] = (float) linearVelInWorld[0];
this->linearVelInWorld[1] = (float) linearVelInWorld[1];
this->linearVelInWorld[2] = (float) linearVelInWorld[2];
lastSimulationStep = Simulation::simulation->simulationStep;
}
void StickyObj::setCollidingPointPos(int i, Vector3D pos){
dVector3 temp;
dBodyVectorFromWorld(bodyID,pos.getX(),pos.getY(),pos.getZ(),temp);
collidingPointPos[i] = Vector3D(temp[0],temp[1],temp[2]);
}
/* returns the angular velocity of body b in the body reference frame */
void body_angular_velocity_principal_axes (t_real *res, dBodyID b) {
const t_real * omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega[0], omega[1], omega[2], res);
}
void
dxJointTransmission::getInfo2( dReal worldFPS,
dReal /*worldERP*/,
const Info2Descr* info )
{
dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
const dReal *p[2], *omega[2];
int i;
// Transform all needed quantities to the global frame.
for (i = 0 ; i < 2 ; i += 1) {
dBodyGetRelPointPos(node[i].body,
anchors[i][0], anchors[i][1], anchors[i][2],
a[i]);
dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
n[i]);
p[i] = dBodyGetPosition(node[i].body);
omega[i] = dBodyGetAngularVel(node[i].body);
}
if (update) {
// Make sure both gear reference frames end up with the same
// handedness.
if (dCalcVectorDot3(n[0], n[1]) < 0) {
dNegateVector3(axes[0]);
dNegateVector3(n[0]);
}
}
// Calculate the mesh geometry based on the current mode.
switch (mode) {
case dTransmissionParallelAxes:
// Simply calculate the contact point as the point on the
// baseline that will yield the correct ratio.
dIASSERT (ratio > 0);
dSubtractVectors3(d, a[1], a[0]);
dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
dCopyVector3(c[1], c[0]);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
dCalcVectorCross3(l[i], d, n[i]);
}
break;
case dTransmissionIntersectingAxes:
// Calculate the line of intersection between the planes of the
// gears.
dCalcVectorCross3(l[0], n[0], n[1]);
dCopyVector3(l[1], l[0]);
nn = dCalcVectorDot3(n[0], n[1]);
dIASSERT(fabs(nn) != 1);
na_0 = dCalcVectorDot3(n[0], a[0]);
na_1 = dCalcVectorDot3(n[1], a[1]);
dAddScaledVectors3(O, n[0], n[1],
(na_0 - na_1 * nn) / (1 - nn * nn),
(na_1 - na_0 * nn) / (1 - nn * nn));
// Find the contact point as:
//
// c = ((r_a - O) . l) l + O
//
// where r_a the anchor point of either gear and l, O the tangent
// line direction and origin.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3(d, a[i], O);
m = dCalcVectorDot3(d, l[i]);
dAddScaledVectors3(c[i], O, l[i], 1, m);
}
break;
case dTransmissionChainDrive:
dSubtractVectors3(d, a[0], a[1]);
m = dCalcVectorLength3(d);
dIASSERT(m > 0);
// Caclulate the angle of the contact point relative to the
// baseline.
cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
// Calculate the contact radius in the local reference
// frame of the chain. This has axis x pointing along the
// baseline, axis y pointing along the sprocket axis and
// the remaining axis normal to both.
u[0] = radii[i] * cosphi;
u[1] = 0;
u[2] = radii[i] * sinphi;
// Transform the contact radius into the global frame.
dCalcVectorCross3(z, d, n[i]);
v[0] = dCalcVectorDot3(d, u);
v[1] = dCalcVectorDot3(n[i], u);
v[2] = dCalcVectorDot3(z, u);
// Finally calculate contact points and l.
dAddVectors3(c[i], a[i], v);
dCalcVectorCross3(l[i], v, n[i]);
dNormalize3(l[i]);
// printf ("%d: %f, %f, %f\n",
// i, l[i][0], l[i][1], l[i][2]);
}
break;
}
if (update) {
// We need to calculate an initial reference frame for each
// wheel which we can measure the current phase against. This
// frame will have the initial contact radius as the x axis,
// the wheel axis as the z axis and their cross product as the
// y axis.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], a[i]);
radii[i] = dCalcVectorLength3(r[i]);
dIASSERT(radii[i] > 0);
dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
reference[i]);
dNormalize3(reference[i]);
dCopyVector3(reference[i] + 8, axes[i]);
dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
// printf ("%f\n", dDOT(r[i], n[i]));
// printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
// reference[i][0],reference[i][1],reference[i][2],
// reference[i][4],reference[i][5],reference[i][6],
// reference[i][8],reference[i][9],reference[i][10]);
radii[i] = radii[i];
phase[i] = 0;
}
ratio = radii[0] / radii[1];
update = 0;
}
for (i = 0 ; i < 2 ; i += 1) {
dReal phase_hat;
dSubtractVectors3 (r[i], c[i], a[i]);
// Transform the (global) contact radius into the gear's
// reference frame.
dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
dMultiply0_331(t, reference[i], s);
// Now simply calculate its angle on the plane relative to the
// x-axis which is the initial contact radius. This will be
// an angle between -pi and pi that is coterminal with the
// actual phase of the wheel. To find the real phase we
// estimate it by adding omega * dt to the old phase and then
// find the closest angle to that, that is coterminal to
// theta.
theta = atan2(t[1], t[0]);
phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
if (phase_hat > M_PI_2) {
if (theta < 0) {
theta += (dReal)(2 * M_PI);
}
theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
} else if (phase_hat < -M_PI_2) {
if (theta > 0) {
theta -= (dReal)(2 * M_PI);
}
theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
}
if (phase_hat - theta > M_PI) {
phase[i] = theta + (dReal)(2 * M_PI);
} else if (phase_hat - theta < -M_PI) {
phase[i] = theta - (dReal)(2 * M_PI);
} else {
phase[i] = theta;
}
dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
}
// Calculate the phase error. Depending on the mode the condition
// is that the distances traveled by each contact point must be
// either equal (chain and sprockets) or opposite (gears).
if (mode == dTransmissionChainDrive) {
delta = (dCalcVectorLength3(r[0]) * phase[0] -
dCalcVectorLength3(r[1]) * phase[1]);
} else {
delta = (dCalcVectorLength3(r[0]) * phase[0] +
dCalcVectorLength3(r[1]) * phase[1]);
}
// When in chain mode a torque reversal, signified by the change
// in sign of the wheel phase difference, has the added effect of
// switching the active chain branch. We must therefore reflect
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dIASSERT(a > 0);
dAddScaledVectors3(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
}
}
// Do not add the constraint if there's backlash and we're in the
// backlash gap.
if (backlash == 0 || fabs(delta) > backlash) {
// The constraint is satisfied iff the absolute velocity of the
// contact point projected onto the tangent of the wheels is equal
// for both gears. This velocity can be calculated as:
//
// u = v + omega x r_c
//
// The constraint therefore becomes:
// (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
// (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], p[i]);
}
dCalcVectorCross3(info->J1a, r[0], l[0]);
dCalcVectorCross3(info->J2a, l[1], r[1]);
dCopyVector3(info->J1l, l[0]);
dCopyNegatedVector3(info->J2l, l[1]);
if (delta > 0) {
if (backlash > 0) {
info->lo[0] = -dInfinity;
info->hi[0] = 0;
}
info->c[0] = -worldFPS * erp * (delta - backlash);
} else {
if (backlash > 0) {
info->lo[0] = 0;
info->hi[0] = dInfinity;
}
info->c[0] = -worldFPS * erp * (delta + backlash);
}
}
info->cfm[0] = cfm;
// printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
// Cache the contact point (in world coordinates) to avoid
// recalculation if requested by the user.
dCopyVector3(contacts[0], c[0]);
dCopyVector3(contacts[1], c[1]);
}
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::vectorFromWorld(const Vec3f &v, Vec3f &result)
{
dVector3 t;
dBodyVectorFromWorld(_BodyID, v.x(), v.y(), v.z(), t);
result.setValue(Vec3f(t[0], t[1], t[2]));
}
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"));
}
}
