/// <summary>
/// Called to queue a change to a prim
/// to use in place of old taint mechanism so changes do have a time sequence
/// </summary>
public void AddChange(AuroraPhysXPrim prim,changes what,Object arg)
{
AODEchangeitem item = new AODEchangeitem();
item.prim = prim;
item.what = what;
item.arg = arg;
// lock (ChangesQueue)
{
ChangesQueue.Enqueue(item);
}
}
public void remActivePrim(AuroraPhysXPrim deactivatePrim)
{
lock (_activeprimsLock)
{
_activeprims.Remove(deactivatePrim);
}
}
/// <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>
public void RemovePrimThreadLocked(AuroraPhysXPrim prim)
{
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
lock (prim)
{
remCollisionEventReporting(prim);
remActivePrim(prim);
lock (ode)
{
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
{
m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
}
}
catch (AccessViolationException)
{
m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
}
}
if (!prim.childPrim)
{
lock (prim.childrenPrim)
{
foreach (AuroraPhysXPrim prm in prim.childrenPrim)
{
RemovePrimThreadLocked (prm);
}
}
}
lock (_prims)
_prims.Remove (prim);
}
}
}
public override PhysicsObject AddPrimShape(ISceneChildEntity entity)
{
bool isPhysical = ((entity.ParentEntity.RootChild.Flags & PrimFlags.Physics) != 0);
bool isPhantom = ((entity.ParentEntity.RootChild.Flags & PrimFlags.Phantom) != 0);
bool physical = isPhysical & !isPhantom;
/*IOpenRegionSettingsModule WSModule = entity.ParentEntity.Scene.RequestModuleInterface<IOpenRegionSettingsModule> ();
if (WSModule != null)
if (!WSModule.AllowPhysicalPrims)
physical = false;*/
AuroraPhysXPrim newPrim;
newPrim = new AuroraPhysXPrim (entity, this, false, ode);
if(physical)
newPrim.IsPhysical = physical;
lock (_prims)
_prims.Add(newPrim);
return newPrim;
}
public void addActivePrim(AuroraPhysXPrim activatePrim)
{
// adds active prim.. (ones that should be iterated over in collisions_optimized
lock (_activeprimsLock)
{
if (!_activeprims.Contains(activatePrim))
_activeprims.Add(activatePrim);
//else
// m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
}
}
} // end MoveLinear()
private void MoveAngular (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraPhysXPrim 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_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 * pTimestep));
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
// 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
#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) * pTimestep), 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)
{
Vector3 angularMotorVelocity = new Vector3 ();
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
// velocity may still be acheived.
Vector3 dir = Vector3.One * rotq;
float mult = /*dir.X > 0 ?*/ -1f /*: 1*/;
mult *= m_bankingMix;//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!
banking.Z += (effSquared * (mult)) * (angularMotorVelocity.X);
m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
if (Math.Abs(m_lastAngularVelocity.Z) > m_bankingMix)
{
Vector3 bankingRot = new Vector3 (m_lastAngularVelocity.Z * (effSquared * 10 * (m_bankingMix * (-1))), 0, 0);
bankingRot *= rotq;
banking += bankingRot;
//m_angularMotorDirection.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency);
//m_linearMotorDirectionLASTSET.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency * 100);
}
}
}
#endregion
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking;
if (!m_lastAngularVelocity.ApproxEquals (Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
#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) / 10;
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
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
}
} // 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()
internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraPhysXPrim 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;
MoveLinear (pTimestep, pParentScene, parent);
MoveAngular (pTimestep, pParentScene, parent);
LimitRotation(pTimestep);
// WE deal with updates
parent.RequestPhysicsterseUpdate();
} // end Step
internal void Disable(AuroraPhysXPrim parent)
{
if (!m_enabled)
return;
m_enabled = false;
parent.SetMaterial((int)m_previousMaterial, false); //Revert to the original
parent.ThrottleUpdates = true;
//d.BodyDisable(Body);
m_linearMotorDirection = Vector3.Zero;
m_linearMotorDirectionLASTSET = Vector3.Zero;
m_angularMotorDirection = Vector3.Zero;
}
}//end SetDefaultsForType
internal void Enable(IntPtr pBody, AuroraPhysXPrim parent, AuroraODEPhysicsScene pParentScene)
{
if (m_enabled)
return;
m_enabled = true;
m_previousMaterial = (Material)parent.m_material;
parent.SetMaterial((int)Material.Glass, false); //This seems to happen in SL... and its needed for here
parent.ThrottleUpdates = false;
m_body = pBody;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
GetMass (pBody);
}
