public PhysicsScene GetScene(String sceneIdentifier)
{
if (_mScene == null)
{
// Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
// http://opensimulator.org/mantis/view.php?id=2750).
d.InitODE();
_mScene = new AuroraODEPhysicsScene(ode, sceneIdentifier);
}
return _mScene;
}
public PhysicsScene GetScene(String sceneIdentifier)
{
if (_mScene == null)
{
if (!m_initialized) //Only initialize ode once!
{
// Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
// http://opensimulator.org/mantis/view.php?id=2750).
d.SetODEFile();
d.InitODE();
m_initialized = true;
}
_mScene = new AuroraODEPhysicsScene(sceneIdentifier);
}
return _mScene;
}
public bool DoAChange(AuroraODEPhysicsScene.changes what, object arg)
{
if (m_frozen && what != AuroraODEPhysicsScene.changes.Add && what != AuroraODEPhysicsScene.changes.Remove)
return false;
if (prim_geom == IntPtr.Zero && what != AuroraODEPhysicsScene.changes.Add &&
what != AuroraODEPhysicsScene.changes.Remove)
{
m_frozen = true;
return false;
}
// nasty switch
switch (what)
{
case AuroraODEPhysicsScene.changes.Add:
changeadd();
break;
case AuroraODEPhysicsScene.changes.Remove:
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildRemove(this);
}
else
ChildRemove(this);
RemoveGeom();
m_targetSpace = IntPtr.Zero;
return true;
case AuroraODEPhysicsScene.changes.Link:
AuroraODEPrim tmp = (AuroraODEPrim)arg;
changelink(tmp);
break;
case AuroraODEPhysicsScene.changes.DeLink:
changelink(null);
break;
case AuroraODEPhysicsScene.changes.Position:
changePosition((Vector3)arg);
break;
case AuroraODEPhysicsScene.changes.Orientation:
changeOrientation((Quaternion)arg);
break;
case AuroraODEPhysicsScene.changes.PosOffset:
donullchange();
break;
case AuroraODEPhysicsScene.changes.OriOffset:
donullchange();
break;
case AuroraODEPhysicsScene.changes.Velocity:
changevelocity(arg);
break;
case AuroraODEPhysicsScene.changes.Acceleration:
changeacceleration(arg);
break;
case AuroraODEPhysicsScene.changes.AngVelocity:
changeangvelocity(arg);
break;
case AuroraODEPhysicsScene.changes.Force:
changeforce(arg);
break;
case AuroraODEPhysicsScene.changes.Torque:
changeSetTorque(arg);
break;
case AuroraODEPhysicsScene.changes.AddForce:
changeAddForce(arg);
break;
case AuroraODEPhysicsScene.changes.AddAngForce:
changeAddAngularForce(arg);
break;
case AuroraODEPhysicsScene.changes.AngLock:
changeAngularLock(arg);
break;
case AuroraODEPhysicsScene.changes.Size:
changesize(arg);
break;
case AuroraODEPhysicsScene.changes.Shape:
changeshape(arg);
break;
case AuroraODEPhysicsScene.changes.CollidesWater:
changefloatonwater(arg);
break;
case AuroraODEPhysicsScene.changes.VolumeDtc:
changevoldtc(arg);
break;
case AuroraODEPhysicsScene.changes.Physical:
changePhysicsStatus((bool)arg);
break;
case AuroraODEPhysicsScene.changes.Selected:
changeSelectedStatus((bool)arg);
break;
case AuroraODEPhysicsScene.changes.disabled:
changedisable();
break;
// case changes.buildingrepresentation:
// m_buildingRepresentation = false;
// break;
case AuroraODEPhysicsScene.changes.blockphysicalreconstruction:
if ((bool)arg)
DestroyBody();
else
{
m_blockPhysicalReconstruction = false;
if (!childPrim)
MakeBody();
}
if (!childPrim && childrenPrim.Count > 0)
{
foreach (AuroraODEPrim prm in childrenPrim)
prm.BlockPhysicalReconstruction = (bool)arg;
}
break;
case AuroraODEPhysicsScene.changes.VehicleType:
changeVehicleType((int)arg);
break;
case AuroraODEPhysicsScene.changes.VehicleFloatParam:
strVehicleFloatParam strf = (strVehicleFloatParam)arg;
changeVehicleFloatParam(strf.param, strf.value);
break;
case AuroraODEPhysicsScene.changes.VehicleVectorParam:
strVehicleVectorParam strv = (strVehicleVectorParam)arg;
changeVehicleVectorParam(strv.param, strv.value);
break;
case AuroraODEPhysicsScene.changes.VehicleRotationParam:
strVehicleQuartParam strq = (strVehicleQuartParam)arg;
changeVehicleRotationParam(strq.param, strq.value);
break;
case AuroraODEPhysicsScene.changes.VehicleFlags:
strVehicleBoolParam strb = (strVehicleBoolParam)arg;
changeVehicleFlags(strb.param, strb.value);
break;
case AuroraODEPhysicsScene.changes.VehicleSetCameraPos:
changeSetCameraPos((Quaternion)arg);
break;
case AuroraODEPhysicsScene.changes.Null:
donullchange();
break;
default:
donullchange();
break;
}
return false;
}
public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, float Density)
{
m_vehicle = new AuroraODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
if (!pos.IsFinite())
{
pos = new Vector3((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f),
parent_scene.GetTerrainHeightAtXY((parent_scene.Region.RegionSizeX * 0.5f), (parent_scene.Region.RegionSizeY * 0.5f)));
m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
}
_position = pos;
fakepos = false;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE * 50f);
m_density = Density / 100;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
prev_geom = IntPtr.Zero;
if (!size.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
}
_orientation = rotation;
fakeori = false;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
m_isphysical = false;
else
{
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (m_isphysical)
m_targetSpace = _parent_scene.space;
}
m_primName = primName;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
AddChange(changes.Add, null);
}
public AuroraODERayCastRequestManager(AuroraODEPhysicsScene pScene)
{
m_scene = pScene;
nearCallback = near;
}
public AuroraODEPrim(string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position, Vector3 size, Quaternion rotation,
int material, float friction, float restitution, float gravityMultiplier, float density, AuroraODEPhysicsScene parent_scene)
{
m_vehicle = new AuroraODEDynamics();
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE*50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE*50f);
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
_name = name;
PhysicsShapeType = physicsType;
_size = size;
_position = position;
fakepos = 0;
_orientation = rotation;
fakeori = 0;
_pbs = shape;
_parent_scene = parent_scene;
m_targetSpace = IntPtr.Zero;
/*
m_isphysical = pisPhysical;
if (m_isphysical)
m_targetSpace = _parent_scene.space;
*/
m_isphysical = false;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
hasOOBoffsetFromMesh = false;
_triMeshData = IntPtr.Zero;
SetMaterial(material, friction, restitution, gravityMultiplier, density);
CalcPrimBodyData();
_parent_scene.AddSimulationChange(() => changeadd());
}
public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation, Vector3 size)
{
m_uuid = UUID.Random();
m_taintRotation = rotation;
// _orientation = rotation;
// _lastorientation = rotation;
if (pos.IsFinite())
{
if (pos.Z > 9999999f || pos.Z <-90f)
{
// pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
pos.Z = parent_scene.GetTerrainHeightAtXY(parent_scene.Region.RegionSizeX * 0.5f, parent_scene.Region.RegionSizeY * 0.5f) + 5.0f;
}
_position = pos;
m_taintPosition.X = pos.X;
m_taintPosition.Y = pos.Y;
m_taintPosition.Z = pos.Z;
}
else
{
_position.X = (float)parent_scene.Region.RegionSizeX * 0.5f;
_position.Y = (float)parent_scene.Region.RegionSizeY * 0.5f;
_position.Z = parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
m_taintPosition.X = _position.X;
m_taintPosition.Y = _position.Y;
m_taintPosition.Z = _position.Z;
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
_parent_scene = parent_scene;
CAPSULE_RADIUS = parent_scene.avCapRadius;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
CAPSULE_LENGTH = (size.Z * 1.1f) - CAPSULE_RADIUS * 2.0f;
if ((m_collisionFlags & CollisionCategories.Land) == 0)
AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS;
else
AvatarHalfsize = CAPSULE_LENGTH * 0.5f + CAPSULE_RADIUS - 0.3f;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
m_name = avName;
}
public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
Vector3 size)
{
m_uuid = UUID.Random();
_parent_scene = parent_scene;
m_taintRotation = rotation;
if (pos.IsFinite())
{
if (pos.Z > 9999999f || pos.Z < -90f)
{
pos.Z =
_parent_scene.GetTerrainHeightAtXY(_parent_scene.Region.RegionSizeX * 0.5f,
_parent_scene.Region.RegionSizeY * 0.5f) + 5.0f;
}
_position = pos;
}
else
{
_position.X = _parent_scene.Region.RegionSizeX * 0.5f;
_position.Y = _parent_scene.Region.RegionSizeY * 0.5f;
_position.Z = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
MainConsole.Instance.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
CAPSULE_RADIUS = _parent_scene.avCapRadius;
CAPSULE_LENGTH = (size.Z*1.1f) - CAPSULE_RADIUS*2.0f;
AvatarHalfsize = CAPSULE_LENGTH*0.5f + CAPSULE_RADIUS;
m_isPhysical = false; // current status: no ODE information exists
_parent_scene.AddSimulationChange(() => RebuildAvatar());
m_name = avName;
}
// end Step
private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
{
d.Vector3 dvel_now = d.BodyGetLinearVel(Body);
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
m_newVelocity = m_lastLinearVelocityVector;
m_newVelocity *= rotq; // apply obj rotation to velocity vector
}
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
double Zchange = Math.Abs(m_lastposChange.Z);
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, tempHoverHeight);
}
}
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
{
#if (!ISWIN)
foreach (Vector3 vector3 in m_forcelist)
TaintedForce = TaintedForce + (vector3);
#else
TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t));
#endif
}
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
// add Gravity andBuoyancy
m_newVelocity.Z += pTimestep*_pParentScene.gravityz*parent.ParentEntity.GravityMultiplier*
(1f - m_VehicleBuoyancy);
m_lastPositionVector = parent.Position;
// Apply velocity
d.BodySetLinearVel(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z);
// 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);
}
internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim 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);
}
//end SetDefaultsForType
internal void Enable(IntPtr pBody, AuroraODEPrim parent, AuroraODEPhysicsScene pParentScene)
{
if (m_enabled)
return;
m_enabled = true;
m_lastLinearVelocityVector = parent.Velocity;
m_lastPositionVector = parent.Position;
m_lastAngularVelocity = parent.RotationalVelocity;
parent.ThrottleUpdates = false;
m_body = pBody;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
GetMass(pBody);
}
public AuroraODECharacter(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
Vector3 size)
{
m_uuid = UUID.Random();
_parent_scene = parent_scene;
m_taintRotation = rotation;
if (pos.IsFinite())
{
if (pos.Z > 9999999f || pos.Z < -90f)
{
pos.Z =
_parent_scene.GetTerrainHeightAtXY(_parent_scene.Region.RegionSizeX*0.5f,
_parent_scene.Region.RegionSizeY*0.5f) + 5.0f;
}
_position = pos;
}
else
{
_position.X = _parent_scene.Region.RegionSizeX*0.5f;
_position.Y = _parent_scene.Region.RegionSizeY*0.5f;
_position.Z = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + 10f;
MainConsole.Instance.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
m_isPhysical = false; // current status: no ODE information exists
Size = size;
m_name = avName;
}
internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim parent)
{
m_body = pBody;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
MoveLinear(pTimestep, pParentScene);
MoveAngular(pTimestep, pParentScene);
LimitRotation(pTimestep);
/*if (!parent.m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
parent.Amotor != IntPtr.Zero)
{
d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 9000f);
d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.FMax, int.MaxValue);
d.Vector3 avel2 = d.BodyGetAngularVel(Body);
if (parent.m_angularlock.X == 0)
avel2.X = 0;
if (parent.m_angularlock.Y == 0)
avel2.Y = 0;
if (parent.m_angularlock.Z == 0)
avel2.Z = 0;
d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
d.BodySetAngularDamping(Body, 1);
d.BodySetTorque(Body, 0, 0, 0);
}*/
// WE deal with updates
parent.RequestPhysicsterseUpdate();
} // end Step
public void AddChange(AuroraODEPhysicsScene.changes what, object arg)
{
_parent_scene.AddChange(this, what, arg);
}
private Vector3 showposition; // a temp hack for now rest of code expects position to be changed immediately
public AuroraODEPrim(ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical)
{
m_vehicle = new AuroraODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE * 50f);
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
_size = entity.Scale;
_position = entity.AbsolutePosition;
fakepos = 0;
_orientation = entity.GetWorldRotation();
fakeori = 0;
_pbs = entity.Shape;
_parent_entity = entity;
_parent_scene = parent_scene;
m_targetSpace = IntPtr.Zero;
/*
m_isphysical = pisPhysical;
if (m_isphysical)
m_targetSpace = _parent_scene.space;
*/
m_isphysical = false;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
hasOOBoffsetFromMesh = false;
_triMeshData = IntPtr.Zero;
CalcPrimBodyData();
_parent_scene.AddSimulationChange(() => changeadd());
}
// end MoveLinear()
private void MoveAngular(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim 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);
}
// 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
#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)
{
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);
banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X);
m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
if (!parent.LinkSetIsColliding /* && Math.Abs(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;
}
}
#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);
}
}
public bool setMesh(AuroraODEPhysicsScene parent_scene, IMesh mesh)
{
// This sleeper is there to moderate how long it takes between
// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
//Thread.Sleep(10);
//Kill Body so that mesh can re-make the geom
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
DestroyBody();
}
}
IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
// Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
// Also fixed, needs release after usage
if (vertexCount == 0 || indexCount == 0)
{
MainConsole.Instance.WarnFormat("[PHYSICS]: Got invalid mesh on prim at <{0},{1},{2}>. It can be a sculpt with alpha channel in map. Replacing it by a small box.", _position.X, _position.Y, _position.Z);
_size.X = 0.01f;
_size.Y = 0.01f;
_size.Z = 0.01f;
return false;
}
primOOBoffset = mesh.GetCentroid();
hasOOBoffsetFromMesh = true;
mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
if (m_MeshToTriMeshMap.ContainsKey(mesh.Key))
{
_triMeshData = m_MeshToTriMeshMap[mesh.Key];
}
else
{
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount,
triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
m_MeshToTriMeshMap[mesh.Key] = _triMeshData;
}
try
{
if (prim_geom == IntPtr.Zero)
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
}
}
catch (AccessViolationException)
{
MainConsole.Instance.Error("[PHYSICS]: MESH LOCKED");
return false;
}
return true;
}
}//end SetDefaultsForType
internal void Enable(IntPtr pBody, AuroraODEPrim parent, AuroraODEPhysicsScene pParentScene)
{
if (m_enabled)
return;
m_enabled = true;
m_previousMaterial = (Material)parent.m_material;
parent.SetMaterial((int)Material.Glass); //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;
d.Mass mass;
d.BodyGetMass(pBody, out mass);
Mass = mass.mass;
Mass *= 2;
}
public ODESpecificAvatar(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation,
Vector3 size) : base(avName, parent_scene, pos, rotation, size)
{
base._parent_ref = this;
}
internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim parent)
{
m_body = pBody;
if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
MoveLinear(pTimestep, pParentScene);
MoveAngular(pTimestep, pParentScene);
LimitRotation(pTimestep);
// WE deal with updates
parent.RequestPhysicsterseUpdate();
} // end Step
public AuroraODEPrim (ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical, CollisionLocker dode)
{
m_vehicle = new AuroraODEDynamics ();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE * 50f);
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
prev_geom = IntPtr.Zero;
_size = entity.Scale;
_position = entity.AbsolutePosition;
fakepos = false;
_orientation = entity.GetWorldRotation ();
fakeori = false;
_pbs = entity.Shape;
_parent_entity = entity;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (m_isphysical)
m_targetSpace = _parent_scene.space;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
AddChange (changes.Add, null);
}
} // end Step
private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene)
{
d.Vector3 pos = d.BodyGetPosition (Body);
d.Vector3 oldPos = pos;
if (m_lastPositionVector.X != pos.X ||
m_lastPositionVector.Y != pos.Y ||
m_lastPositionVector.Z != pos.Z)
{
m_lastPositionVector = d.BodyGetPosition (Body);
m_lastAngularVelocity = new Vector3 ((float)d.BodyGetAngularVel (Body).X, (float)d.BodyGetAngularVel (Body).Y, (float)d.BodyGetAngularVel (Body).Z);
}
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 * m_linearMotorDecayTimescale / (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;
}
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;
}
m_linearMotorDirection = 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;
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel(Body);
m_dir.Z += (float)vel_now.Z; // Preserve the accumulated falling velocity
#region Blocking End Points
//This makes sure that the vehicle doesn't leave the defined limits of position
if (m_BlockingEndPoint != Vector3.Zero)
{
Vector3 posChange = new Vector3();
posChange.X = (float)(pos.X - m_lastPositionVector.X);
posChange.Y = (float)(pos.Y - m_lastPositionVector.Y);
posChange.Z = (float)(pos.Z - m_lastPositionVector.Z);
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
pos.X -= posChange.X + 1;
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
pos.Y -= posChange.Y + 1;
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
pos.Z -= posChange.Z + 1;
if (pos.X <= 0)
pos.X += posChange.X + 1;
if (pos.Y <= 0)
pos.Y += posChange.Y + 1;
}
#endregion
// 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 = (float)_pParentScene.GetWaterLevel((float)pos.X, (float)pos.Y) + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)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 already heigher, use its height as target height
if (pos.Z > m_VhoverTargetHeight)
m_VhoverTargetHeight = (float)pos.Z;
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{
if ((pos.Z - (pos.Z - m_VhoverTargetHeight)) >= _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y))
pos.Z = m_VhoverTargetHeight;
}
}
else
{
// m_VhoverEfficiency - 0=boucy, 1=Crit.damped
// m_VhoverTimescale - time to acheive height
float herr0 = (float)pos.Z - m_VhoverTargetHeight;
// Replace Vertical speed with correction figure if significant
if (Math.Abs(herr0) > 0.01f)
{
//Note: we use 1.05 because it doesn't disappear completely, only very critically damped
m_dir.Z = (float)((-((herr0 * pTimestep * 50.0f) / m_VhoverTimescale)) * (1.05 - m_VhoverEfficiency));
}
else
//Too small, zero it.
m_dir.Z = 0f;
}
}
//Do this here, because it shouldn't clear out gravity or the tainted forces (not part of vehicle physics)
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 = d.BodyGetPosition(Body);
#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 PreferredAxisOfMotion = new Vector3 (1 + 1 * (m_linearDeflectionEfficiency / m_linearDeflectionTimescale) * pTimestep * pTimestep * pTimestep, 1, 1);
PreferredAxisOfMotion *= m_referenceFrame;
//Multiply it so that it scales linearly
m_dir *= PreferredAxisOfMotion;
#endregion
if (Mass == 0)
{
d.Mass mass;
d.BodyGetMass(m_body, out mass);
Mass = mass.mass;
Mass *= 2;
}
//No Setting forces, only velocity! Forces are NOT recommended to be used by the ODE manual
//d.BodySetForce(Body, _pParentScene.gravityx + TaintedForce.X,
// _pParentScene.gravityy + TaintedForce.Y,
// (_pParentScene.gravityz * Mass * (1f - m_VehicleBuoyancy)) + TaintedForce.Z);
//d.BodySetForce(Body, _pParentScene.gravityx + TaintedForce.X + (m_dir.X * 10000),
// _pParentScene.gravityy + TaintedForce.Y + (m_dir.Y * 10000),
// (_pParentScene.gravityz * Mass * (1f - m_VehicleBuoyancy)) + TaintedForce.Z + (m_dir.Z * 10000));
m_dir += TaintedForce;
//Uses the square to make bouyancy more effective as in SL, as it seems to effect gravity more the higher the value is
//This check helps keep things from being pushed into the ground and the consequence of being shoved back out
m_dir.Z += ((_pParentScene.gravityz * (float)Mass) * ((((1 - m_VehicleBuoyancy) * (1 - m_VehicleBuoyancy))) * pTimestep));
d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
//Check for changes and only set it once
if (pos.X != oldPos.X || pos.Y != oldPos.Y || pos.Z != oldPos.Z)
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
// apply friction
// note: seems more effective with how SL does this with the square
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / (pTimestep * pTimestep));
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
public AuroraODEPrim (ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical)
{
m_vehicle = new AuroraODEDynamics ();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
// correct for changed timestep
PID_D /= (parent_scene.ODE_STEPSIZE * 50f); // original ode fps of 50
PID_G /= (parent_scene.ODE_STEPSIZE * 50f);
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
_size = entity.Scale;
_position = entity.AbsolutePosition;
fakepos = 0;
_orientation = entity.GetWorldRotation ();
fakeori = 0;
_pbs = entity.Shape;
_parent_entity = entity;
_parent_scene = parent_scene;
m_targetSpace = IntPtr.Zero;
/*
m_isphysical = pisPhysical;
if (m_isphysical)
m_targetSpace = _parent_scene.space;
*/
m_isphysical = false;
m_forceacc = Vector3.Zero;
m_angularforceacc = Vector3.Zero;
hasOOBoffsetFromMesh = false;
_triMeshData = IntPtr.Zero;
CalcPrimBodyData();
m_UpdateTimecntr = 0;
m_UpdateFPScntr = 2.5f * parent_scene.StepTime; // this parameter needs retunning and possible came from ini file
if (m_UpdateTimecntr > .1f) // try to keep it under 100ms
m_UpdateTimecntr = .1f;
AddChange (changes.Add, null);
}
} // end MoveLinear()
private void MoveAngular(float pTimestep, AuroraODEPhysicsScene _pParentScene)
{
/*
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
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
{
if (m_userLookAt != Vector3.Zero)
{
/*m_lastCameraRotation = llRotBetween(new Vector3(d.BodyGetPosition(m_body).X, d.BodyGetPosition(m_body).Y, d.BodyGetPosition(m_body).Z), m_userLookAt);
m_lastCameraRotation *= 10;
Vector3 move = ToEuler(m_lastCameraRotation);
//move.Z *= (-1);
//move *= new Quaternion(d.BodyGetQuaternion(Body).X, d.BodyGetQuaternion(Body).Y, d.BodyGetQuaternion(Body).Z, d.BodyGetQuaternion(Body).W);
move.Z *= (float)(-2 * Math.PI);
move.Y = 0;
move.X = 0;
m_angularMotorVelocity += move / pTimestep;*/
m_userLookAt.Z = m_userLookAt.X * 10;
m_userLookAt.X = 0;
m_userLookAt.Y = 0;
m_angularMotorVelocity += m_userLookAt;
Console.WriteLine(m_userLookAt.Z);
//Console.WriteLine(move.Z);
}
}
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
//Add the mass, if the vehicle is attempting to turn, it does matter how much it weighs
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) * (float)Mass / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) * (float)Mass / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) * (float)Mass / (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
{
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
}
if (m_angularMotorVelocity.ApproxEquals(Vector3.Zero, 0.01f))
m_angularMotorVelocity = Vector3.Zero;
d.Quaternion rot = d.BodyGetQuaternion(Body);
Vector3 vertattr = Vector3.Zero;
Vector3 bank = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
#region Vertical attractor section
if (m_verticalAttractionTimescale < 300)
{
Quaternion rotqq = new Quaternion((float)rot.X + m_referenceFrame.X,
(float)rot.Y + m_referenceFrame.Y,
(float)rot.Z + m_referenceFrame.Z,
(float)rot.W); // rotq = rotation of object
m_angularMotorVelocity *= rotqq;
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
Quaternion rotq = new Quaternion((float)rot.X, (float)rot.Y, (float)rot.Z, (float)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;
// 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 using square-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += (float)(bounce * angularVelocity.X);
vertattr.Y += (float)(bounce * angularVelocity.Y);
#region Banking
//X is the part that deals with banking
// NOTES on banking SEE http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial#Banking ----
//VEHICLE_BANKING_EFFICIENCY - slider between -1 (leans out of turns),
// 0 (no banking), and +1 (leans into turns) m_bankingEfficiency
//VEHICLE_BANKING_MIX - slider between 0 (static banking)
// and 1 (dynamic banking) m_bankingMix
//VEHICLE_BANKING_TIMESCALE - exponential timescale for the banking
// behavior to take full effect m_bankingTimescale
// ---- END NOTES
/*//Banking cuts down on the X as it adds to the Z
// Save this for later so we can reduce X as needed
float oldAngularVelZ = m_angularMotorVelocity.Z;
//Efficiency makes it go either inward or outward... so it can be multiplied by the X velocity
// so that we get a rotation in the right direction with the right amount of force
// Then divide by the timescale and timeStep so that we don't apply it all at once
//if(m_angularMotorVelocity.Z > 0)
if (m_angularMotorVelocity.X != 0)
{
}
m_angularMotorVelocity.Z += (m_bankingEfficiency * m_angularMotorVelocity.X) / (m_bankingTimescale / pTimestep)*5;
//Figure the difference of velocity transfered from X --> Z
float angularVelZChange = m_angularMotorVelocity.Z - oldAngularVelZ;
//Now remove the difference that came from Z since it was transfered from X --> Z
/*if (m_angularMotorVelocity.X < 0)
m_angularMotorVelocity.X -= angularVelZChange / 3;
else
m_angularMotorVelocity.X -= angularVelZChange / 3;*/
float addAmount = m_angularMotorVelocity.X;
m_angularMotorVelocity.Z += (((1 - m_bankingMix) * m_bankingEfficiency * addAmount) / (m_bankingTimescale / pTimestep) * 5);
//float oldZAngVel = m_angularMotorVelocity.Z;
//m_angularMotorVelocity.X -= oldZAngVel - m_angularMotorVelocity.Z;
if (m_angularMotorVelocity.Z != 0)
{
}
#endregion
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
#endregion
#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 (1 + (1 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale) * pTimestep * pTimestep * pTimestep), 1, 1);
PreferredAxisOfMotion *= m_referenceFrame;
//Multiply it so that it scales linearly
deflection = PreferredAxisOfMotion;
//deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));
#endregion
// Sum of velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr + bank;
m_lastAngularVelocity *= deflection;
#region Limit Motor Up
double Zchange = d.BodyGetLinearVel(Body).Z;
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0 && Zchange > 0) //if it isn't going up, don't apply the limiting force
{
//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
m_lastAngularVelocity *= (new Vector3(1 - ((float)Zchange * (pTimestep * 10)), 1, 1) * m_referenceFrame);
}
#endregion
#region Block X,Y,Z rotation
//Block off X,Y,Z rotation as requested
if ((m_flags & (VehicleFlag.NO_X)) != 0)
m_lastAngularVelocity.X = 0;
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
m_lastAngularVelocity.Y = 0;
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
m_lastAngularVelocity.Z = 0;
#endregion
if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
#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
// Apply to the body
//
d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
}
/// <summary>
/// Dereference the creator scene so that it can be garbage collected if needed.
/// </summary>
internal void Dispose()
{
m_scene = null;
}
public AuroraODERayCastRequestManager(AuroraODEPhysicsScene pScene)
{
m_scene = pScene;
nearCallback = near;
ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
}
public AuroraODEPrim(String primName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
{
m_vehicle = new AuroraODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
if (!pos.IsFinite())
{
pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
}
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
prev_geom = IntPtr.Zero;
if (!pos.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
}
_orientation = rotation;
m_taintrot = _orientation;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
m_isphysical = false;
else
{
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (m_isphysical)
m_targetSpace = _parent_scene.space;
}
m_primName = primName;
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
// don't do .add() here; old geoms get recycled with the same hash
}
/// <summary>
/// Dereference the creator scene so that it can be garbage collected if needed.
/// </summary>
internal void Dispose()
{
m_scene = null;
if (ContactgeomsArray != IntPtr.Zero)
Marshal.FreeHGlobal(ContactgeomsArray);
}
public void setMesh(AuroraODEPhysicsScene parent_scene, IMesh mesh)
{
// This sleeper is there to moderate how long it takes between
// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
//Thread.Sleep(10);
//Kill Body so that mesh can re-make the geom
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
DestroyBody();
}
}
IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
if (m_MeshToTriMeshMap.ContainsKey(mesh))
{
_triMeshData = m_MeshToTriMeshMap[mesh];
}
else
{
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
m_MeshToTriMeshMap[mesh] = _triMeshData;
}
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
if (prim_geom == IntPtr.Zero)
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
}
}
catch (AccessViolationException)
{
m_log.Error("[PHYSICS]: MESH LOCKED");
return;
}
}
} // end Step
private void MoveLinear (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
{
Vector3 motorDirection = m_linearMotorDirection;
if(!motorDirection.ApproxEquals(Vector3.Zero, 0.01f) || m_linearMotorApply > 90) // requested m_linearMotorDirection is significant
{
if(m_linearMotorApply <= 80)
if(m_linearMotorTimescale > 1)
m_linearMotorDirection /= m_linearMotorTimescale;
else
{
m_linearMotorDirection *= m_linearMotorTimescale;
motorDirection *= m_linearMotorTimescale;
}
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
//Interpolate between the current and last
float diff = 100 - m_linearMotorApply;
if(m_linearMotorApply >= 90)
motorDirection = (m_linearMotorDirection * (diff / 10f)) + (m_linearMotorDirectionLASTSET * (1 - (diff / 10f)));
// add drive to body
Vector3 addAmount = motorDirection / m_linearMotorTimescale;
addAmount *= pTimestep;
m_lastLinearVelocityVector += (addAmount); // lastLinearVelocityVector is the current body velocity vector?
//This is a huge problem with the Bwind script, it 'must' be disabled
// 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_linearMotorDirection.X = m_linearMotorDirectionLASTSET.X;
if (Math.Abs (m_lastLinearVelocityVector.Y) > Math.Abs (m_linearMotorDirectionLASTSET.Y))
m_linearMotorDirection.Y = m_linearMotorDirectionLASTSET.Y;
if (Math.Abs (m_lastLinearVelocityVector.Z) > Math.Abs (m_linearMotorDirectionLASTSET.Z))
m_linearMotorDirection.Z = m_linearMotorDirectionLASTSET.Z;*/
if(!addAmount.ApproxEquals(Vector3.Zero, 0.01f))
{
// decay applied velocity
float decayTime = m_linearMotorDecayTimescale / pTimestep;
Vector3 decayfraction = Vector3.One / decayTime;
if(decayfraction.X > 0.9f)
decayfraction.X = 0.9f;
if(decayfraction.Y > 0.9f)
decayfraction.Y = 0.9f;
if(decayfraction.Z > 0.9f)
decayfraction.Z = 0.9f;
Vector3 decayAmt = (motorDirection * decayfraction);
motorDirection -= decayAmt;
decayAmt = (m_linearMotorDirection * decayfraction);
m_linearMotorDirection -= decayAmt;
}
if(m_linearMotorApply > 0)
m_linearMotorApply--;
}
else if(m_linearMotorApply > 0)
m_linearMotorApply--;
// 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 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
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;
Vector3 pos = parent.Position;
// 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);
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
double Zchange = Math.Abs(m_lastposChange.Z);
if (m_BlockingEndPoint != Vector3.Zero)
{
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= m_lastposChange.X + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= m_lastposChange.Y + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= m_lastposChange.Z + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += m_lastposChange.X + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += m_lastposChange.Y + 1;
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_dir.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, tempHoverHeight);
}
}
else
{
float herr0 = pos.Z - tempHoverHeight;
// Replace Vertical speed with correction figure if significant
if (herr0 > 0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else if(herr0 < -0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50f) / m_VhoverTimescale);
}
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
}
#endregion
#region No X,Y,Z
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;
#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]);
}
}
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
#region limitations
if (Math.Abs (m_dir.X) > 1000 ||
Math.Abs (m_dir.Y) > 1000 ||
Math.Abs (m_dir.Z) > 1000)
{
m_dir = Vector3.Zero;
/*
//This vehicle is f***ed
parent.RaiseOutOfBounds (parent.Position);
parent._zeroFlag = true;
parent.m_disabled = true;
parent.m_frozen = true;*/
return;
}
#endregion
if(m_dir.ApproxEquals(Vector3.Zero, 0.001f))
m_dir = Vector3.Zero;
m_dir += TaintedForce;
m_lastPositionVector = parent.Position;
// 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));
/*if(parent.LinkSetIsColliding)
{
}*/
if(m_linearMotorDirectionLASTSET.X != 0 && (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X) < 10)
decayamount.X += 0.025f * (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X);
if(m_linearMotorDirectionLASTSET.Y != 0 && (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y) < 10)
decayamount.Y += 0.025f * (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y);
if(m_linearMotorDirectionLASTSET.Z != 0 && (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z) < 10)
decayamount.Z += 0.025f * (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z);
if(m_linearMotorApply <= 0)
decayamount += new Vector3(0.1f, 0.1f, 0.1f);
if(decayamount.X > 1)
decayamount.X = 1;
if(decayamount.Y > 1)
decayamount.Y = 1;
if(decayamount.Z > 1)
decayamount.Z = 1;
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
if(m_linearMotorApply <= 0 ? m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.1f) :
m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.001f))
{
m_lastLinearVelocityVector = Vector3.Zero;
m_linearZeroFlag = true;
}
else
{
m_linearZeroFlag = false;
}
} // end MoveLinear()