} //end MoveAngular
internal void LimitRotation(float timestep)
{
SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
SafeNativeMethods.Quaternion m_rot = new SafeNativeMethods.Quaternion();
bool changed = false;
m_rot.X = rotq.X;
m_rot.Y = rotq.Y;
m_rot.Z = rotq.Z;
m_rot.W = rotq.W;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rotq.X >= m_RollreferenceFrame.X)
{
m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
}
if (rotq.Y >= m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
}
if (rotq.X <= -m_RollreferenceFrame.X)
{
m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
}
if (rotq.Y <= -m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
}
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
m_rot.X = 0;
m_rot.Y = 0;
changed = true;
}
if (changed)
{
SafeNativeMethods.BodySetQuaternion(Body, ref m_rot);
}
}
} // end MoveLinear()
private void MoveAngular(float pTimestep)
{
/*
* private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
* private int m_angularMotorApply = 0; // application frame counter
* private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
* private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
* private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
* private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
* private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
*/
// Get what the body is doing, this includes 'external' influences
SafeNativeMethods.Vector3 angularVelocity = SafeNativeMethods.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
* m_angularMotorVelocity.Y = angularVelocity.Y;
* m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
if (!SafeNativeMethods.BodyIsEnabled(Body))
{
SafeNativeMethods.BodyEnable(Body);
}
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
SafeNativeMethods.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
} //end MoveAngular
}// end Step
private void MoveLinear(float pTimestep, OdeScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
if (!SafeNativeMethods.BodyIsEnabled(Body))
{
SafeNativeMethods.BodyEnable(Body);
}
// add drive to body
Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / pTimestep);
m_lastLinearVelocityVector += (addAmount * 10); // lastLinearVelocityVector is the current body velocity vector?
// This will work temporarily, but we really need to compare speed on an axis
// KF: Limit body velocity to applied velocity?
if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
{
m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
}
if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
{
m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
}
if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
{
m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
}
// decay applied velocity
Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / pTimestep)));
//Console.WriteLine("decay: " + decayfraction);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//Console.WriteLine("actual: " + m_linearMotorDirection);
}
else
{ // requested is not significant
// if what remains of applied is small, zero it.
if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastLinearVelocityVector = Vector3.Zero;
}
}
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
Vector3 grav = Vector3.Zero;
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
SafeNativeMethods.Mass objMass;
SafeNativeMethods.BodyGetMass(Body, out objMass);
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
SafeNativeMethods.Vector3 vel_now = SafeNativeMethods.BodyGetLinearVel(Body);
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
SafeNativeMethods.Vector3 pos = SafeNativeMethods.BodyGetPosition(Body);
// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
Vector3 posChange = new Vector3();
posChange.X = pos.X - m_lastPositionVector.X;
posChange.Y = pos.Y - m_lastPositionVector.Y;
posChange.Z = pos.Z - m_lastPositionVector.Z;
double Zchange = Math.Abs(posChange.Z);
if (m_BlockingEndPoint != Vector3.Zero)
{
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= posChange.X + 1;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= posChange.Y + 1;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= posChange.Z + 1;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += posChange.X + 1;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += posChange.Y + 1;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
}
if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
{
pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
// Check if hovering
if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
{
// If body is aready heigher, use its height as target height
if (pos.Z > m_VhoverTargetHeight)
{
m_VhoverTargetHeight = pos.Z;
}
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{
SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
}
}
else
{
float herr0 = pos.Z - m_VhoverTargetHeight;
// Replace Vertical speed with correction figure if significant
if (Math.Abs(herr0) > 0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else
{
m_dir.Z = 0f;
}
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
}
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
{
//Start Experimental Values
if (Zchange > .3)
{
grav.Z = (float)(grav.Z * 3);
}
if (Zchange > .15)
{
grav.Z = (float)(grav.Z * 2);
}
if (Zchange > .75)
{
grav.Z = (float)(grav.Z * 1.5);
}
if (Zchange > .05)
{
grav.Z = (float)(grav.Z * 1.25);
}
if (Zchange > .025)
{
grav.Z = (float)(grav.Z * 1.125);
}
float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
float postemp = (pos.Z - terraintemp);
if (postemp > 2.5f)
{
grav.Z = (float)(grav.Z * 1.037125);
}
//End Experimental Values
}
if ((m_flags & (VehicleFlag.NO_X)) != 0)
{
m_dir.X = 0;
}
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
{
m_dir.Y = 0;
}
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
{
m_dir.Z = 0;
}
m_lastPositionVector = SafeNativeMethods.BodyGetPosition(Body);
// Apply velocity
SafeNativeMethods.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
// apply gravity force
SafeNativeMethods.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
