public void SetRotationLocked(Quaternion taintRot)
{
d.Quaternion q = new d.Quaternion
{
W = taintRot.W,
X = taintRot.X,
Y = taintRot.Y,
Z = taintRot.Z
};
d.BodySetQuaternion(Body, ref q); // just keep in sync with rest of simutator
}
internal void LimitRotation(float timestep)
{
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
d.Quaternion m_rot = new d.Quaternion();
bool changed = false;
m_rot.X = rotq.X;
m_rot.Y = rotq.Y;
m_rot.Z = rotq.Z;
m_rot.W = rotq.W;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rotq.X >= m_RollreferenceFrame.X)
{
m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
}
if (rotq.Y >= m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
}
if (rotq.X <= -m_RollreferenceFrame.X)
{
m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
}
if (rotq.Y <= -m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
}
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
m_rot.X = 0;
m_rot.Y = 0;
changed = true;
}
if (changed)
{
d.BodySetQuaternion(Body, ref m_rot);
}
}
private void SetLinearMotorProperties()
{
Vector3 dirNorm = m_lastLinearVelocityVector;
dirNorm.Normalize();
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
dirNorm *= rotq;
if (m_lMotor1 != IntPtr.Zero)
{
d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z);
d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length());
d.JointSetLMotorParam(m_lMotor1, (int)dParam.FMax, 35f * objMass.mass);
}
}
public void changeadd ()
{
// all prims are now created non physical
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
m_targetSpace = targetspace;
if (_mesh == null)
{
if (_parent_scene.needsMeshing (_parent_entity))
{
// Don't need to re-enable body.. it's done in SetMesh
_mesh = _parent_scene.mesher.CreateMesh (_parent_entity.Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
// createmesh returns null when it's a shape that isn't a cube.
// m_log.Debug(m_localID);
}
}
//Console.WriteLine("changeadd 1");
CreateGeom (m_targetSpace, _mesh);
if (prim_geom != IntPtr.Zero)
{
CalculatePrimMass();
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
// _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
/*
if (m_isphysical && Body == IntPtr.Zero)
{
if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
{
changeshape(_pbs);
}
else
{
MakeBody();
}
}
*/
changeSelectedStatus (m_isSelected);
}
public void changeprimsizeshape()
{
_parent_scene.actor_name_map.Remove(prim_geom);
WhiteCoreODEPrim parent = null;
bool chp = childPrim;
if (chp)
parent = (WhiteCoreODEPrim) _parent;
// Cleanup of old prim geometry and Bodies
if (chp)
{
if (parent != null)
parent.DestroyBody();
}
else
{
DestroyBody();
}
if (prim_geom != IntPtr.Zero)
{
try
{
d.GeomDestroy(prim_geom);
}
catch (AccessViolationException)
{
prim_geom = IntPtr.Zero;
MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
}
prim_geom = IntPtr.Zero;
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0)
_size.X = 0.01f;
if (_size.Y <= 0)
_size.Y = 0.01f;
if (_size.Z <= 0)
_size.Z = 0.01f;
CreateGeom(m_targetSpace);
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_parent_scene.actor_name_map[prim_geom] = this;
}
changeSelectedStatus(m_isSelected);
if (chp)
{
if (parent != null)
{
parent.MakeBody();
}
}
else
MakeBody();
}
public void MakeBody ()
{
// d.Vector3 dvtmp;
// d.Vector3 dbtmp;
d.Mass tmpdmass = new d.Mass
{
};
d.Mass objdmass = new d.Mass
{
};
d.Matrix3 mat = new d.Matrix3 ();
d.Matrix3 mymat = new d.Matrix3 ();
d.Quaternion quat = new d.Quaternion ();
d.Quaternion myrot = new d.Quaternion ();
Vector3 rcm;
if (childPrim) // child prims don't get own bodies;
return;
if (Body != IntPtr.Zero) // who shouldn't have one already ?
{
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (!m_isphysical) // only physical things get a body
return;
Body = d.BodyCreate (_parent_scene.world);
calcdMass (); // compute inertia on local frame
DMassDup (ref primdMass, out objdmass);
// rotate inertia
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.RfromQ (out mymat, ref myrot);
d.MassRotate (ref objdmass, ref mymat);
// set the body rotation and position
d.BodySetRotation (Body, ref mymat);
// recompute full object inertia if needed
if (childrenPrim.Count > 0)
{
rcm.X = _position.X + objdmass.c.X;
rcm.Y = _position.Y + objdmass.c.Y;
rcm.Z = _position.Z + objdmass.c.Z;
lock (childrenPrim)
{
foreach (AuroraODEPrim prm in childrenPrim)
{
prm.calcdMass (); // recompute inertia on local frame
DMassCopy (ref prm.primdMass, ref tmpdmass);
// apply prim current rotation to inertia
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.RfromQ (out mat, ref quat);
d.MassRotate (ref tmpdmass, ref mat);
Vector3 ppos = prm._position;
ppos.X += tmpdmass.c.X - rcm.X;
ppos.Y += tmpdmass.c.Y - rcm.Y;
ppos.Z += tmpdmass.c.Z - rcm.Z;
// refer inertia to root prim center of mass position
d.MassTranslate (ref tmpdmass,
ppos.X,
ppos.Y,
ppos.Z);
d.MassAdd (ref objdmass, ref tmpdmass); // add to total object inertia
// fix prim colision cats
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
d.GeomClearOffset (prm.prim_geom);
d.GeomSetBody (prm.prim_geom, Body);
d.GeomSetOffsetWorldRotation (prm.prim_geom, ref mat); // set relative rotation
}
}
}
d.GeomClearOffset (prim_geom); // make sure we don't have a hidden offset
// associate root geom with body
d.GeomSetBody (prim_geom, Body);
d.BodySetPosition (Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
d.GeomSetOffsetWorldPosition (prim_geom, _position.X, _position.Y, _position.Z);
d.MassTranslate (ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
myrot.W = -myrot.W;
d.RfromQ (out mymat, ref myrot);
d.MassRotate (ref objdmass, ref mymat);
d.BodySetMass (Body, ref objdmass);
_mass = objdmass.mass;
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
// disconnect from world gravity so we can apply buoyancy
if (!testRealGravity)
d.BodySetGravityMode (Body, false);
d.BodySetAutoDisableFlag (Body, true);
d.BodySetAutoDisableSteps (Body, body_autodisable_frames);
// d.BodySetLinearDampingThreshold(Body, 0.01f);
// d.BodySetAngularDampingThreshold(Body, 0.001f);
d.BodySetDamping (Body, .001f, .001f);
m_disabled = false;
d.GeomSetCategoryBits (prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits (prim_geom, (int)m_collisionFlags);
m_interpenetrationcount = 0;
m_collisionscore = 0;
if (m_targetSpace != _parent_scene.space)
{
_parent_scene.waitForSpaceUnlock (m_targetSpace);
if (d.SpaceQuery (m_targetSpace, prim_geom))
d.SpaceRemove (m_targetSpace, prim_geom);
m_targetSpace = _parent_scene.space;
d.SpaceAdd (m_targetSpace, prim_geom);
}
lock (childrenPrim)
{
foreach (AuroraODEPrim prm in childrenPrim)
{
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
Vector3 ppos = prm._position;
d.GeomSetOffsetWorldPosition (prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
d.GeomSetCategoryBits (prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits (prm.prim_geom, (int)prm.m_collisionFlags);
prm.Body = Body;
prm.m_disabled = false;
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
_parent_scene.addActivePrim (prm);
if (prm.m_targetSpace != _parent_scene.space)
{
_parent_scene.waitForSpaceUnlock (m_targetSpace);
if (d.SpaceQuery (prm.m_targetSpace, prm.prim_geom))
d.SpaceRemove (prm.m_targetSpace, prm.prim_geom);
prm.m_targetSpace = _parent_scene.space;
d.SpaceAdd (m_targetSpace, prm.prim_geom);
}
}
}
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals (Vector3.One, 0.0f)) && _parent == null)
{
createAMotor (m_angularlock);
}
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
m_vehicle.Enable (Body, this, _parent_scene);
}
_parent_scene.addActivePrim (this);
/* d.Mass mtmp;
d.BodyGetMass(Body, out mtmp);
d.Matrix3 mt = d.GeomGetRotation(prim_geom);
d.Matrix3 mt2 = d.BodyGetRotation(Body);
dvtmp = d.GeomGetPosition(prim_geom);
dbtmp = d.BodyGetPosition(Body);
*/
}
/// <summary>
/// Add a child prim to this parent prim.
/// </summary>
/// <param name="prim">Child prim</param>
private void AddChildPrim(OdePrim prim)
{
if (LocalID == prim.LocalID)
return;
if (Body == IntPtr.Zero)
{
Body = d.BodyCreate(_parent_scene.world);
setMass();
}
lock (childrenPrim)
{
if (childrenPrim.Contains(prim))
return;
// m_log.DebugFormat(
// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
childrenPrim.Add(prim);
foreach (OdePrim prm in childrenPrim)
{
d.Mass m2;
d.MassSetZero(out m2);
d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
d.MassRotate(ref m2, ref mat);
d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
d.MassAdd(ref pMass, ref m2);
}
foreach (OdePrim prm in childrenPrim)
{
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
if (prm.m_assetFailed)
{
d.GeomSetCategoryBits(prm.prim_geom, 0);
d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
}
else
{
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
}
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
if (Body != IntPtr.Zero)
{
d.GeomSetBody(prm.prim_geom, Body);
prm.childPrim = true;
d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
//d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
}
else
{
m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
}
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
prm.m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
{
prm.createAMotor(m_angularlock);
}
prm.Body = Body;
_parent_scene.ActivatePrim(prm);
}
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
if (m_assetFailed)
{
d.GeomSetCategoryBits(prim_geom, 0);
d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
}
else
{
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
d.Quaternion quat2 = new d.Quaternion();
quat2.W = _orientation.W;
quat2.X = _orientation.X;
quat2.Y = _orientation.Y;
quat2.Z = _orientation.Z;
d.Matrix3 mat2 = new d.Matrix3();
d.RfromQ(out mat2, ref quat2);
d.GeomSetBody(prim_geom, Body);
d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
//d.GeomSetOffsetRotation(prim_geom, ref mat2);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
m_interpenetrationcount = 0;
m_collisionscore = 0;
m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
if (m_vehicle.Type != Vehicle.TYPE_NONE)
m_vehicle.Enable(Body, _parent_scene);
_parent_scene.ActivatePrim(this);
}
}
public void changeshape(float timestamp)
{
string oldname = _parent_scene.geom_name_map[prim_geom];
// Cleanup of old prim geometry and Bodies
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
try
{
d.GeomDestroy(prim_geom);
}
catch (System.AccessViolationException)
{
prim_geom = IntPtr.Zero;
m_log.Error("[PHYSICS]: PrimGeom dead");
}
prim_geom = IntPtr.Zero;
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Construction of new prim
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
// createmesh returns null when it doesn't mesh.
CreateGeom(m_targetSpace, mesh);
}
else
{
_mesh = null;
//Console.WriteLine("changeshape");
CreateGeom(m_targetSpace, null);
}
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
//myrot.W = _orientation.w;
myrot.W = _orientation.W;
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
d.GeomSetQuaternion(prim_geom, ref myrot);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical && Body == IntPtr.Zero)
{
// Re creates body on size.
// EnableBody also does setMass()
enableBody();
if (Body != IntPtr.Zero)
{
d.BodyEnable(Body);
}
}
_parent_scene.geom_name_map[prim_geom] = oldname;
changeSelectedStatus(timestamp);
if (childPrim)
{
if (_parent is AuroraODEPrim)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
m_taintshape = false;
}
private d.Quaternion ConvertTodQuat(Quaternion q)
{
d.Quaternion dq = new d.Quaternion { X = q.X, Y = q.Y, Z = q.Z, W = q.W };
return dq;
}
public void rotate(float timestep)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
if (Body != IntPtr.Zero)
{
// KF: If this is a root prim do BodySet
d.BodySetQuaternion(Body, ref myrot);
if (m_isphysical)
{
if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
}
else
{
// daughter prim, do Geom set
d.GeomSetQuaternion(prim_geom, ref myrot);
}
resetCollisionAccounting();
m_taintrot = _orientation;
}
public void ProcessTaints(float timestep)
{
if (!m_shouldBePhysical)
return;
if (m_tainted_isPhysical != m_isPhysical)
{
if (m_tainted_isPhysical)
{
// Create avatar capsule and related ODE data
if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero)) // && Amotor == IntPtr.Zero))
{
m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+ (Shell!=IntPtr.Zero ? "Shell ":"")
+ (Body!=IntPtr.Zero ? "Body ":"")
);
// + (Amotor!=IntPtr.Zero ? "Amotor ":""));
}
AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);//, _parent_scene.avStandupTensor);
_parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
_parent_scene.AddCharacter(this);
}
else
{
_parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy (Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
Shell = IntPtr.Zero;
}
}
m_isPhysical = m_tainted_isPhysical;
}
if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
{
if (Shell != IntPtr.Zero && Body != IntPtr.Zero) // && Amotor != IntPtr.Zero)
{
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy (Amotor);
Amotor = IntPtr.Zero;
}
m_pidControllerActive = true;
// no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
float prevCapsule = CAPSULE_LENGTH;
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
d.BodyDestroy(Body);
d.GeomDestroy(Shell);
AvatarGeomAndBodyCreation(_position.X, _position.Y,
_position.Z + (CAPSULE_LENGTH - prevCapsule));//, _parent_scene.avStandupTensor);
Velocity = Vector3.Zero;
_parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
}
else
{
m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+ (Shell==IntPtr.Zero ? "Shell ":"")
+ (Body==IntPtr.Zero ? "Body ":"")
);
}
}
if (!m_taintPosition.ApproxEquals(_position, 0.05f))
{
if (Body != IntPtr.Zero)
{
d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
_position.X = m_taintPosition.X;
_position.Y = m_taintPosition.Y;
_position.Z = m_taintPosition.Z;
}
}
if (Body != IntPtr.Zero)
{
d.Quaternion q = new d.Quaternion ();
q.W = m_taintRotation.W;
q.X = m_taintRotation.X;
q.Y = m_taintRotation.Y;
q.Z = m_taintRotation.Z;
d.BodySetQuaternion (Body, ref q); // just keep in sync with rest of simutator
}
}
public void changeadd()
{
// all prims are now created non physical
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
m_targetSpace = targetspace;
if (_mesh == null)
{
if (_parent_scene.needsMeshing(_parent_entity))
{
// Don't need to re-enable body.. it's done in SetMesh
_mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size,
_parent_scene.meshSculptLOD, true);
//Tell things above if they want to cache it or something
if(_mesh != null)
_parent_entity.ParentEntity.GeneratedMesh(_parent_entity, _mesh);
// createmesh returns null when it's a shape that isn't a cube.
// MainConsole.Instance.Debug(m_localID);
// Remove the reference to any JPEG2000 sculpt data so it can be GCed
_pbs.SculptData = null;
}
}
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace, _mesh);
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
// _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
changeSelectedStatus(m_isSelected);
}
/// <summary>
/// Change prim in response to a shape taint.
/// </summary>
private void changeshape()
{
m_taintshape = false;
// Cleanup of old prim geometry and Bodies
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
RemoveGeom();
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Construction of new prim
IMesh mesh = null;
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in CreateMesh
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
// createmesh returns null when it doesn't mesh.
mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
if (mesh == null)
CheckMeshAsset();
else
m_assetFailed = false;
}
CreateGeom(m_targetSpace, mesh);
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
//myrot.W = _orientation.w;
myrot.W = _orientation.W;
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
d.GeomSetQuaternion(prim_geom, ref myrot);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical && Body == IntPtr.Zero)
{
// Re creates body on size.
// EnableBody also does setMass()
enableBody();
if (Body != IntPtr.Zero)
{
d.BodyEnable(Body);
}
}
changeSelectedStatus();
if (childPrim)
{
if (_parent is OdePrim)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
// m_taintshape = false;
}
/// <summary>
/// Change prim in response to a size taint.
/// </summary>
private void changesize()
{
#if SPAM
m_log.DebugFormat("[ODE PRIM]: Called changesize");
#endif
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
//kill body to rebuild
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
if (d.SpaceQuery(m_targetSpace, prim_geom))
{
// _parent_scene.waitForSpaceUnlock(m_targetSpace);
d.SpaceRemove(m_targetSpace, prim_geom);
}
RemoveGeom();
// we don't need to do space calculation because the client sends a position update also.
IMesh mesh = null;
// Construction of new prim
if (_parent_scene.needsMeshing(_pbs))
{
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
// Don't need to re-enable body.. it's done in SetMesh
if (_parent_scene.needsMeshing(_pbs))
{
mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
if (mesh == null)
CheckMeshAsset();
else
m_assetFailed = false;
}
}
CreateGeom(m_targetSpace, mesh);
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical && Body == IntPtr.Zero && !childPrim)
{
// Re creates body on size.
// EnableBody also does setMass()
enableBody();
d.BodyEnable(Body);
}
changeSelectedStatus();
if (childPrim)
{
if (_parent is OdePrim)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
m_taintsize = _size;
}
/// <summary>
/// Add prim in response to an add taint.
/// </summary>
private void changeadd()
{
// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
if (targetspace == IntPtr.Zero)
targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
m_targetSpace = targetspace;
IMesh mesh = null;
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
// createmesh returns null when it's a shape that isn't a cube.
// m_log.Debug(m_localID);
if (mesh == null)
CheckMeshAsset();
else
m_assetFailed = false;
}
#if SPAM
Console.WriteLine("changeadd 1");
#endif
CreateGeom(m_targetSpace, mesh);
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
if (IsPhysical && Body == IntPtr.Zero)
enableBody();
changeSelectedStatus();
m_taintadd = false;
}
public void changeOrientation(Quaternion newrot)
{
if (m_isphysical)
{
if (childPrim)
{
if (m_blockPhysicalReconstruction) // inertia is messed, must rebuild
{
_orientation = newrot;
}
}
else
{
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = newrot;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newrot;
if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
}
if (Body != IntPtr.Zero)
d.BodyEnable(Body);
}
else
{
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = newrot;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_orientation = newrot;
}
}
if (--fakeori < 0)
fakeori = 0;
changeSelectedStatus(m_isSelected);
resetCollisionAccounting();
}
public void changeprimsizeshape()
{
_parent_scene.actor_name_map.Remove(prim_geom);
AuroraODEPrim parent = null;
bool chp = childPrim;
if (chp)
parent = (AuroraODEPrim)_parent;
// Cleanup of old prim geometry and Bodies
if (chp)
{
if (parent != null)
parent.DestroyBody();
}
else
{
DestroyBody();
}
if (prim_geom != IntPtr.Zero)
{
try
{
d.GeomDestroy(prim_geom);
}
catch (AccessViolationException)
{
prim_geom = IntPtr.Zero;
MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
}
prim_geom = IntPtr.Zero;
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0)
_size.X = 0.01f;
if (_size.Y <= 0)
_size.Y = 0.01f;
if (_size.Z <= 0)
_size.Z = 0.01f;
// Construction of new prim
if (_parent_scene.needsMeshing(_parent_entity))
{
float meshlod = _parent_scene.meshSculptLOD;
IMesh mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size, meshlod, true);
// createmesh returns null when it doesn't mesh.
CreateGeom(m_targetSpace, mesh);
}
else
{
_mesh = null;
//Console.WriteLine("changeshape");
CreateGeom(m_targetSpace, null);
}
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
_parent_scene.actor_name_map[prim_geom] = this;
}
changeSelectedStatus(m_isSelected);
if (chp)
{
if (parent != null)
{
parent.MakeBody();
}
}
else
MakeBody();
;
}
// I'm the parent
// prim is the child
public void ParentPrim(AuroraODEPrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
if (this.m_localID != prim.m_localID)
{
if (Body == IntPtr.Zero)
{
Body = d.BodyCreate(_parent_scene.world);
setMass();
}
if (Body != IntPtr.Zero)
{
lock (childrenPrim)
{
if (!childrenPrim.Contains(prim))
{
//Console.WriteLine("childrenPrim.Add " + prim);
childrenPrim.Add(prim);
foreach (AuroraODEPrim prm in childrenPrim)
{
d.Mass m2;
d.MassSetZero(out m2);
prim.Mass = prim.CalculateMass();
d.MassSetBoxTotal(out m2, prim.Mass, prm._size.X, prm._size.Y, prm._size.Z);
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
d.MassRotate(ref m2, ref mat);
d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
d.MassAdd(ref pMass, ref m2);
}
foreach (AuroraODEPrim prm in childrenPrim)
{
prm.m_collisionCategories |= CollisionCategories.Body;
prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
if (prm.prim_geom == IntPtr.Zero)
{
m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
d.Quaternion quat = new d.Quaternion();
quat.W = prm._orientation.W;
quat.X = prm._orientation.X;
quat.Y = prm._orientation.Y;
quat.Z = prm._orientation.Z;
d.Matrix3 mat = new d.Matrix3();
d.RfromQ(out mat, ref quat);
if (Body != IntPtr.Zero)
{
d.GeomSetBody(prm.prim_geom, Body);
prm.childPrim = true;
d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
//d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
}
else
{
m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
}
prm.m_interpenetrationcount = 0;
prm.m_collisionscore = 0;
prm.m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
{
prm.createAMotor(m_angularlock);
}
prm.Body = Body;
_parent_scene.addActivePrim(prm);
}
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
d.Quaternion quat2 = new d.Quaternion();
quat2.W = _orientation.W;
quat2.X = _orientation.X;
quat2.Y = _orientation.Y;
quat2.Z = _orientation.Z;
d.Matrix3 mat2 = new d.Matrix3();
d.RfromQ(out mat2, ref quat2);
d.GeomSetBody(prim_geom, Body);
d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
//d.GeomSetOffsetPosition(prim.prim_geom,
// (Position.X - prm.Position.X) - pMass.c.X,
// (Position.Y - prm.Position.Y) - pMass.c.Y,
// (Position.Z - prm.Position.Z) - pMass.c.Z);
//d.GeomSetOffsetRotation(prim_geom, ref mat2);
d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
d.BodySetMass(Body, ref pMass);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
m_interpenetrationcount = 0;
m_collisionscore = 0;
m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, this, _parent_scene);
_parent_scene.addActivePrim(this);
}
}
}
}
}
} //end MoveAngular
internal void LimitRotation(float timestep)
{
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
d.Quaternion m_rot = new d.Quaternion();
bool changed = false;
m_rot.X = rotq.X;
m_rot.Y = rotq.Y;
m_rot.Z = rotq.Z;
m_rot.W = rotq.W;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rotq.X >= m_RollreferenceFrame.X)
{
m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
}
if (rotq.Y >= m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
}
if (rotq.X <= -m_RollreferenceFrame.X)
{
m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
}
if (rotq.Y <= -m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
}
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
m_rot.X = 0;
m_rot.Y = 0;
changed = true;
}
if (changed)
d.BodySetQuaternion(Body, ref m_rot);
}
public void changesize(float timestamp)
{
string oldname = _parent_scene.geom_name_map[prim_geom];
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Cleanup of old prim geometry
if (_mesh != null)
{
// Cleanup meshing here
}
//kill body to rebuild
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
if (d.SpaceQuery(m_targetSpace, prim_geom))
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
d.SpaceRemove(m_targetSpace, prim_geom);
}
d.GeomDestroy(prim_geom);
prim_geom = IntPtr.Zero;
// we don't need to do space calculation because the client sends a position update also.
// Construction of new prim
if (_parent_scene.needsMeshing(_pbs))
{
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
// Don't need to re-enable body.. it's done in SetMesh
IMesh mesh = null;
if (_parent_scene.needsMeshing(_pbs))
mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
//IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
//Console.WriteLine("changesize 1");
CreateGeom(m_targetSpace, mesh);
}
else
{
_mesh = null;
//Console.WriteLine("changesize 2");
CreateGeom(m_targetSpace, _mesh);
}
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical && Body == IntPtr.Zero && !childPrim)
{
// Re creates body on size.
// EnableBody also does setMass()
enableBody();
d.BodyEnable(Body);
}
_parent_scene.geom_name_map[prim_geom] = oldname;
changeSelectedStatus(timestamp);
if (childPrim)
{
if (_parent is AuroraODEPrim)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
m_taintsize = _size;
}
public void changeadd()
{
// int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
// if (targetspace == IntPtr.Zero)
// targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
m_targetSpace = targetspace;
if (_mesh == null)
{
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
_mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
// createmesh returns null when it's a shape that isn't a cube.
// m_log.Debug(m_localID);
}
}
lock (_parent_scene.OdeLock)
{
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace, _mesh);
if (prim_geom != IntPtr.Zero)
{
CalculatePrimMass();
/*
if (m_isphysical)
MakeBody();
else
*/
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
}
_parent_scene.geom_name_map[prim_geom] = this.m_primName;
_parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
}
changeSelectedStatus(m_isSelected);
}
public void enableBody()
{
// Don't enable this body if we're a child prim
// this should be taken care of in the parent function not here
if (!childPrim)
{
// Sets the geom to a body
Body = d.BodyCreate(_parent_scene.world);
setMass();
d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.BodySetQuaternion(Body, ref myrot);
d.GeomSetBody(prim_geom, Body);
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
d.BodySetAutoDisableFlag(Body, true);
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
d.BodySetDamping(Body, 1, 1);
// disconnect from world gravity so we can apply buoyancy
if(!testRealGravity)
d.BodySetGravityMode(Body, false);
m_interpenetrationcount = 0;
m_collisionscore = 0;
m_disabled = false;
// The body doesn't already have a finite rotation mode set here
if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
m_vehicle.Enable(Body, this, _parent_scene);
}
_parent_scene.addActivePrim(this);
}
}
private d.Quaternion ConvertTodQuat(Quaternion q)
{
d.Quaternion dq = new d.Quaternion();
dq.X = q.X;
dq.Y = q.Y;
dq.Z = q.Z;
dq.W = q.W;
return dq;
}
private void UpdateDataFromGeom()
{
if (prim_geom != IntPtr.Zero)
{
d.Vector3 lpos = d.GeomGetPosition(prim_geom);
_position.X = lpos.X;
_position.Y = lpos.Y;
_position.Z = lpos.Z;
d.Quaternion qtmp = new d.Quaternion { };
d.GeomCopyQuaternion(prim_geom, out qtmp);
_orientation.W = qtmp.W;
_orientation.X = qtmp.X;
_orientation.Y = qtmp.Y;
_orientation.Z = qtmp.Z;
}
}
public void SetRotationLocked(Quaternion taintRot)
{
d.Quaternion q = new d.Quaternion
{
W = taintRot.W,
X = taintRot.X,
Y = taintRot.Y,
Z = taintRot.Z
};
d.BodySetQuaternion(Body, ref q); // just keep in sync with rest of simutator
}
public void changemoveandrotate(Vector3 newpos,Quaternion newrot)
{
if (IsPhysical)
{
if (childPrim) // inertia is messed, must rebuild
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.DestroyBody();
parent.MakeBody();
}
else
{
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newrot.X;
myrot.Y = newrot.Y;
myrot.Z = newrot.Z;
myrot.W = newrot.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
}
if (Body != IntPtr.Zero)
d.BodyEnable(Body);
}
else
{
// string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
// int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
_parent_scene.waitForSpaceUnlock(m_targetSpace);
IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, newpos, m_targetSpace);
m_targetSpace = tempspace;
_parent_scene.waitForSpaceUnlock(m_targetSpace);
if (newrot != _orientation)
{
d.Quaternion myrot = new d.Quaternion();
myrot.X = newrot.X;
myrot.Y = newrot.Y;
myrot.Z = newrot.Z;
myrot.W = newrot.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
}
d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
_parent_scene.waitForSpaceUnlock(m_targetSpace);
d.SpaceAdd(m_targetSpace, prim_geom);
}
_orientation=newrot;
_position = newpos;
fakepos = false;
fakeori = false;
changeSelectedStatus(m_isSelected);
resetCollisionAccounting();
}
}// end Step
private void MoveAngular(float pTimestep)
{
/*
* 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);
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
* m_angularMotorVelocity.Y = angularVelocity.Y;
* m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, 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;
//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled(Body))
{
d.BodyEnable(Body);
}
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
}
public void changeprimsizeshape()
{
_parent_scene.geom_name_map.Remove(prim_geom);
_parent_scene.actor_name_map.Remove(prim_geom);
bool chp = childPrim;
// Cleanup of old prim geometry and Bodies
if (IsPhysical && Body != IntPtr.Zero)
{
if (chp)
{
if (_parent != null)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
DestroyBody();
}
}
if (prim_geom != IntPtr.Zero)
{
try
{
d.GeomDestroy(prim_geom);
}
catch (System.AccessViolationException)
{
prim_geom = IntPtr.Zero;
m_log.Error("[PHYSICS]: PrimGeom dead");
}
prim_geom = IntPtr.Zero;
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Construction of new prim
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
IMesh mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, meshlod, IsPhysical);
// createmesh returns null when it doesn't mesh.
CreateGeom(m_targetSpace, mesh);
}
else
{
_mesh = null;
//Console.WriteLine("changeshape");
CreateGeom(m_targetSpace, null);
}
lock (_parent_scene.OdeLock)
{
if (prim_geom != IntPtr.Zero)
{
CalculatePrimMass();
if (m_isphysical && !chp)
MakeBody();
else
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
myrot.X = _orientation.X;
myrot.Y = _orientation.Y;
myrot.Z = _orientation.Z;
myrot.W = _orientation.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
}
_parent_scene.geom_name_map[prim_geom] = this.m_primName;
_parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
}
changeSelectedStatus(m_isSelected);
if (chp)
{
if (_parent is AuroraODEPrim)
{
AuroraODEPrim parent = (AuroraODEPrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
}
private void MoveLinear(float pTimestep, OdeScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
if (!d.BodyIsEnabled(Body))
{
d.BodyEnable(Body);
}
// add drive to body
Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / pTimestep);
m_lastLinearVelocityVector += (addAmount * 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;
}
// decay applied velocity
Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / pTimestep)));
//Console.WriteLine("decay: " + decayfraction);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//Console.WriteLine("actual: " + m_linearMotorDirection);
}
else
{ // requested is not significant
// if what remains of applied is small, zero it.
if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastLinearVelocityVector = Vector3.Zero;
}
}
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
Vector3 grav = Vector3.Zero;
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel(Body);
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
d.Vector3 pos = d.BodyGetPosition(Body);
// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
Vector3 posChange = new Vector3();
posChange.X = pos.X - m_lastPositionVector.X;
posChange.Y = pos.Y - m_lastPositionVector.Y;
posChange.Z = pos.Z - m_lastPositionVector.Z;
double Zchange = Math.Abs(posChange.Z);
if (m_BlockingEndPoint != Vector3.Zero)
{
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= posChange.X + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= posChange.Y + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= posChange.Z + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += posChange.X + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += posChange.Y + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
}
if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
{
pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
// Check if hovering
if ((m_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 = _pParentScene.GetWaterLevel() + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, 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 aready heigher, use its height as target height
if (pos.Z > m_VhoverTargetHeight)
{
m_VhoverTargetHeight = pos.Z;
}
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{
d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
}
}
else
{
float herr0 = pos.Z - m_VhoverTargetHeight;
// Replace Vertical speed with correction figure if significant
if (Math.Abs(herr0) > 0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else
{
m_dir.Z = 0f;
}
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
}
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
{
//Start Experimental Values
if (Zchange > .3)
{
grav.Z = (float)(grav.Z * 3);
}
if (Zchange > .15)
{
grav.Z = (float)(grav.Z * 2);
}
if (Zchange > .75)
{
grav.Z = (float)(grav.Z * 1.5);
}
if (Zchange > .05)
{
grav.Z = (float)(grav.Z * 1.25);
}
if (Zchange > .025)
{
grav.Z = (float)(grav.Z * 1.125);
}
float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
float postemp = (pos.Z - terraintemp);
if (postemp > 2.5f)
{
grav.Z = (float)(grav.Z * 1.037125);
}
//End Experimental Values
}
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);
// Apply velocity
d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
// apply gravity force
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
internal void Step()
{
IntPtr Body = rootPrim.Body;
d.Mass dmass;
d.BodyGetMass(Body, out dmass);
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
Quaternion rotq = objrotq; // rotq = rotation of object
rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
Quaternion irotq = Quaternion.Inverse(rotq);
d.Vector3 dvtmp;
Vector3 tmpV;
Vector3 curVel; // velocity in world
Vector3 curAngVel; // angular velocity in world
Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
Vector3 torque = Vector3.Zero; // actually angular aceleration until mult by Inertia in vehicle frame
d.Vector3 dtorque = new d.Vector3();
dvtmp = d.BodyGetLinearVel(Body);
curVel.X = dvtmp.X;
curVel.Y = dvtmp.Y;
curVel.Z = dvtmp.Z;
Vector3 curLocalVel = curVel * irotq; // current velocity in local
dvtmp = d.BodyGetAngularVel(Body);
curAngVel.X = dvtmp.X;
curAngVel.Y = dvtmp.Y;
curAngVel.Z = dvtmp.Z;
Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
float ldampZ = 0;
// linear motor
if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
{
tmpV = m_linearMotorDirection - curLocalVel; // velocity error
tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
tmpV *= rotq; // to world
if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
{
tmpV.Z = 0;
}
if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
{
// have offset, do it now
tmpV *= dmass.mass;
d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
}
else
{
force.X += tmpV.X;
force.Y += tmpV.Y;
force.Z += tmpV.Z;
}
m_lmEfect *= m_lmDecay;
// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
m_ffactor = 0.0f;
}
else
{
m_lmEfect = 0;
m_ffactor = 1f;
}
// hover
if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
{
// d.Vector3 pos = d.BodyGetPosition(Body);
d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
pos.Z -= 0.21f; // minor offset that seems to be always there in sl
float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
float perr;
// default to global but don't go underground
perr = m_VhoverHeight - pos.Z;
if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
{
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
perr += _pParentScene.GetWaterLevel();
}
else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
perr += t;
}
else
{
float w = _pParentScene.GetWaterLevel();
if (t > w)
{
perr += t;
}
else
{
perr += w;
}
}
}
else if (t > m_VhoverHeight)
{
perr = t - pos.Z;
}
;
if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
{
ldampZ = m_VhoverEfficiency * m_invtimestep;
perr *= (1.0f + ldampZ) / m_VhoverTimescale;
// force.Z += perr - curVel.Z * tmp;
force.Z += perr;
ldampZ *= -curVel.Z;
force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
}
else // no buoyancy
{
force.Z += _pParentScene.gravityz;
}
}
else
{
// default gravity and Buoyancy
force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
}
// linear deflection
if (m_linearDeflectionEfficiency > 0)
{
float len = curVel.Length();
if (len > 0.01) // if moving
{
Vector3 atAxis;
atAxis = Xrot(rotq); // where are we pointing to
atAxis *= len; // make it same size as world velocity vector
tmpV = -atAxis; // oposite direction
atAxis -= curVel; // error to one direction
len = atAxis.LengthSquared();
tmpV -= curVel; // error to oposite
float lens = tmpV.LengthSquared();
if (len > 0.01 || lens > 0.01) // do nothing if close enougth
{
if (len < lens)
{
tmpV = atAxis;
}
tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
force.X += tmpV.X;
force.Y += tmpV.Y;
if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
{
force.Z += tmpV.Z;
}
}
}
}
// linear friction/damping
if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
{
tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
tmpV *= rotq; // to world
if (ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
{
tmpV.Z = ldampZ;
}
force.X += tmpV.X;
force.Y += tmpV.Y;
force.Z += tmpV.Z;
}
// vertical atractor
if (m_verticalAttractionTimescale < 300)
{
float roll;
float pitch;
float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
float ftmp2;
ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
m_amdampX = ftmp2;
m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
GetRollPitch(irotq, out roll, out pitch);
if (roll > halfpi)
{
roll = pi - roll;
}
else if (roll < -halfpi)
{
roll = -pi - roll;
}
float effroll = pitch / halfpi;
effroll *= effroll;
effroll = 1 - effroll;
effroll *= roll;
torque.X += effroll * ftmp;
if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
{
float effpitch = roll / halfpi;
effpitch *= effpitch;
effpitch = 1 - effpitch;
effpitch *= pitch;
torque.Y += effpitch * ftmp;
}
if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
{
float broll = effroll;
/*
* if (broll > halfpi)
* broll = pi - broll;
* else if (broll < -halfpi)
* broll = -pi - broll;
*/
broll *= m_bankingEfficiency;
if (m_bankingMix != 0)
{
float vfact = Math.Abs(curLocalVel.X) / 10.0f;
if (vfact > 1.0f)
{
vfact = 1.0f;
}
if (curLocalVel.X >= 0)
{
broll *= (1 + (vfact - 1) * m_bankingMix);
}
else
{
broll *= -(1 + (vfact - 1) * m_bankingMix);
}
}
// make z rot be in world Z not local as seems to be in sl
broll = broll / m_bankingTimescale;
tmpV = Zrot(irotq);
tmpV *= broll;
torque.X += tmpV.X;
torque.Y += tmpV.Y;
torque.Z += tmpV.Z;
m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
m_amdampY = m_amdampZ;
}
else
{
m_amdampZ = 1 / m_angularFrictionTimescale.Z;
m_amdampY = m_amdampX;
}
}
else
{
m_ampwr = 1.0f;
m_amdampX = 1 / m_angularFrictionTimescale.X;
m_amdampY = 1 / m_angularFrictionTimescale.Y;
m_amdampZ = 1 / m_angularFrictionTimescale.Z;
}
// angular motor
if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
{
tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
torque.X += tmpV.X * m_ampwr;
torque.Y += tmpV.Y * m_ampwr;
torque.Z += tmpV.Z;
m_amEfect *= m_amDecay;
}
else
{
m_amEfect = 0;
}
// angular deflection
if (m_angularDeflectionEfficiency > 0)
{
Vector3 dirv;
if (curLocalVel.X > 0.01f)
{
dirv = curLocalVel;
}
else if (curLocalVel.X < -0.01f)
{
// use oposite
dirv = -curLocalVel;
}
else
{
// make it fall into small positive x case
dirv.X = 0.01f;
dirv.Y = curLocalVel.Y;
dirv.Z = curLocalVel.Z;
}
float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
if (Math.Abs(dirv.Z) > 0.01)
{
torque.Y += -(float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
}
if (Math.Abs(dirv.Y) > 0.01)
{
torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
}
}
// angular friction
if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
{
torque.X -= curLocalAngVel.X * m_amdampX;
torque.Y -= curLocalAngVel.Y * m_amdampY;
torque.Z -= curLocalAngVel.Z * m_amdampZ;
}
if (force.X != 0 || force.Y != 0 || force.Z != 0)
{
force *= dmass.mass;
d.BodyAddForce(Body, force.X, force.Y, force.Z);
}
if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
{
torque *= m_referenceFrame; // to object frame
dtorque.X = torque.X;
dtorque.Y = torque.Y;
dtorque.Z = torque.Z;
d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
}
}
public void changeadd()
{
// all prims are now created non physical
IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
m_targetSpace = targetspace;
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace);
CalcPrimBodyData();
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
d.Quaternion myrot = new d.Quaternion();
Quaternion fake = _orientation;
myrot.X = fake.X;
myrot.Y = fake.Y;
myrot.Z = fake.Z;
myrot.W = fake.W;
d.GeomSetQuaternion(prim_geom, ref myrot);
// _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
}
changeSelectedStatus(m_isSelected);
}