//end ProcessVehicleFlags
internal void ProcessTypeChange(WhiteCoreODEPrim parent, Vehicle pType, float timestep)
{
// Set Defaults For Type
m_type = pType;
switch (pType)
{
case Vehicle.TYPE_NONE:
m_linearFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 1000/timestep;
m_linearMotorDecayTimescale = 1000/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 1000/timestep;
m_angularMotorDecayTimescale = 120/timestep;
m_VhoverHeight = 0;
m_VhoverTimescale = 1000/timestep;
m_VehicleBuoyancy = 0;
m_linearDeflectionEfficiency = 1;
m_linearDeflectionTimescale = 1/timestep;
m_angularDeflectionEfficiency = 0;
m_angularDeflectionTimescale = 1000/timestep;
m_bankingEfficiency = 0;
m_bankingMix = 1;
m_bankingTimescale = 1000/timestep;
m_flags = 0;
m_referenceFrame = Quaternion.Identity;
break;
case Vehicle.TYPE_SLED:
m_linearFrictionTimescale = new Vector3(30/timestep, 1/timestep, 1000/timestep);
m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 1000/timestep;
m_linearMotorDecayTimescale = 120/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 1000/timestep;
m_angularMotorDecayTimescale = 120/timestep;
m_VhoverHeight = 0;
m_VhoverEfficiency = 10;
m_VhoverTimescale = 10/timestep;
m_VehicleBuoyancy = 0;
m_linearDeflectionEfficiency = 1;
m_linearDeflectionTimescale = 1/timestep;
m_angularDeflectionEfficiency = 0;
m_angularDeflectionTimescale = 1000/timestep;
m_bankingEfficiency = 0;
m_bankingMix = 1;
m_bankingTimescale = 10/timestep;
m_referenceFrame = Quaternion.Identity;
m_flags &=
~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
break;
case Vehicle.TYPE_CAR:
m_linearFrictionTimescale = new Vector3(100/timestep, 2/timestep, 1000/timestep);
m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 1/timestep;
m_linearMotorDecayTimescale = 60/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 1/timestep;
m_angularMotorDecayTimescale = 0.8f/timestep;
m_VhoverHeight = 0;
m_VhoverEfficiency = 0;
m_VhoverTimescale = 1000/timestep;
m_VehicleBuoyancy = 0;
m_linearDeflectionEfficiency = 1;
m_linearDeflectionTimescale = 2/timestep;
m_angularDeflectionEfficiency = 0;
m_angularDeflectionTimescale = 10/timestep;
m_verticalAttractionEfficiency = 1f;
m_verticalAttractionTimescale = 10f/timestep;
m_bankingEfficiency = -0.2f;
m_bankingMix = 1;
m_bankingTimescale = 1/timestep;
m_referenceFrame = Quaternion.Identity;
m_flags &=
~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT);
m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
break;
case Vehicle.TYPE_BOAT:
m_linearFrictionTimescale = new Vector3(10/timestep, 3/timestep, 2/timestep);
m_angularFrictionTimescale = new Vector3(10/timestep, 10/timestep, 10/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 5/timestep;
m_linearMotorDecayTimescale = 60/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 4/timestep;
m_angularMotorDecayTimescale = 4/timestep;
m_VhoverHeight = 0;
m_VhoverEfficiency = 0.5f;
m_VhoverTimescale = 2/timestep;
m_VehicleBuoyancy = 1;
m_linearDeflectionEfficiency = 0.5f;
m_linearDeflectionTimescale = 3/timestep;
m_angularDeflectionEfficiency = 0.5f;
m_angularDeflectionTimescale = 5/timestep;
m_verticalAttractionEfficiency = 0.5f;
m_verticalAttractionTimescale = 5f/timestep;
m_bankingEfficiency = -0.3f;
m_bankingMix = 0.8f;
m_bankingTimescale = 1/timestep;
m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
VehicleFlag.LIMIT_MOTOR_UP |
VehicleFlag.HOVER_WATER_ONLY);
break;
case Vehicle.TYPE_AIRPLANE:
m_linearFrictionTimescale = new Vector3(200/timestep, 10/timestep, 5/timestep);
m_angularFrictionTimescale = new Vector3(20/timestep, 20/timestep, 20/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 2/timestep;
m_linearMotorDecayTimescale = 60/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 4/timestep;
m_angularMotorDecayTimescale = 4/timestep;
m_VhoverHeight = 0;
m_VhoverEfficiency = 0.5f;
m_VhoverTimescale = 1000/timestep;
m_VehicleBuoyancy = 0;
m_linearDeflectionEfficiency = 0.5f;
m_linearDeflectionTimescale = 3/timestep;
m_angularDeflectionEfficiency = 1;
m_angularDeflectionTimescale = 2/timestep;
m_verticalAttractionEfficiency = 0.9f;
m_verticalAttractionTimescale = 2f/timestep;
m_bankingEfficiency = 1;
m_bankingMix = 0.7f;
m_bankingTimescale = 2;
m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP |
VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
break;
case Vehicle.TYPE_BALLOON:
m_linearFrictionTimescale = new Vector3(5/timestep, 5/timestep, 5/timestep);
m_angularFrictionTimescale = new Vector3(10/timestep, 10/timestep, 10/timestep);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorTimescale = 5/timestep;
m_linearMotorDecayTimescale = 60/timestep;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorTimescale = 6/timestep;
m_angularMotorDecayTimescale = 10/timestep;
m_VhoverHeight = 5;
m_VhoverEfficiency = 0.8f;
m_VhoverTimescale = 10/timestep;
m_VehicleBuoyancy = 1;
m_linearDeflectionEfficiency = 0;
m_linearDeflectionTimescale = 5/timestep;
m_angularDeflectionEfficiency = 0;
m_angularDeflectionTimescale = 5/timestep;
m_verticalAttractionEfficiency = 1f;
m_verticalAttractionTimescale = 100f/timestep;
m_bankingEfficiency = 0;
m_bankingMix = 0.7f;
m_bankingTimescale = 5/timestep;
m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP |
VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
VehicleFlag.HOVER_UP_ONLY);
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
break;
}
}
internal void Step(IntPtr pBody, float pTimestep, WhiteCoreODEPhysicsScene pParentScene, WhiteCoreODEPrim parent)
{
m_body = pBody;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
if (Mass == 0)
GetMass(pBody);
if (Mass == 0)
return; //No noMass vehicles...
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
// scale time so parameters work as before
// until we scale then acording to ode step time
MoveLinear(pTimestep, pParentScene, parent);
MoveAngular(pTimestep, pParentScene, parent);
LimitRotation(pTimestep);
}
internal void Disable(WhiteCoreODEPrim parent)
{
if (!m_enabled || m_type == Vehicle.TYPE_NONE)
return;
m_enabled = false;
parent.ThrottleUpdates = true;
parent.ForceSetVelocity(Vector3.Zero);
parent.ForceSetRotVelocity(Vector3.Zero);
parent.ForceSetPosition(parent.Position);
d.BodySetGravityMode(Body, false);
m_body = IntPtr.Zero;
m_linearMotorDirection = Vector3.Zero;
m_linearMotorDirectionLASTSET = Vector3.Zero;
m_angularMotorDirection = Vector3.Zero;
}
//end SetDefaultsForType
internal void Enable(IntPtr pBody, WhiteCoreODEPrim parent, WhiteCoreODEPhysicsScene pParentScene)
{
if (m_enabled)
return;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
m_body = pBody;
d.BodySetGravityMode(Body, true);
m_enabled = true;
m_lastLinearVelocityVector = parent.Velocity;
m_lastPositionVector = parent.Position;
m_lastAngularVelocity = parent.RotationalVelocity;
parent.ThrottleUpdates = false;
GetMass(pBody);
}
/// <summary>
/// This is called from within simulate but outside the locked portion
/// We need to do our own locking here
/// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
/// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
/// that the space was using.
/// </summary>
/// <param name="prim"></param>
internal void RemovePrimThreadLocked(WhiteCoreODEPrim prim)
{
remCollisionEventReporting(prim);
remActivePrim(prim);
prim.m_frozen = true;
if (prim.prim_geom != IntPtr.Zero)
{
prim.DestroyBody();
prim.IsPhysical = false;
prim.m_targetSpace = IntPtr.Zero;
try
{
if (prim.prim_geom != IntPtr.Zero)
{
d.GeomDestroy(prim.prim_geom);
prim.prim_geom = IntPtr.Zero;
}
else
{
MainConsole.Instance.Warn("[PHYSICS]: Unable to remove prim from physics scene");
}
}
catch (AccessViolationException)
{
MainConsole.Instance.Info(
"[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
}
}
if (!prim.childPrim)
{
lock (prim.childrenPrim)
foreach (WhiteCoreODEPrim prm in prim.childrenPrim)
RemovePrimThreadLocked(prm);
}
lock (_prims)
_prims.Remove(prim);
}
public override PhysicsActor AddPrimShape(UUID primID, uint localID, string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position,
Vector3 size, Quaternion rotation, bool isPhysical, int material, float friction, float restitution,
float gravityMultiplier, float density)
{
WhiteCoreODEPrim newPrim = new WhiteCoreODEPrim(name, physicsType, shape, position, size, rotation, material, friction, restitution, gravityMultiplier, density, this);
newPrim.UUID = primID;
newPrim.LocalID = localID;
if (isPhysical)
newPrim.IsPhysical = isPhysical;
lock (_prims)
_prims.Add(newPrim);
return newPrim;
}
/// <summary>
/// Routine to figure out if we need to mesh this prim with our mesher
/// </summary>
/// <param name="entity"></param>
/// <returns></returns>
internal bool needsMeshing(WhiteCoreODEPrim prim, byte physicalType)
{
PrimitiveBaseShape pbs = prim.Shape;
// most of this is redundant now as the mesher will return null if it cant mesh a prim
// but we still need to check for sculptie meshing being enabled so this is the most
// convenient place to do it for now...
// //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
// //MainConsole.Instance.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
int iPropertiesNotSupportedDefault = 0;
// return true;
if (forceSimplePrimMeshing)
return true;
// let simple spheres use ode sphere object
PrimitiveBaseShape sphere = PrimitiveBaseShape.CreateSphere();
if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1
&& pbs.Scale.X == pbs.Scale.Y && pbs.Scale.X == pbs.Scale.Z && pbs.ProfileHollow == sphere.ProfileHollow &&
pbs.PathBegin == sphere.PathBegin && pbs.PathEnd == sphere.PathEnd &&
pbs.PathCurve == sphere.PathCurve && pbs.HollowShape == sphere.HollowShape &&
pbs.PathRadiusOffset == sphere.PathRadiusOffset && pbs.PathRevolutions == sphere.PathRevolutions &&
pbs.PathScaleY == sphere.PathScaleY && pbs.PathShearX == sphere.PathShearX &&
pbs.PathShearY == sphere.PathShearY && pbs.PathSkew == sphere.PathSkew &&
pbs.PathTaperY == sphere.PathTaperY && pbs.PathTwist == sphere.PathTwist &&
pbs.PathTwistBegin == sphere.PathTwistBegin && pbs.ProfileBegin == sphere.ProfileBegin &&
pbs.ProfileEnd == sphere.ProfileEnd && pbs.ProfileHollow == sphere.ProfileHollow &&
pbs.ProfileShape == sphere.ProfileShape)
return false;
if (pbs.SculptEntry && !meshSculptedPrim)
return false;
else if (pbs.SculptType != (byte) SculptType.Mesh &&
pbs.SculptType != (byte) SculptType.None)
return true; //Sculpty, mesh it
else if (pbs.SculptType == (byte) SculptType.Mesh)
{
//Mesh, we need to see what the prims says to do with it
if (physicalType == (byte) PhysicsShapeType.Prim)
return false; //Supposed to be a simple box, nothing more
else
return true; //Mesh it!
}
// if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
if (!forceSimplePrimMeshing)
{
if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte) Extrusion.Straight)
/*|| (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
&& pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)*/)
{
if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
&& pbs.ProfileHollow == 0
&& pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
&& pbs.PathBegin == 0 && pbs.PathEnd == 0
&& pbs.PathTaperX == 0 && pbs.PathTaperY == 0
&& pbs.PathScaleX == 100 && pbs.PathScaleY == 100
&& pbs.PathShearX == 0 && pbs.PathShearY == 0)
{
#if SPAM
MainConsole.Instance.Warn("NonMesh");
#endif
return false;
}
}
}
if (pbs.ProfileHollow != 0)
iPropertiesNotSupportedDefault++;
else if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
iPropertiesNotSupportedDefault++;
else if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
iPropertiesNotSupportedDefault++;
else if (pbs.PathBegin != 0 || pbs.PathEnd != 0)
iPropertiesNotSupportedDefault++;
else if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
iPropertiesNotSupportedDefault++;
else if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
iPropertiesNotSupportedDefault++;
else if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte) Extrusion.Straight)
iPropertiesNotSupportedDefault++;
else if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1 &&
(pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
iPropertiesNotSupportedDefault++;
else if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
iPropertiesNotSupportedDefault++;
// test for torus
else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.Square &&
pbs.PathCurve == (byte) Extrusion.Curve1)
iPropertiesNotSupportedDefault++;
else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.HalfCircle &&
(pbs.PathCurve == (byte) Extrusion.Curve1 || pbs.PathCurve == (byte) Extrusion.Curve2))
iPropertiesNotSupportedDefault++;
else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.EquilateralTriangle)
{
if (pbs.PathCurve == (byte) Extrusion.Straight)
{
iPropertiesNotSupportedDefault++;
}
else if (pbs.PathCurve == (byte) Extrusion.Curve1)
{
iPropertiesNotSupportedDefault++;
}
}
if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.Circle)
{
if (pbs.PathCurve == (byte) Extrusion.Straight)
{
iPropertiesNotSupportedDefault++;
}
// ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
else if (pbs.PathCurve == (byte) Extrusion.Curve1)
{
iPropertiesNotSupportedDefault++;
}
}
if (iPropertiesNotSupportedDefault == 0)
{
#if SPAM
MainConsole.Instance.Warn("NonMesh");
#endif
return false;
}
#if SPAM
MainConsole.Instance.Debug("Mesh");
#endif
return true;
}
private void changelink(WhiteCoreODEPrim newparent)
{
// If the newly set parent is not null
// create link
if (_parent == null && newparent != null)
{
newparent.ParentPrim(this);
}
// If the newly set parent is null
// destroy link
else if (_parent != null)
{
if (_parent is WhiteCoreODEPrim)
{
if (newparent != _parent)
{
WhiteCoreODEPrim obj = (WhiteCoreODEPrim) _parent;
obj.ChildDelink(this);
childPrim = false;
if (newparent != null)
{
newparent.ParentPrim(this);
}
}
}
}
_parent = newparent;
}
internal void addActivePrim(WhiteCoreODEPrim activatePrim)
{
// adds active prim.. (ones that should be iterated over in collisions_optimized
lock (_activeprimsLock)
{
if (!_activeprims.Contains(activatePrim))
_activeprims.Add(activatePrim);
}
}
internal void BadPrim(WhiteCoreODEPrim whitecoreODEPrim)
{
DeletePrim(whitecoreODEPrim);
//Can't really do this here... as it will be readded before the delete gets called, which is wrong...
//So... leave the prim out there for now
//AddPrimShape(whitecoreODEPrim.ParentEntity);
}
private void SetInStaticSpace(WhiteCoreODEPrim prm)
{
if (prm.m_targetSpace != IntPtr.Zero && prm.m_targetSpace == _parent_scene.space)
{
if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
}
prm.m_targetSpace = _parent_scene.calculateSpaceForGeom(prm._position);
d.SpaceAdd(prm.m_targetSpace, prm.prim_geom);
}
private void ChildSetGeom(WhiteCoreODEPrim odePrim)
{
DestroyBody();
MakeBody();
}
private void ChildRemove(WhiteCoreODEPrim odePrim)
{
// Okay, we have a delinked child.. destroy all body and remake
if (odePrim != this && !childrenPrim.Contains(odePrim))
return;
DestroyBody();
if (odePrim == this)
{
WhiteCoreODEPrim newroot = null;
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
newroot = childrenPrim[0];
childrenPrim.RemoveAt(0);
foreach (WhiteCoreODEPrim prm in childrenPrim)
{
newroot.childrenPrim.Add(prm);
}
childrenPrim.Clear();
}
if (newroot != null)
{
newroot.childPrim = false;
newroot._parent = null;
newroot.MakeBody();
}
}
return;
}
else
{
lock (childrenPrim)
{
childrenPrim.Remove(odePrim);
odePrim.childPrim = false;
odePrim._parent = null;
}
}
MakeBody();
}
// end MoveLinear()
private void MoveAngular(float pTimestep, WhiteCoreODEPhysicsScene _pParentScene, WhiteCoreODEPrim parent)
{
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
// Vector3 angularVelocity = Vector3.Zero;
/*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
{
if (m_userLookAt != Quaternion.Identity)
{
Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq);
camrot.Normalize ();
m_angularMotorVelocity += Vector3.One * camrot;
Console.WriteLine (Vector3.One * camrot);
}
}*/
if (m_angularMotorDirection.LengthSquared() > 1e-6f)
{
m_angularMotorVelocity.X = (m_angularMotorDirection.X - angularVelocity.X)/(m_angularMotorTimescale);
m_angularMotorVelocity.Y = (m_angularMotorDirection.Y - angularVelocity.Y)/(m_angularMotorTimescale);
m_angularMotorVelocity.Z = (m_angularMotorDirection.Z - angularVelocity.Z)/(m_angularMotorTimescale);
m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);
}
else
{
m_angularMotorVelocity = Vector3.Zero;
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
{
float VAservo = 0;
if (Type == Vehicle.TYPE_BOAT)
{
VAservo = 0.2f/(m_verticalAttractionTimescale);
VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
}
else
{
if (parent.LinkSetIsColliding)
VAservo = 0.05f/(m_verticalAttractionTimescale);
else
VAservo = 0.2f/(m_verticalAttractionTimescale);
VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
}
// 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
#region Deflection
//Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
Vector3 PreferredAxisOfMotion =
new Vector3((10*(m_angularDeflectionEfficiency/m_angularDeflectionTimescale)), 0, 0);
PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame);
//Multiply it so that it scales linearly
//deflection = PreferredAxisOfMotion;
//deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));
#endregion
#region Banking
if (m_bankingEfficiency != 0)
{
// 20131224 not used Vector3 dir = Vector3.One*rotq;
float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
//Changes which way it banks in and out of turns
//Use the square of the efficiency, as it looks much more how SL banking works
float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
if (m_bankingEfficiency < 0)
effSquared *= -1; //Keep the negative!
float mix = Math.Abs(m_bankingMix);
if (m_angularMotorVelocity.X == 0)
{
/*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
{
Vector3 axisAngle;
float angle;
parent.Orientation.GetAxisAngle(out axisAngle, out angle);
Vector3 rotatedVel = parent.Velocity * parent.Orientation;
if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
else
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
}*/
}
else
banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X)*4;
if (!parent.LinkSetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
//If they are colliding, we probably shouldn't shove the prim around... probably
{
float angVelZ = m_angularMotorVelocity.X*-1;
/*if(angVelZ > mix)
angVelZ = mix;
else if(angVelZ < -mix)
angVelZ = -mix;*/
//This controls how fast and how far the banking occurs
Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
if (bankingRot.X > 3)
bankingRot.X = 3;
else if (bankingRot.X < -3)
bankingRot.X = -3;
bankingRot *= rotq;
banking += bankingRot;
}
m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
}
#endregion
#region Downward Force
Vector3 downForce = Vector3.Zero;
double Zchange = m_lastposChange.Z;
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force
{
if (Zchange < -0.1f)
{
if (Zchange < -0.3f)
Zchange = -0.3f;
//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
downForce = (new Vector3(0, ((float) Math.Abs(Zchange)*(pTimestep*_pParentScene.PID_P/4)), 0));
downForce *= rotq;
}
}
#endregion
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking + downForce;
#region Linear Motor Offset
//Offset section
if (m_linearMotorOffset != Vector3.Zero)
{
//Offset of linear velocity doesn't change the linear velocity,
// but causes a torque to be applied, for example...
//
// IIIII >>> IIIII
// IIIII >>> IIIII
// IIIII >>> IIIII
// ^
// | Applying a force at the arrow will cause the object to move forward, but also rotate
//
//
// The torque created is the linear velocity crossed with the offset
//Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy
Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET/m_linearMotorOffset);
if (float.IsNaN(torqueFromOffset.X))
torqueFromOffset.X = 0;
if (float.IsNaN(torqueFromOffset.Y))
torqueFromOffset.Y = 0;
if (float.IsNaN(torqueFromOffset.Z))
torqueFromOffset.Z = 0;
d.BodyAddTorque(Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z);
}
#endregion
/*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}*/
// apply friction
// Apply to the body
if (m_lastAngularVelocity.LengthSquared() < 0.0001f)
{
m_lastAngularVelocity = Vector3.Zero;
d.BodySetAngularVel(Body, 0, 0, 0);
m_angularZeroFlag = true;
}
else
{
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
m_angularZeroFlag = false;
m_lastAngularVelocity.X *= (1.0f - 1.0f/m_angularFrictionTimescale.X);
m_lastAngularVelocity.Y *= (1.0f - 1.0f/m_angularFrictionTimescale.Y);
m_lastAngularVelocity.Z *= (1.0f - 1.0f/m_angularFrictionTimescale.Z);
}
}
internal void remActivePrim(WhiteCoreODEPrim deactivatePrim)
{
lock (_activeprimsLock)
_activeprims.Remove(deactivatePrim);
}
// end Step
private void MoveLinear(float pTimestep, WhiteCoreODEPhysicsScene _pParentScene, WhiteCoreODEPrim parent)
{
if (m_linearMotorDirection.LengthSquared() < 0.0001f)
{
m_linearMotorDirection = Vector3.Zero;
m_newVelocity = Vector3.Zero;
}
else
{
Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale);
m_lastLinearVelocityVector += (addAmount);
m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);
// convert requested object velocity to world-referenced vector
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
//Vector3 oldVelocity = m_newVelocity;
m_newVelocity = m_lastLinearVelocityVector*rotq; // apply obj rotation to velocity vector
//if (oldVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
// m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity
}
//if (m_newVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
// m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity
d.Vector3 dpos = d.BodyGetPosition(Body);
Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);
if (!(m_lastPositionVector.X == 0 &&
m_lastPositionVector.Y == 0 &&
m_lastPositionVector.Z == 0))
{
// Only do this if we have a last position
m_lastposChange.X = pos.X - m_lastPositionVector.X;
m_lastposChange.Y = pos.Y - m_lastPositionVector.Y;
m_lastposChange.Z = pos.Z - m_lastPositionVector.Z;
}
#region Blocking Change
if (m_BlockingEndPoint != Vector3.Zero)
{
bool needUpdateBody = false;
if (pos.X >= (m_BlockingEndPoint.X - 1))
{
pos.X -= m_lastposChange.X + 1;
needUpdateBody = true;
}
if (pos.Y >= (m_BlockingEndPoint.Y - 1))
{
pos.Y -= m_lastposChange.Y + 1;
needUpdateBody = true;
}
if (pos.Z >= (m_BlockingEndPoint.Z - 1))
{
pos.Z -= m_lastposChange.Z + 1;
needUpdateBody = true;
}
if (pos.X <= 0)
{
pos.X += m_lastposChange.X + 1;
needUpdateBody = true;
}
if (pos.Y <= 0)
{
pos.Y += m_lastposChange.Y + 1;
needUpdateBody = true;
}
if (needUpdateBody)
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
#endregion
#region Terrain checks
float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
if (pos.Z < terrainHeight - 5)
{
pos.Z = terrainHeight + 2;
m_lastPositionVector = pos;
//Make sure that we don't have an explosion the next frame with the posChange
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
else if (pos.Z < terrainHeight)
m_newVelocity.Z += 1;
#endregion
#region Hover
// 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;
}
float tempHoverHeight = m_VhoverTargetHeight;
if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
{
// If body is aready heigher, use its height as target height
if (pos.Z > tempHoverHeight)
tempHoverHeight = pos.Z;
}
if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2)
{
float h = tempHoverHeight;
float groundHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
if (groundHeight >= tempHoverHeight)
h = groundHeight;
d.BodySetPosition(Body, pos.X, pos.Y, h);
}
}
else
{
float herr0 = pos.Z - tempHoverHeight;
// Replace Vertical speed with correction figure if significant
if (herr0 > 0.01f)
{
m_newVelocity.Z = -((herr0*50.0f)/m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else if (herr0 < -0.01f)
{
m_newVelocity.Z = -((herr0*50f)/m_VhoverTimescale);
}
else
{
m_newVelocity.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
}
#endregion
#region No X,Y,Z
if ((m_flags & (VehicleFlag.NO_X)) != 0)
m_newVelocity.X = 0;
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
m_newVelocity.Y = 0;
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
m_newVelocity.Z = 0;
#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
{
TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t));
}
catch (IndexOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
catch (ArgumentOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
m_forcelist = new List<Vector3>();
}
// force to deltaV
m_newVelocity += TaintedForce*(pTimestep/Mass);
#endregion
#region Deflection
//Forward is the prefered direction
/*Vector3 deflectionamount = m_newVelocity / (m_linearDeflectionTimescale / pTimestep);
//deflectionamount *= m_linearDeflectionEfficiency;
if (deflectionamount != Vector3.Zero)
{
}
Vector3 deflection = Vector3.One / deflectionamount;
m_newVelocity /= deflection;*/
#endregion
#region limitations
if (m_newVelocity.LengthSquared() > 1e6f)
{
m_newVelocity /= m_newVelocity.Length();
m_newVelocity *= 1000f;
}
else if (m_newVelocity.LengthSquared() < 1e-6f)
m_newVelocity = Vector3.Zero;
#endregion
m_lastPositionVector = parent.Position;
float grav = -1*Mass*pTimestep;
// Apply velocity
if (m_newVelocity != Vector3.Zero)
{
if ((Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED) && !parent.LinkSetIsColliding)
{
//Force ODE gravity here!!!
}
else
d.BodySetLinearVel(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z + grav);
}
// apply friction
m_lastLinearVelocityVector.X *= (1.0f - 1/m_linearFrictionTimescale.X);
m_lastLinearVelocityVector.Y *= (1.0f - 1/m_linearFrictionTimescale.Y);
m_lastLinearVelocityVector.Z *= (1.0f - 1/m_linearFrictionTimescale.Z);
}
// I'm the parent
// prim is the child
public void ParentPrim(WhiteCoreODEPrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
if (this.m_localID != prim.m_localID)
{
DestroyBody();
lock (childrenPrim)
{
foreach (WhiteCoreODEPrim prm in prim.childrenPrim.Where(prm => !childrenPrim.Contains(prm)))
{
childrenPrim.Add(prm);
}
if (!childrenPrim.Contains(prim)) // must allow full reconstruction
childrenPrim.Add(prim);
}
//Remove old children
prim.childrenPrim.Clear();
prim.childPrim = true;
prim._parent = this;
if (prim.Body != IntPtr.Zero)
{
prim.DestroyBody(); // don't loose bodies around
prim.Body = IntPtr.Zero;
}
MakeBody(); // full nasty reconstruction
}
}