/// <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(ODEPrim prim)
{
RemCollisionEventReporting(prim);
RemoveActivePrim(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("[ODE Physics]: Unable to remove prim from physics scene");
}
}
catch (AccessViolationException)
{
MainConsole.Instance.Info(
"[ODE Physics]: Couldn't remove prim from physics scene, it was already be removed.");
}
}
if (!prim.childPrim)
{
lock (prim.childrenPrim)
foreach (ODEPrim prm in prim.childrenPrim)
RemovePrimThreadLocked(prm);
}
lock (_prims)
_prims.Remove(prim);
}
internal void AddActivePrim(ODEPrim activatePrim)
{
// adds active prim.. (ones that should be iterated over in collisions_optimized
lock (_activeprimsLock)
{
if (!_activeprims.Contains(activatePrim))
_activeprims.Add(activatePrim);
}
}
internal void RemoveActivePrim(ODEPrim deactivatePrim)
{
lock (_activeprimsLock)
_activeprims.Remove(deactivatePrim);
}
// end Step
void MoveLinear(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
{
bool ishovering = false;
bool bypass_buoyancy = false;
d.Vector3 dpos = d.BodyGetPosition(Body);
d.Vector3 dvel_now = d.BodyGetLinearVel(Body);
d.Quaternion drotq_now = d.BodyGetQuaternion(Body);
Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);
Vector3 vel_now = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
Quaternion rotq = new Quaternion(drotq_now.X, drotq_now.Y, drotq_now.Z, drotq_now.W);
rotq *= m_referenceFrame; //add reference rotation to rotq
Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
m_newVelocity = Vector3.Zero;
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;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
else if(pos.Z < terrainHeight) {
m_newVelocity.Z += 1;
}
#endregion
#region Hover
Vector3 hovervel = Vector3.Zero;
if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) {
ishovering = true;
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) {
m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) {
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) {
m_VhoverTargetHeight = m_VhoverHeight;
}
else {
float waterlevel = (float)_pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f;
float terrainlevel = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
if(waterlevel > terrainlevel) {
m_VhoverTargetHeight = waterlevel + m_VhoverHeight;
}
else {
m_VhoverTargetHeight = terrainlevel + 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;
bypass_buoyancy = true; //emulate sl bug
}
}
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 {
hovervel.Z -= ((dvel_now.Z * 0.1f * m_VhoverEfficiency) + (pos.Z - tempHoverHeight)) / m_VhoverTimescale;
hovervel.Z *= 7.0f * (1.0f + m_VhoverEfficiency);
if(hovervel.Z > 50.0f) hovervel.Z = 50.0f;
if(hovervel.Z < -50.0f) hovervel.Z = -50.0f;
}
}
#endregion
#region limitations
//limit maximum velocity
if(vel_now.LengthSquared() > 1e6f) {
vel_now /= vel_now.Length();
vel_now *= 1000f;
d.BodySetLinearVel(Body, vel_now.X, vel_now.Y, vel_now.Z);
}
//block movement in x and y when low velocity
bool enable_ode_gravity = true;
if(vel_now.LengthSquared() < 0.02f) {
d.BodySetLinearVel(Body, 0.0f, 0.0f, 0.0f);
vel_now = Vector3.Zero;
if(parent.LinkSetIsColliding)
enable_ode_gravity = false;
}
#endregion
#region Linear motors
//cancel directions of linear friction for certain vehicles without having effect on ode gravity
Vector3 vt_vel_now = vel_now;
bool no_grav_calc = false;
if((Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON) && m_VehicleBuoyancy != 1.0f) {
vt_vel_now.Z = 0.0f;
no_grav_calc = true;
}
if(!bypass_buoyancy) {
//apply additional gravity force over ode gravity
if(m_VehicleBuoyancy == 1.0f) enable_ode_gravity = false;
else if(m_VehicleBuoyancy != 0.0f && enable_ode_gravity) {
float grav = _pParentScene.gravityz * parent.GravityMultiplier * -m_VehicleBuoyancy;
m_newVelocity.Z += grav * Mass;
}
}
//set ode gravity
d.BodySetGravityMode(Body, enable_ode_gravity);
//add default linear friction (mimic sl friction as much as possible)
float initialFriction = 0.055f;
float defaultFriction = 180f;
Vector3 friction = Vector3.Zero;
if(parent.LinkSetIsColliding || ishovering) {
if(vt_vel_now.X > 0.0f) friction.X += initialFriction;
if(vt_vel_now.Y > 0.0f) friction.Y += initialFriction;
if(vt_vel_now.Z > 0.0f) friction.Z += initialFriction;
if(vt_vel_now.X < 0.0f) friction.X -= initialFriction;
if(vt_vel_now.Y < 0.0f) friction.Y -= initialFriction;
if(vt_vel_now.Z < 0.0f) friction.Z -= initialFriction;
friction += vt_vel_now / defaultFriction;
friction *= irotq;
}
//world -> body orientation
vel_now *= irotq;
vt_vel_now *= irotq;
//add linear friction
if(vt_vel_now.X > 0.0f)
friction.X += vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
else
friction.X -= vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
if(vt_vel_now.Y > 0.0f)
friction.Y += vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
else
friction.Y -= vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
if(vt_vel_now.Z > 0.0f)
friction.Z += vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
else
friction.Z -= vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
friction /= 1.35f; //1.5f;
//add linear forces
//not the best solution, but it is really close to sl motor velocity, and just works
Vector3 motorVelocity = (m_linearMotorDirection * 3.0f - vel_now) / m_linearMotorTimescale / 5.0f; //2.8f;
Vector3 motorfrictionamp = new Vector3(4.0f, 4.0f, 4.0f);
Vector3 motorfrictionstart = new Vector3(1.0f, 1.0f, 1.0f);
motorVelocity *= motorfrictionstart + motorfrictionamp / (m_linearFrictionTimescale * pTimestep);
float addVel = 0.15f;
if(motorVelocity.LengthSquared() > 0.01f) {
if(motorVelocity.X > 0.0f) motorVelocity.X += addVel;
if(motorVelocity.Y > 0.0f) motorVelocity.Y += addVel;
if(motorVelocity.Z > 0.0f) motorVelocity.Z += addVel;
if(motorVelocity.X < 0.0f) motorVelocity.X -= addVel;
if(motorVelocity.Y < 0.0f) motorVelocity.Y -= addVel;
if(motorVelocity.Z < 0.0f) motorVelocity.Z -= addVel;
}
//free run
if(vel_now.X > m_linearMotorDirection.X && m_linearMotorDirection.X >= 0.0f) motorVelocity.X = 0.0f;
if(vel_now.Y > m_linearMotorDirection.Y && m_linearMotorDirection.Y >= 0.0f) motorVelocity.Y = 0.0f;
if(vel_now.Z > m_linearMotorDirection.Z && m_linearMotorDirection.Z >= 0.0f) motorVelocity.Z = 0.0f;
if(vel_now.X < m_linearMotorDirection.X && m_linearMotorDirection.X <= 0.0f) motorVelocity.X = 0.0f;
if(vel_now.Y < m_linearMotorDirection.Y && m_linearMotorDirection.Y <= 0.0f) motorVelocity.Y = 0.0f;
if(vel_now.Z < m_linearMotorDirection.Z && m_linearMotorDirection.Z <= 0.0f) motorVelocity.Z = 0.0f;
//decay linear motor
m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);
#endregion
#region Deflection
//does only deflect on x axis from world orientation with z axis rotated to body
//it is easier to filter out gravity deflection for vehicles(car) without rotation problems
Quaternion irotq_z = irotq;
irotq_z.X = 0.0f;
irotq_z.Y = 0.0f;
float mag = (float)Math.Sqrt(irotq_z.W * irotq_z.W + irotq_z.Z * irotq_z.Z); //normalize
irotq_z.W /= mag;
irotq_z.Z /= mag;
Vector3 vel_defl = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
vel_defl *= irotq_z;
if(no_grav_calc) {
vel_defl.Z = 0.0f;
if(!parent.LinkSetIsColliding) vel_defl.Y = 0.0f;
}
Vector3 deflection = vel_defl / m_linearDeflectionTimescale * m_linearDeflectionEfficiency * 100.0f;
float deflectionLengthY = Math.Abs(deflection.Y);
float deflectionLengthX = Math.Abs(deflection.X);
deflection.Z = 0.0f;
if((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) == 0) {
deflection.Z = deflectionLengthX;
deflection.X = -deflection.X;
}
if(vel_defl.X < 0.0f) deflection.X = -deflectionLengthY;
else if(vel_defl.X >= 0.0f) deflection.X = deflectionLengthY;
deflection.Y = -deflection.Y;
irotq_z.W = -irotq_z.W;
deflection *= irotq_z;
#endregion
#region Deal with tainted forces
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>();
}
#endregion
#region Add Forces
//add forces
m_newVelocity -= (friction *= Mass / pTimestep);
m_newVelocity += TaintedForce;
motorVelocity *= Mass / pTimestep;
#endregion
#region No X,Y,Z
if((m_flags & (VehicleFlag.NO_X)) != 0)
m_newVelocity.X = -vel_now.X * Mass / pTimestep;
if((m_flags & (VehicleFlag.NO_Y)) != 0)
m_newVelocity.Y = -vel_now.Y * Mass / pTimestep;
if((m_flags & (VehicleFlag.NO_Z)) != 0)
m_newVelocity.Z = -vel_now.Z * Mass / pTimestep;
#endregion
m_newVelocity *= rotq;
m_newVelocity += (hovervel *= Mass / pTimestep);
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
m_newVelocity += deflection;
motorVelocity *= rotq;
if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0 && motorVelocity.Z > 0.0f) motorVelocity.Z = 0.0f;
m_newVelocity += motorVelocity;
}
d.BodyAddForce(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z);
}
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)
{
ODEPrim newPrim = new ODEPrim(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;
}
void SetInStaticSpace(ODEPrim 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);
}
void MoveAngular(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
{
bool ishovering = false;
d.Vector3 d_angularVelocity = d.BodyGetAngularVel(Body);
d.Vector3 d_lin_vel_now = d.BodyGetLinearVel(Body);
d.Quaternion drotq = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(drotq.X, drotq.Y, drotq.Z, drotq.W);
rotq *= m_referenceFrame; //add reference rotation to rotq
Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
Vector3 angularVelocity = new Vector3(d_angularVelocity.X, d_angularVelocity.Y, d_angularVelocity.Z);
Vector3 linearVelocity = new Vector3(d_lin_vel_now.X, d_lin_vel_now.Y, d_lin_vel_now.Z);
Vector3 friction = Vector3.Zero;
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
//limit maximum rotation speed
if(angularVelocity.LengthSquared() > 1e3f) {
angularVelocity = Vector3.Zero;
d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
}
angularVelocity *= irotq; //world to body orientation
if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) ishovering = true;
#region Angular motor
Vector3 motorDirection = Vector3.Zero;
if(Type == Vehicle.TYPE_BOAT) {
//keep z flat for boats, no sidediving lol
Vector3 tmp = new Vector3(0.0f, 0.0f, m_angularMotorDirection.Z);
m_angularMotorDirection.Z = 0.0f;
m_angularMotorDirection += tmp * irotq;
}
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering){
motorDirection = m_angularMotorDirection * 0.34f; //0.3f;
}
m_angularMotorVelocity.X = (motorDirection.X - angularVelocity.X) / m_angularMotorTimescale;
m_angularMotorVelocity.Y = (motorDirection.Y - angularVelocity.Y) / m_angularMotorTimescale;
m_angularMotorVelocity.Z = (motorDirection.Z - angularVelocity.Z) / m_angularMotorTimescale;
m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);
if(m_angularMotorDirection.LengthSquared() > 0.0f)
{
if(angularVelocity.X > m_angularMotorDirection.X && m_angularMotorDirection.X >= 0.0f) m_angularMotorVelocity.X = 0.0f;
if(angularVelocity.Y > m_angularMotorDirection.Y && m_angularMotorDirection.Y >= 0.0f) m_angularMotorVelocity.Y = 0.0f;
if(angularVelocity.Z > m_angularMotorDirection.Z && m_angularMotorDirection.Z >= 0.0f) m_angularMotorVelocity.Z = 0.0f;
if(angularVelocity.X < m_angularMotorDirection.X && m_angularMotorDirection.X <= 0.0f) m_angularMotorVelocity.X = 0.0f;
if(angularVelocity.Y < m_angularMotorDirection.Y && m_angularMotorDirection.Y <= 0.0f) m_angularMotorVelocity.Y = 0.0f;
if(angularVelocity.Z < m_angularMotorDirection.Z && m_angularMotorDirection.Z <= 0.0f) m_angularMotorVelocity.Z = 0.0f;
}
#endregion
#region friction
float initialFriction = 0.0001f;
if(angularVelocity.X > initialFriction) friction.X += initialFriction;
if(angularVelocity.Y > initialFriction) friction.Y += initialFriction;
if(angularVelocity.Z > initialFriction) friction.Z += initialFriction;
if(angularVelocity.X < -initialFriction) friction.X -= initialFriction;
if(angularVelocity.Y < -initialFriction) friction.Y -= initialFriction;
if(angularVelocity.Z < -initialFriction) friction.Z -= initialFriction;
if(angularVelocity.X > 0.0f)
friction.X += angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
else
friction.X -= angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
if(angularVelocity.Y > 0.0f)
friction.Y += angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
else
friction.Y -= angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
if(angularVelocity.Z > 0.0f)
friction.Z += angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
else
friction.Z -= angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
if(Math.Abs(m_angularMotorDirection.X) > 0.01f) friction.X = 0.0f;
if(Math.Abs(m_angularMotorDirection.Y) > 0.01f) friction.Y = 0.0f;
if(Math.Abs(m_angularMotorDirection.Z) > 0.01f) friction.Z = 0.0f;
#endregion
#region Vertical attraction
if(m_verticalAttractionTimescale < 300)
{
float VAservo = 38.0f / m_verticalAttractionTimescale;
if(Type == Vehicle.TYPE_CAR) VAservo = 10.0f / m_verticalAttractionTimescale;
Vector3 verterr = new Vector3(0.0f, 0.0f, 1.0f);
verterr *= rotq;
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0.0f;
vertattr *= irotq;
//when upsidedown prefer x rotation of body, to keep forward movement direction the same
if(verterr.Z < 0.0f) {
vertattr.Y = -vertattr.Y * 2.0f;
if(vertattr.X < 0.0f) vertattr.X = -2.0f - vertattr.X;
else vertattr.X = 2.0f - vertattr.X;
}
vertattr *= VAservo;
vertattr.X += (vertattr.X - angularVelocity.X) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);
vertattr.Y += (vertattr.Y - angularVelocity.Y) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);
if((m_flags & (VehicleFlag.LIMIT_ROLL_ONLY)) != 0) vertattr.Y = 0.0f;
}
#endregion
#region deflection
//rotates body to direction of movement (linearMovement vector)
/* temporary disabled due to instabilities, needs to be rewritten
if(m_angularDeflectionTimescale < 300)
{
float Dservo = 0.05f * m_angularDeflectionTimescale * m_angularDeflectionEfficiency;
float mag = (float)linearVelocity.LengthSquared();
if(mag > 0.01f) {
linearVelocity.Y = -linearVelocity.Y;
linearVelocity *= rotq;
mag = (float)Math.Sqrt(mag);
linearVelocity.Y /= mag;
linearVelocity.Z /= mag;
deflection.Y = -linearVelocity.Z;
deflection.Z = -linearVelocity.Y;
deflection *= Dservo;
}
}
*/
#endregion
#region banking
if(m_verticalAttractionTimescale < 300 && m_bankingEfficiency > 0) { //vertical attraction must be enabled
float mag = (linearVelocity.X * linearVelocity.X + linearVelocity.Y * linearVelocity.Y);
if(mag > 0.01f) {
mag = (float)Math.Sqrt(mag);
if(mag > 20.0f) mag = 1.0f;
else mag /= 20.0f;
}
else mag = 0.0f;
float b_static = -m_angularMotorDirection.X * 0.12f * (1.0f - m_bankingMix);
float b_dynamic = -m_angularMotorDirection.X * 0.12f * mag * m_bankingMix;
banking.Z = (b_static + b_dynamic - d_angularVelocity.Z) / m_bankingTimescale * m_bankingEfficiency;
}
#endregion
m_lastAngularVelocity = angularVelocity;
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
angularVelocity += deflection;
angularVelocity -= friction;
}
else {
banking = Vector3.Zero;
}
angularVelocity += m_angularMotorVelocity;
angularVelocity += vertattr;
angularVelocity *= rotq;
angularVelocity += banking;
if(angularVelocity.LengthSquared() < 1e-5f) {
d.BodySetAngularVel(Body, 0, 0, 0);
m_angularZeroFlag = true;
}
else {
d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
m_angularZeroFlag = false;
}
}
internal void Enable(IntPtr pBody, ODEPrim parent, ODEPhysicsScene 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);
}
internal void ProcessTypeChange(ODEPrim 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;
}
}
void ChildDelink(ODEPrim odePrim)
{
// Okay, we have a delinked child.. destroy all body and remake
if (odePrim != this && !childrenPrim.Contains(odePrim))
return;
DestroyBody();
if (odePrim == this)
{
ODEPrim newroot = null;
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
newroot = childrenPrim[0];
childrenPrim.RemoveAt(0);
foreach (ODEPrim prm in childrenPrim)
{
newroot.childrenPrim.Add(prm);
}
childrenPrim.Clear();
}
if (newroot != null)
{
newroot.childPrim = false;
newroot._parent = null;
newroot.MakeBody();
}
}
}
else
{
lock (childrenPrim)
{
childrenPrim.Remove(odePrim);
odePrim.childPrim = false;
odePrim._parent = null;
//odePrim.UpdateDataFromGeom ();
odePrim.MakeBody();
}
}
MakeBody();
}
internal void Disable(ODEPrim 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;
}
void ChildSetGeom(ODEPrim odePrim)
{
DestroyBody();
MakeBody();
}
// I'm the parent
// prim is the child
public void ParentPrim(ODEPrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
if (m_localID != prim.m_localID)
{
DestroyBody();
lock (childrenPrim)
{
foreach (ODEPrim 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
}
}
void ChangeLink(ODEPrim 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 ODEPrim)
{
if (newparent != _parent)
{
ODEPrim obj = (ODEPrim) _parent;
obj.ChildDelink(this);
childPrim = false;
if (newparent != null)
{
newparent.ParentPrim(this);
}
}
}
}
_parent = newparent;
}
/// <summary>
/// Routine to figure out if we need to mesh this prim with our mesher
/// </summary>
internal bool NeedsMeshing(ODEPrim 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;
if (pbs.SculptType != (byte) SculptType.Mesh &&
pbs.SculptType != (byte) SculptType.None)
return true; //Sculpty, mesh it
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;
}
internal void Step(IntPtr pBody, float pTimestep, ODEPhysicsScene pParentScene, ODEPrim 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 BadPrim(ODEPrim universeODEPrim)
{
RemovePrim(universeODEPrim);
//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(universeODEPrim.ParentEntity);
}
void ChildRemove(ODEPrim odePrim)
{
// is this one of ours?
if (odePrim != this)
return;
bool havePrim;
lock(childrenPrim)
havePrim = childrenPrim.Contains (odePrim);
// Okay, we have a delinked child.. destroy all body and remake
if (!havePrim)
return;
DestroyBody();
if (odePrim == this)
{
ODEPrim newroot = null;
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
newroot = childrenPrim[0];
childrenPrim.RemoveAt(0);
foreach (ODEPrim prm in childrenPrim)
{
newroot.childrenPrim.Add(prm);
}
childrenPrim.Clear();
}
if (newroot != null)
{
newroot.childPrim = false;
newroot._parent = null;
newroot.MakeBody();
}
}
return;
}
lock (childrenPrim)
{
childrenPrim.Remove(odePrim);
odePrim.childPrim = false;
odePrim._parent = null;
}
MakeBody();
}
