Exemple #1
0
/*
 * 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]);	
}
Exemple #2
0
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;
}
Exemple #4
0
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];
}
Exemple #6
0
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;
}
Exemple #7
0
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);
        }
    }
}
Exemple #8
0
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;
}
Exemple #10
0
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]);
}
Exemple #11
0
/* 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);
}
Exemple #12
0
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]);
}
Exemple #13
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 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"));
  }
}