private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
PrimitiveBaseShape pbs, bool isphysical, uint localID)
{
Vector3 pos = position;
Vector3 siz = size;
Quaternion rot = rotation;
OdePrim newPrim;
lock (OdeLock)
{
newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
lock (_prims)
_prims.Add(newPrim);
}
newPrim.LocalID = localID;
return newPrim;
}
public void addActivePrim(OdePrim activatePrim)
{
// adds active prim.. (ones that should be iterated over in collisions_optimized
lock (_activeprims)
{
if (!_activeprims.Contains(activatePrim))
_activeprims.Add(activatePrim);
//else
// m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
}
}
private void ChildDelink(OdePrim odePrim)
{
// Okay, we have a delinked child.. need to rebuild the body.
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
prm.childPrim = true;
prm.disableBody();
//prm.m_taintparent = null;
//prm._parent = null;
//prm.m_taintPhysics = false;
//prm.m_disabled = true;
//prm.childPrim = false;
}
}
disableBody();
lock (childrenPrim)
{
//Console.WriteLine("childrenPrim.Remove " + odePrim);
childrenPrim.Remove(odePrim);
}
if (Body != IntPtr.Zero)
{
_parent_scene.remActivePrim(this);
}
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildDelink calls ParentPrim");
AddChildPrim(prm);
}
}
}
/// <summary>
/// This is called from within simulate but outside the locked portion
/// We need to do our own locking here
/// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
/// Simulate() -- justincc).
///
/// 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)
{
// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
lock (prim)
{
RemoveCollisionEventReporting(prim);
if (prim.prim_geom != IntPtr.Zero)
{
prim.ResetTaints();
if (prim.IsPhysical)
{
prim.disableBody();
if (prim.childPrim)
{
prim.childPrim = false;
prim.Body = IntPtr.Zero;
prim.m_disabled = true;
prim.IsPhysical = false;
}
}
// we don't want to remove the main space
// If the geometry is in the targetspace, remove it from the target space
//m_log.Warn(prim.m_targetSpace);
//if (prim.m_targetSpace != IntPtr.Zero)
//{
//if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
//{
//if (d.GeomIsSpace(prim.m_targetSpace))
//{
//waitForSpaceUnlock(prim.m_targetSpace);
//d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
prim.m_targetSpace = IntPtr.Zero;
//}
//else
//{
// m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
//((OdePrim)prim).m_targetSpace.ToString());
//}
//}
//}
//m_log.Warn(prim.prim_geom);
if (!prim.RemoveGeom())
m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
lock (_prims)
_prims.Remove(prim);
//If there are no more geometries in the sub-space, we don't need it in the main space anymore
//if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
//{
//if (prim.m_targetSpace != null)
//{
//if (d.GeomIsSpace(prim.m_targetSpace))
//{
//waitForSpaceUnlock(prim.m_targetSpace);
//d.SpaceRemove(space, prim.m_targetSpace);
// free up memory used by the space.
//d.SpaceDestroy(prim.m_targetSpace);
//int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
//resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
//}
//else
//{
//m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
//((OdePrim) prim).m_targetSpace.ToString());
//}
//}
//}
if (SupportsNINJAJoints)
RemoveAllJointsConnectedToActorThreadLocked(prim);
}
}
}
private void ChildSetGeom(OdePrim odePrim)
{
// m_log.DebugFormat(
// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
//if (IsPhysical && Body != IntPtr.Zero)
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//prm.childPrim = true;
prm.disableBody();
//prm.m_taintparent = null;
//prm._parent = null;
//prm.m_taintPhysics = false;
//prm.m_disabled = true;
//prm.childPrim = false;
}
}
disableBody();
// Spurious - Body == IntPtr.Zero after disableBody()
// if (Body != IntPtr.Zero)
// {
// _parent_scene.DeactivatePrim(this);
// }
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildSetGeom calls ParentPrim");
AddChildPrim(prm);
}
}
}
/// <summary>
/// Add a child prim to this parent prim.
/// </summary>
/// <param name="prim">Child prim</param>
public void AddChildPrim(OdePrim prim)
{
//Console.WriteLine("AddChildPrim " + Name);
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 (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);
if (prm.prim_geom == IntPtr.Zero)
{
m_log.WarnFormat(
"[PHYSICS]: Unable to link one of the linkset elements {0} for parent {1}. No geom yet",
prm.Name, prim.Name);
continue;
}
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
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.addActivePrim(prm);
}
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//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.addActivePrim(this);
}
}
}
}
}
protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
{
// 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();
}
}
if (reMakeBody)
MakeBody();
return;
}
else
{
lock (childrenPrim)
{
childrenPrim.Remove(odePrim);
odePrim.childPrim = false;
odePrim._parent = null;
if (reMakeBody)
odePrim.MakeBody();
}
}
MakeBody();
}
public void AddChange(OdePrim prim, changes what, Object arg)
{
ODEchangeitem item = new ODEchangeitem();
item.prim = prim;
item.what = what;
item.arg = arg;
ChangesQueue.Enqueue(item);
}
/// <summary>
/// Add a child prim to this parent prim.
/// </summary>
/// <param name="prim">Child prim</param>
// I'm the parent
// prim is the child
public void ParentPrim(OdePrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
if (this.m_localID != prim.m_localID)
{
DestroyBody(); // for now we need to rebuil entire object on link change
lock (childrenPrim)
{
// adopt the prim
if (!childrenPrim.Contains(prim))
childrenPrim.Add(prim);
// see if this prim has kids and adopt them also
// should not happen for now
foreach (OdePrim prm in prim.childrenPrim)
{
if (!childrenPrim.Contains(prm))
{
if (prm.Body != IntPtr.Zero)
{
if (prm.prim_geom != IntPtr.Zero)
d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
if(prm.Body != prim.Body)
prm.DestroyBody(); // don't loose bodies around
prm.Body = IntPtr.Zero;
}
childrenPrim.Add(prm);
prm._parent = this;
}
}
}
//Remove old children from the prim
prim.childrenPrim.Clear();
prim.childPrim = true;
prim._parent = this;
if (prim.Body != IntPtr.Zero)
{
if (prim.prim_geom != IntPtr.Zero)
d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
prim.DestroyBody(); // don't loose bodies around
prim.Body = IntPtr.Zero;
}
MakeBody(); // full nasty reconstruction
}
}
private 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) // delinking the root prim
{
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();
}
//sets non physical prim m_targetSpace to right space in spaces grid for static prims
// should only be called for non physical prims unless they are becoming non physical
private void SetInStaticSpace(OdePrim prim)
{
IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
prim.m_targetSpace = targetSpace;
}
private void ChildSetGeom(OdePrim odePrim)
{
// well..
DestroyBody();
MakeBody();
}
private float CalculateMass()
{
float volume = 0;
// No material is passed to the physics engines yet.. soo..
// we're using the m_density constant in the class definition
float returnMass = 0;
switch (_pbs.ProfileShape)
{
case ProfileShape.Square:
// Profile Volume
volume = _size.X*_size.Y*_size.Z;
// If the user has 'hollowed out'
// ProfileHollow is one of those 0 to 50000 values :P
// we like percentages better.. so turning into a percentage
if (((float) _pbs.ProfileHollow/50000f) > 0.0)
{
float hollowAmount = (float) _pbs.ProfileHollow/50000f;
// calculate the hollow volume by it's shape compared to the prim shape
float hollowVolume = 0;
switch (_pbs.HollowShape)
{
case HollowShape.Square:
case HollowShape.Same:
// Cube Hollow volume calculation
float hollowsizex = _size.X*hollowAmount;
float hollowsizey = _size.Y*hollowAmount;
float hollowsizez = _size.Z*hollowAmount;
hollowVolume = hollowsizex*hollowsizey*hollowsizez;
break;
case HollowShape.Circle:
// Hollow shape is a perfect cyllinder in respect to the cube's scale
// Cyllinder hollow volume calculation
float hRadius = _size.X/2;
float hLength = _size.Z;
// pi * r2 * h
hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
break;
case HollowShape.Triangle:
// Equilateral Triangular Prism volume hollow calculation
// Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
float aLength = _size.Y;
// 1/2 abh
hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
break;
default:
hollowVolume = 0;
break;
}
volume = volume - hollowVolume;
}
break;
case ProfileShape.Circle:
if (_pbs.PathCurve == (byte)Extrusion.Straight)
{
// Cylinder
float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
// Approximating the cylinder's irregularity.
if (volume1 > volume2)
{
volume = (float)volume1 - (volume1 - volume2);
}
else if (volume2 > volume1)
{
volume = (float)volume2 - (volume2 - volume1);
}
else
{
// Regular cylinder
volume = volume1;
}
}
else
{
// We don't know what the shape is yet, so use default
volume = _size.X * _size.Y * _size.Z;
}
// If the user has 'hollowed out'
// ProfileHollow is one of those 0 to 50000 values :P
// we like percentages better.. so turning into a percentage
if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
{
float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
// calculate the hollow volume by it's shape compared to the prim shape
float hollowVolume = 0;
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Circle:
// Hollow shape is a perfect cyllinder in respect to the cube's scale
// Cyllinder hollow volume calculation
float hRadius = _size.X / 2;
float hLength = _size.Z;
// pi * r2 * h
hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
break;
case HollowShape.Square:
// Cube Hollow volume calculation
float hollowsizex = _size.X * hollowAmount;
float hollowsizey = _size.Y * hollowAmount;
float hollowsizez = _size.Z * hollowAmount;
hollowVolume = hollowsizex * hollowsizey * hollowsizez;
break;
case HollowShape.Triangle:
// Equilateral Triangular Prism volume hollow calculation
// Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
float aLength = _size.Y;
// 1/2 abh
hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
break;
default:
hollowVolume = 0;
break;
}
volume = volume - hollowVolume;
}
break;
case ProfileShape.HalfCircle:
if (_pbs.PathCurve == (byte)Extrusion.Curve1)
{
if (_size.X == _size.Y && _size.Y == _size.Z)
{
// regular sphere
// v = 4/3 * pi * r^3
float sradius3 = (float)Math.Pow((_size.X / 2), 3);
volume = (float)((4f / 3f) * Math.PI * sradius3);
}
else
{
// we treat this as a box currently
volume = _size.X * _size.Y * _size.Z;
}
}
else
{
// We don't know what the shape is yet, so use default
volume = _size.X * _size.Y * _size.Z;
}
break;
case ProfileShape.EquilateralTriangle:
/*
v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
// seed mesh
Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
*/
float xA = -0.25f * _size.X;
float yA = -0.45f * _size.Y;
float xB = 0.5f * _size.X;
float yB = 0;
float xC = -0.25f * _size.X;
float yC = 0.45f * _size.Y;
volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
// If the user has 'hollowed out'
// ProfileHollow is one of those 0 to 50000 values :P
// we like percentages better.. so turning into a percentage
float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
if (((float)fhollowFactor / 50000f) > 0.0)
{
float hollowAmount = (float)fhollowFactor / 50000f;
// calculate the hollow volume by it's shape compared to the prim shape
float hollowVolume = 0;
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Triangle:
// Equilateral Triangular Prism volume hollow calculation
// Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
float aLength = _size.Y;
// 1/2 abh
hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
break;
case HollowShape.Square:
// Cube Hollow volume calculation
float hollowsizex = _size.X * hollowAmount;
float hollowsizey = _size.Y * hollowAmount;
float hollowsizez = _size.Z * hollowAmount;
hollowVolume = hollowsizex * hollowsizey * hollowsizez;
break;
case HollowShape.Circle:
// Hollow shape is a perfect cyllinder in respect to the cube's scale
// Cyllinder hollow volume calculation
float hRadius = _size.X / 2;
float hLength = _size.Z;
// pi * r2 * h
hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
break;
default:
hollowVolume = 0;
break;
}
volume = volume - hollowVolume;
}
break;
default:
// we don't have all of the volume formulas yet so
// use the common volume formula for all
volume = _size.X*_size.Y*_size.Z;
break;
}
// Calculate Path cut effect on volume
// Not exact, in the triangle hollow example
// They should never be zero or less then zero..
// we'll ignore it if it's less then zero
// ProfileEnd and ProfileBegin are values
// from 0 to 50000
// Turning them back into percentages so that I can cut that percentage off the volume
float PathCutEndAmount = _pbs.ProfileEnd;
float PathCutStartAmount = _pbs.ProfileBegin;
if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
{
float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
// Check the return amount for sanity
if (pathCutAmount >= 0.99f)
pathCutAmount = 0.99f;
volume = volume - (volume*pathCutAmount);
}
UInt16 taperX = _pbs.PathScaleX;
UInt16 taperY = _pbs.PathScaleY;
float taperFactorX = 0;
float taperFactorY = 0;
// Mass = density * volume
if (taperX != 100)
{
if (taperX > 100)
{
taperFactorX = 1.0f - ((float)taperX / 200);
//m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
}
else
{
taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
//m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
}
volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
}
if (taperY != 100)
{
if (taperY > 100)
{
taperFactorY = 1.0f - ((float)taperY / 200);
//m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
}
else
{
taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
//m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
}
volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
}
returnMass = m_density*volume;
if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
// Recursively calculate mass
bool HasChildPrim = false;
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
HasChildPrim = true;
}
}
if (HasChildPrim)
{
OdePrim[] childPrimArr = new OdePrim[0];
lock (childrenPrim)
childPrimArr = childrenPrim.ToArray();
for (int i = 0; i < childPrimArr.Length; i++)
{
if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
returnMass += childPrimArr[i].CalculateMass();
// failsafe, this shouldn't happen but with OpenSim, you never know :)
if (i > 256)
break;
}
}
return returnMass;
}
public void remActivePrim(OdePrim deactivatePrim)
{
lock (_activeprims)
{
_activeprims.Remove(deactivatePrim);
}
}
private void changeLink(OdePrim NewParent)
{
if (_parent == null && NewParent != null)
{
NewParent.ParentPrim(this);
}
else if (_parent != null)
{
if (_parent is OdePrim)
{
if (NewParent != _parent)
{
(_parent as OdePrim).ChildDelink(this);
childPrim = false;
if (NewParent != null)
{
NewParent.ParentPrim(this);
}
}
}
}
_parent = NewParent;
}
/// <summary>
/// This is called from within simulate but outside the locked portion
/// We need to do our own locking here
/// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
/// Simulate() -- justincc).
///
/// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
///
/// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
/// that the space was using.
/// </summary>
/// <param name="prim"></param>
public void RemovePrimThreadLocked(OdePrim prim)
{
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
lock (prim)
{
RemoveCollisionEventReporting(prim);
lock (_prims)
_prims.Remove(prim);
if (SupportsNINJAJoints)
{
RemoveAllJointsConnectedToActorThreadLocked(prim);
}
}
}
/// <summary>
/// Stop this prim being subject to physics
/// </summary>
/// <param name="prim"></param>
internal void DeactivatePrim(OdePrim prim)
{
_activeprims.Remove(prim);
}
/// <summary>
/// Simulate the joint proxies of a NINJA actor.
/// </summary>
/// <remarks>
/// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
/// </remarks>
/// <param name="actor"></param>
protected void SimulateActorPendingJoints(OdePrim actor)
{
// If an actor moved, move its joint proxy objects as well.
// There seems to be an event PhysicsActor.OnPositionUpdate that could be used
// for this purpose but it is never called! So we just do the joint
// movement code here.
if (actor.SOPName != null &&
joints_connecting_actor.ContainsKey(actor.SOPName) &&
joints_connecting_actor[actor.SOPName] != null &&
joints_connecting_actor[actor.SOPName].Count > 0)
{
foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
{
if (affectedJoint.IsInPhysicsEngine)
{
DoJointMoved(affectedJoint);
}
else
{
DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
}
}
}
}
private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
{
// obj1LocalID = 0;
//returncollisions = false;
obj2LocalID = 0;
//ctype = 0;
//cStartStop = 0;
if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
return;
switch ((ActorTypes)p2.PhysicsActorType)
{
case ActorTypes.Agent:
cc2 = (OdeCharacter)p2;
// obj1LocalID = cc2.m_localID;
switch ((ActorTypes)p1.PhysicsActorType)
{
case ActorTypes.Agent:
cc1 = (OdeCharacter)p1;
obj2LocalID = cc1.m_localID;
cc1.AddCollisionEvent(cc2.m_localID, contact);
//ctype = (int)CollisionCategories.Character;
//if (cc1.CollidingObj)
//cStartStop = (int)StatusIndicators.Generic;
//else
//cStartStop = (int)StatusIndicators.Start;
//returncollisions = true;
break;
case ActorTypes.Prim:
if (p1 is OdePrim)
{
cp1 = (OdePrim) p1;
obj2LocalID = cp1.m_localID;
cp1.AddCollisionEvent(cc2.m_localID, contact);
}
//ctype = (int)CollisionCategories.Geom;
//if (cp1.CollidingObj)
//cStartStop = (int)StatusIndicators.Generic;
//else
//cStartStop = (int)StatusIndicators.Start;
//returncollisions = true;
break;
case ActorTypes.Ground:
case ActorTypes.Unknown:
obj2LocalID = 0;
//ctype = (int)CollisionCategories.Land;
//returncollisions = true;
break;
}
cc2.AddCollisionEvent(obj2LocalID, contact);
break;
case ActorTypes.Prim:
if (p2 is OdePrim)
{
cp2 = (OdePrim) p2;
// obj1LocalID = cp2.m_localID;
switch ((ActorTypes) p1.PhysicsActorType)
{
case ActorTypes.Agent:
if (p1 is OdeCharacter)
{
cc1 = (OdeCharacter) p1;
obj2LocalID = cc1.m_localID;
cc1.AddCollisionEvent(cp2.m_localID, contact);
//ctype = (int)CollisionCategories.Character;
//if (cc1.CollidingObj)
//cStartStop = (int)StatusIndicators.Generic;
//else
//cStartStop = (int)StatusIndicators.Start;
//returncollisions = true;
}
break;
case ActorTypes.Prim:
if (p1 is OdePrim)
{
cp1 = (OdePrim) p1;
obj2LocalID = cp1.m_localID;
cp1.AddCollisionEvent(cp2.m_localID, contact);
//ctype = (int)CollisionCategories.Geom;
//if (cp1.CollidingObj)
//cStartStop = (int)StatusIndicators.Generic;
//else
//cStartStop = (int)StatusIndicators.Start;
//returncollisions = true;
}
break;
case ActorTypes.Ground:
case ActorTypes.Unknown:
obj2LocalID = 0;
//ctype = (int)CollisionCategories.Land;
//returncollisions = true;
break;
}
cp2.AddCollisionEvent(obj2LocalID, contact);
}
break;
}
//if (returncollisions)
//{
//lock (m_storedCollisions)
//{
//cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
//if (m_storedCollisions.ContainsKey(cDictKey))
//{
//sCollisionData objd = m_storedCollisions[cDictKey];
//objd.NumberOfCollisions += 1;
//objd.lastframe = framecount;
//m_storedCollisions[cDictKey] = objd;
//}
//else
//{
//sCollisionData objd = new sCollisionData();
//objd.ColliderLocalId = obj1LocalID;
//objd.CollidedWithLocalId = obj2LocalID;
//objd.CollisionType = ctype;
//objd.NumberOfCollisions = 1;
//objd.lastframe = framecount;
//objd.StatusIndicator = cStartStop;
//m_storedCollisions.Add(cDictKey, objd);
//}
//}
// }
}
private void ChildDelink(OdePrim odePrim)
{
// m_log.DebugFormat(
// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
// Okay, we have a delinked child.. need to rebuild the body.
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
prm.childPrim = true;
prm.disableBody();
//prm.m_taintparent = null;
//prm._parent = null;
//prm.m_taintPhysics = false;
//prm.m_disabled = true;
//prm.childPrim = false;
}
}
disableBody();
lock (childrenPrim)
{
//Console.WriteLine("childrenPrim.Remove " + odePrim);
childrenPrim.Remove(odePrim);
}
// Spurious - Body == IntPtr.Zero after disableBody()
// if (Body != IntPtr.Zero)
// {
// _parent_scene.DeactivatePrim(this);
// }
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildDelink calls ParentPrim");
AddChildPrim(prm);
}
}
}
/// <summary>
/// This is called from within simulate but outside the locked portion
/// We need to do our own locking here
/// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
///
/// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
/// that the space was using.
/// </summary>
/// <param name="prim"></param>
public void RemovePrimThreadLocked(OdePrim prim)
{
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
lock (prim)
{
remCollisionEventReporting(prim);
lock (ode)
{
if (prim.prim_geom != IntPtr.Zero)
{
prim.ResetTaints();
if (prim.IsPhysical)
{
prim.disableBody();
if (prim.childPrim)
{
prim.childPrim = false;
prim.Body = IntPtr.Zero;
prim.m_disabled = true;
prim.IsPhysical = false;
}
}
// we don't want to remove the main space
// If the geometry is in the targetspace, remove it from the target space
//m_log.Warn(prim.m_targetSpace);
//if (prim.m_targetSpace != IntPtr.Zero)
//{
//if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
//{
//if (d.GeomIsSpace(prim.m_targetSpace))
//{
//waitForSpaceUnlock(prim.m_targetSpace);
//d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
prim.m_targetSpace = IntPtr.Zero;
//}
//else
//{
// m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
//((OdePrim)prim).m_targetSpace.ToString());
//}
//}
//}
//m_log.Warn(prim.prim_geom);
try
{
if (prim.prim_geom != IntPtr.Zero)
{
d.GeomDestroy(prim.prim_geom);
prim.prim_geom = IntPtr.Zero;
}
else
{
m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
}
}
catch (AccessViolationException)
{
m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
}
lock (_prims)
_prims.Remove(prim);
//If there are no more geometries in the sub-space, we don't need it in the main space anymore
//if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
//{
//if (prim.m_targetSpace != null)
//{
//if (d.GeomIsSpace(prim.m_targetSpace))
//{
//waitForSpaceUnlock(prim.m_targetSpace);
//d.SpaceRemove(space, prim.m_targetSpace);
// free up memory used by the space.
//d.SpaceDestroy(prim.m_targetSpace);
//int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
//resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
//}
//else
//{
//m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
//((OdePrim) prim).m_targetSpace.ToString());
//}
//}
//}
if (SupportsNINJAJoints)
{
RemoveAllJointsConnectedToActorThreadLocked(prim);
}
}
}
}
}
private void ChildSetGeom(OdePrim odePrim)
{
//if (IsPhysical && Body != IntPtr.Zero)
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//prm.childPrim = true;
prm.disableBody();
//prm.m_taintparent = null;
//prm._parent = null;
//prm.m_taintPhysics = false;
//prm.m_disabled = true;
//prm.childPrim = false;
}
}
disableBody();
if (Body != IntPtr.Zero)
{
_parent_scene.remActivePrim(this);
}
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildSetGeom calls ParentPrim");
AddChildPrim(prm);
}
}
}
private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
{
// obj1LocalID = 0;
//returncollisions = false;
obj2LocalID = 0;
//ctype = 0;
//cStartStop = 0;
if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
return;
switch ((ActorTypes)p1.PhysicsActorType)
{
case ActorTypes.Agent:
cc1 = (OdeCharacter)p1;
switch ((ActorTypes)p2.PhysicsActorType)
{
case ActorTypes.Agent:
cc2 = (OdeCharacter)p2;
obj2LocalID = cc2.m_localID;
if (p2.SubscribedEvents())
cc2.AddCollisionEvent(cc1.m_localID, contact);
break;
case ActorTypes.Prim:
if (p2 is OdePrim)
{
cp2 = (OdePrim)p2;
obj2LocalID = cp2.m_localID;
if (p2.SubscribedEvents())
cp2.AddCollisionEvent(cc1.m_localID, contact);
}
break;
case ActorTypes.Ground:
case ActorTypes.Unknown:
default:
obj2LocalID = 0;
break;
}
if (p1.SubscribedEvents())
{
contact.SurfaceNormal = -contact.SurfaceNormal;
cc1.AddCollisionEvent(obj2LocalID, contact);
}
break;
case ActorTypes.Prim:
if (p1 is OdePrim)
{
cp1 = (OdePrim)p1;
// obj1LocalID = cp2.m_localID;
switch ((ActorTypes)p2.PhysicsActorType)
{
case ActorTypes.Agent:
if (p2 is OdeCharacter)
{
cc2 = (OdeCharacter)p2;
obj2LocalID = cc2.m_localID;
if (p2.SubscribedEvents())
cc2.AddCollisionEvent(cp1.m_localID, contact);
}
break;
case ActorTypes.Prim:
if (p2 is OdePrim)
{
cp2 = (OdePrim)p2;
obj2LocalID = cp2.m_localID;
if (p2.SubscribedEvents())
cp2.AddCollisionEvent(cp1.m_localID, contact);
}
break;
case ActorTypes.Ground:
case ActorTypes.Unknown:
default:
obj2LocalID = 0;
break;
}
if (p1.SubscribedEvents())
{
contact.SurfaceNormal = -contact.SurfaceNormal;
cp1.AddCollisionEvent(obj2LocalID, contact);
}
}
break;
}
}
private float CalculateMass()
{
float volume = _size.X * _size.Y * _size.Z; // default
float tmp;
float returnMass = 0;
float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
float hollowVolume = hollowAmount * hollowAmount;
switch (_pbs.ProfileShape)
{
case ProfileShape.Square:
// default box
if (_pbs.PathCurve == (byte)Extrusion.Straight)
{
if (hollowAmount > 0.0)
{
switch (_pbs.HollowShape)
{
case HollowShape.Square:
case HollowShape.Same:
break;
case HollowShape.Circle:
hollowVolume *= 0.78539816339f;
break;
case HollowShape.Triangle:
hollowVolume *= (0.5f * .5f);
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
{
//a tube
volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
volume -= volume*tmp*tmp;
if (hollowAmount > 0.0)
{
hollowVolume *= hollowAmount;
switch (_pbs.HollowShape)
{
case HollowShape.Square:
case HollowShape.Same:
break;
case HollowShape.Circle:
hollowVolume *= 0.78539816339f;;
break;
case HollowShape.Triangle:
hollowVolume *= 0.5f * 0.5f;
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
break;
case ProfileShape.Circle:
if (_pbs.PathCurve == (byte)Extrusion.Straight)
{
volume *= 0.78539816339f; // elipse base
if (hollowAmount > 0.0)
{
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Circle:
break;
case HollowShape.Square:
hollowVolume *= 0.5f * 2.5984480504799f;
break;
case HollowShape.Triangle:
hollowVolume *= .5f * 1.27323954473516f;
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
{
volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
volume *= (1.0f - tmp * tmp);
if (hollowAmount > 0.0)
{
// calculate the hollow volume by it's shape compared to the prim shape
hollowVolume *= hollowAmount;
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Circle:
break;
case HollowShape.Square:
hollowVolume *= 0.5f * 2.5984480504799f;
break;
case HollowShape.Triangle:
hollowVolume *= .5f * 1.27323954473516f;
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
break;
case ProfileShape.HalfCircle:
if (_pbs.PathCurve == (byte)Extrusion.Curve1)
{
volume *= 0.52359877559829887307710723054658f;
}
break;
case ProfileShape.EquilateralTriangle:
if (_pbs.PathCurve == (byte)Extrusion.Straight)
{
volume *= 0.32475953f;
if (hollowAmount > 0.0)
{
// calculate the hollow volume by it's shape compared to the prim shape
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Triangle:
hollowVolume *= .25f;
break;
case HollowShape.Square:
hollowVolume *= 0.499849f * 3.07920140172638f;
break;
case HollowShape.Circle:
// Hollow shape is a perfect cyllinder in respect to the cube's scale
// Cyllinder hollow volume calculation
hollowVolume *= 0.1963495f * 3.07920140172638f;
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
{
volume *= 0.32475953f;
volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
volume *= (1.0f - tmp * tmp);
if (hollowAmount > 0.0)
{
hollowVolume *= hollowAmount;
switch (_pbs.HollowShape)
{
case HollowShape.Same:
case HollowShape.Triangle:
hollowVolume *= .25f;
break;
case HollowShape.Square:
hollowVolume *= 0.499849f * 3.07920140172638f;
break;
case HollowShape.Circle:
hollowVolume *= 0.1963495f * 3.07920140172638f;
break;
default:
hollowVolume = 0;
break;
}
volume *= (1.0f - hollowVolume);
}
}
break;
default:
break;
}
float taperX1;
float taperY1;
float taperX;
float taperY;
float pathBegin;
float pathEnd;
float profileBegin;
float profileEnd;
if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
{
taperX1 = _pbs.PathScaleX * 0.01f;
if (taperX1 > 1.0f)
taperX1 = 2.0f - taperX1;
taperX = 1.0f - taperX1;
taperY1 = _pbs.PathScaleY * 0.01f;
if (taperY1 > 1.0f)
taperY1 = 2.0f - taperY1;
taperY = 1.0f - taperY1;
}
else
{
taperX = _pbs.PathTaperX * 0.01f;
if (taperX < 0.0f)
taperX = -taperX;
taperX1 = 1.0f - taperX;
taperY = _pbs.PathTaperY * 0.01f;
if (taperY < 0.0f)
taperY = -taperY;
taperY1 = 1.0f - taperY;
}
volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
volume *= (pathEnd - pathBegin);
// this is crude aproximation
profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
volume *= (profileEnd - profileBegin);
returnMass = m_density * volume;
if (returnMass <= 0)
returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
// else if (returnMass > _parent_scene.maximumMassObject)
// returnMass = _parent_scene.maximumMassObject;
// Recursively calculate mass
bool HasChildPrim = false;
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
HasChildPrim = true;
}
}
if (HasChildPrim)
{
OdePrim[] childPrimArr = new OdePrim[0];
lock (childrenPrim)
childPrimArr = childrenPrim.ToArray();
for (int i = 0; i < childPrimArr.Length; i++)
{
if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
returnMass += childPrimArr[i].CalculateMass();
// failsafe, this shouldn't happen but with OpenSim, you never know :)
if (i > 256)
break;
}
}
if (returnMass > _parent_scene.maximumMassObject)
returnMass = _parent_scene.maximumMassObject;
return returnMass;
}
private PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, Quaternion rotation,
IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
{
PhysicsVector pos = new PhysicsVector(position.X, position.Y, position.Z);
//pos.X = position.X;
//pos.Y = position.Y;
//pos.Z = position.Z;
PhysicsVector siz = new PhysicsVector();
siz.X = size.X;
siz.Y = size.Y;
siz.Z = size.Z;
Quaternion rot = rotation;
OdePrim newPrim;
lock (OdeLock)
{
newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
lock (_prims)
_prims.Add(newPrim);
}
return newPrim;
}
