} // end Step
private void MoveLinear (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraPhysXPrim parent)
{
if (!m_linearMotorDirection.ApproxEquals (Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
// add drive to body
Vector3 addAmount = m_linearMotorDirection / ((m_linearMotorTimescale) / pTimestep);
m_lastLinearVelocityVector += (addAmount); // 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))));
decayfraction.Z = ((1 / (m_linearMotorDecayTimescale / (pTimestep * pTimestep))));
//Console.WriteLine("decay: " + decayfraction);
Vector3 decayAmt = (m_linearMotorDirection * decayfraction);
m_linearMotorDirection -= decayAmt;
//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;
d.Quaternion rot = d.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.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * Mass * (float)parent.ParentEntity.GravityMultiplier * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel (Body);
if(m_lastLinearVelocityVector.Z == 0 && m_verticalAttractionTimescale == 0)
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
else if(Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON)
m_dir.Z += vel_now.Z;
d.Vector3 pos = d.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;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= posChange.Y + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= posChange.Z + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += posChange.X + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += posChange.Y + 1;
d.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;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
// Check if hovering
if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
m_VhoverTargetHeight = (float)_pParentScene.GetWaterLevel (pos.X, pos.Y) + m_VhoverHeight;
}
if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y) + m_VhoverHeight;
}
if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if ((m_flags & 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_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{
d.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.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;
#region Limit Motor Up
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force
{
if (Zchange > -0.1f)
{
//Requires idea of 'up', so use reference frame to rotate it
//Add to the X, because that will normally tilt the vehicle downward (if its rotated, it'll be rotated by the ref. frame
grav += (new Vector3 (0, 0, ((float)Math.Abs (Zchange) * (pTimestep * -_pParentScene.PID_D * _pParentScene.PID_D))));
}
}
#endregion
#region Deal with tainted forces
// 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.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
Vector3 TaintedForce = new Vector3 ();
if (m_forcelist.Count != 0)
{
try
{
for (int i = 0; i < m_forcelist.Count; i++)
{
TaintedForce = TaintedForce + (m_forcelist[i] * 100);
}
}
catch (IndexOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
catch (ArgumentOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
m_forcelist = new List<Vector3> ();
}
#endregion
#region Deflection
//Forward is the prefered direction
/*Vector3 deflectionamount = m_dir / (m_linearDeflectionTimescale / pTimestep);
//deflectionamount *= m_linearDeflectionEfficiency;
if (deflectionamount != Vector3.Zero)
{
}
Vector3 deflection = Vector3.One / deflectionamount;
m_dir /= deflection;*/
#endregion
m_lastPositionVector = d.BodyGetPosition (Body);
#region limitations
if (Math.Abs (m_dir.X) > 1000 ||
Math.Abs (m_dir.Y) > 1000 ||
Math.Abs (m_dir.Z) > 1000)
{
//This vehicle is f***ed
parent.RaiseOutOfBounds (parent.Position);
parent._zeroFlag = true;
parent.m_disabled = true;
parent.m_frozen = true;
return;
}
#endregion
// Apply velocity
d.BodySetLinearVel (Body, m_dir.X, m_dir.Y, m_dir.Z);
// apply gravity force
d.BodyAddForce (Body, grav.X, grav.Y, grav.Z);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()