OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
OpenSimMirror/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs

3226 lines
117 KiB
C#
Raw Blame History

/*
* Copyright (c) Contributors, http://opensimulator.org/
* See CONTRIBUTORS.TXT for a full list of copyright holders.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the OpenSimulator Project nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
* ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
* ODEPrim.cs contains methods dealing with Prim editing, Prim
* characteristics and Kinetic motion.
* ODEDynamics.cs contains methods dealing with Prim Physical motion
* (dynamics) and the associated settings. Old Linear and angular
* motors for dynamic motion have been replace with MoveLinear()
* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
* simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
* switch between 'VEHICLE' parameter use and general dynamics
* settings use.
*/
//#define SPAM
using System;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Threading;
using log4net;
using OpenMetaverse;
using Ode.NET;
using OpenSim.Framework;
using OpenSim.Region.Physics.Manager;
namespace OpenSim.Region.Physics.OdePlugin
{
/// <summary>
/// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
/// </summary>
public class OdePrim : PhysicsActor
{
private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
private bool m_isphysical;
/// <summary>
/// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
/// </summary>
public override bool IsPhysical
{
get { return m_isphysical; }
set
{
m_isphysical = value;
if (!m_isphysical) // Zero the remembered last velocity
m_lastVelocity = Vector3.Zero;
}
}
private Vector3 _position;
private Vector3 _velocity;
private Vector3 _torque;
private Vector3 m_lastVelocity;
private Vector3 m_lastposition;
private Quaternion m_lastorientation = new Quaternion();
private Vector3 m_rotationalVelocity;
private Vector3 _size;
private Vector3 _acceleration;
// private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
private Quaternion _orientation;
private Vector3 m_taintposition;
private Vector3 m_taintsize;
private Vector3 m_taintVelocity;
private Vector3 m_taintTorque;
private Quaternion m_taintrot;
private Vector3 m_angularlock = Vector3.One;
private Vector3 m_taintAngularLock = Vector3.One;
private IntPtr Amotor = IntPtr.Zero;
private Vector3 m_PIDTarget;
private float m_PIDTau;
private float PID_D = 35f;
private float PID_G = 25f;
private bool m_usePID;
// KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
// and are for non-VEHICLES only.
private float m_PIDHoverHeight;
private float m_PIDHoverTau;
private bool m_useHoverPID;
private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
private float m_targetHoverHeight;
private float m_groundHeight;
private float m_waterHeight;
private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// private float m_tensor = 5f;
private int body_autodisable_frames = 20;
private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
| CollisionCategories.Space
| CollisionCategories.Body
| CollisionCategories.Character
);
private bool m_taintshape;
private bool m_taintPhysics;
private bool m_collidesLand = true;
private bool m_collidesWater;
// Default we're a Geometry
private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
// Default, Collide with Other Geometries, spaces and Bodies
private CollisionCategories m_collisionFlags = m_default_collisionFlags;
public bool m_taintremove { get; private set; }
public bool m_taintdisable { get; private set; }
internal bool m_disabled;
public bool m_taintadd { get; private set; }
public bool m_taintselected { get; private set; }
public bool m_taintCollidesWater { get; private set; }
public uint m_localID { get; private set; }
private bool m_taintforce = false;
private bool m_taintaddangularforce = false;
private Vector3 m_force;
private List<Vector3> m_forcelist = new List<Vector3>();
private List<Vector3> m_angularforcelist = new List<Vector3>();
private PrimitiveBaseShape _pbs;
private OdeScene _parent_scene;
/// <summary>
/// The physics space which contains prim geometries
/// </summary>
public IntPtr m_targetSpace = IntPtr.Zero;
public IntPtr prim_geom { get; private set; }
public IntPtr _triMeshData { get; private set; }
private IntPtr _linkJointGroup = IntPtr.Zero;
private PhysicsActor _parent;
private PhysicsActor m_taintparent;
private List<OdePrim> childrenPrim = new List<OdePrim>();
private bool iscolliding;
private bool m_isSelected;
internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
private bool m_throttleUpdates;
private int throttleCounter;
public int m_interpenetrationcount { get; private set; }
internal float m_collisionscore;
public int m_roundsUnderMotionThreshold { get; private set; }
private int m_crossingfailures;
public bool outofBounds { get; private set; }
private float m_density = 10.000006836f; // Aluminum g/cm3;
public bool _zeroFlag { get; private set; }
private bool m_lastUpdateSent;
public IntPtr Body = IntPtr.Zero;
public String Name { get; private set; }
private Vector3 _target_velocity;
private d.Mass pMass;
private int m_eventsubscription;
private CollisionEventUpdate CollisionEventsThisFrame;
private IntPtr m_linkJoint = IntPtr.Zero;
internal volatile bool childPrim;
private ODEDynamics m_vehicle;
internal int m_material = (int)Material.Wood;
public OdePrim(
String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
{
Name = primName;
m_vehicle = new ODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
if (!pos.IsFinite())
{
pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
}
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
if (!pos.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
}
_orientation = rotation;
m_taintrot = _orientation;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
{
IsPhysical = false;
}
else
{
IsPhysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (IsPhysical)
m_targetSpace = _parent_scene.space;
}
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
// don't do .add() here; old geoms get recycled with the same hash
}
public override int PhysicsActorType
{
get { return (int) ActorTypes.Prim; }
set { return; }
}
public override bool SetAlwaysRun
{
get { return false; }
set { return; }
}
public override uint LocalID
{
set {
//m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
m_localID = value; }
}
public override bool Grabbed
{
set { return; }
}
public override bool Selected
{
set
{
// This only makes the object not collidable if the object
// is physical or the object is modified somehow *IN THE FUTURE*
// without this, if an avatar selects prim, they can walk right
// through it while it's selected
m_collisionscore = 0;
if ((IsPhysical && !_zeroFlag) || !value)
{
m_taintselected = value;
_parent_scene.AddPhysicsActorTaint(this);
}
else
{
m_taintselected = value;
m_isSelected = value;
}
if (m_isSelected)
disableBodySoft();
}
}
/// <summary>
/// Set a new geometry for this prim.
/// </summary>
/// <param name="geom"></param>
private void SetGeom(IntPtr geom)
{
prim_geom = geom;
//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
_parent_scene.geom_name_map[prim_geom] = Name;
_parent_scene.actor_name_map[prim_geom] = this;
}
if (childPrim)
{
if (_parent != null && _parent is OdePrim)
{
OdePrim parent = (OdePrim)_parent;
//Console.WriteLine("SetGeom calls ChildSetGeom");
parent.ChildSetGeom(this);
}
}
//m_log.Warn("Setting Geom to: " + prim_geom);
}
private void enableBodySoft()
{
if (!childPrim)
{
if (IsPhysical && Body != IntPtr.Zero)
{
d.BodyEnable(Body);
if (m_vehicle.Type != Vehicle.TYPE_NONE)
m_vehicle.Enable(Body, _parent_scene);
}
m_disabled = false;
}
}
private void disableBodySoft()
{
m_disabled = true;
if (IsPhysical && Body != IntPtr.Zero)
{
d.BodyDisable(Body);
}
}
/// <summary>
/// Make a prim subject to physics.
/// </summary>
private 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);
// disconnect from world gravity so we can apply buoyancy
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.Zero, 0.0f)) && _parent == null)
{
createAMotor(m_angularlock);
}
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
m_vehicle.Enable(Body, _parent_scene);
}
_parent_scene.ActivatePrim(this);
}
}
#region Mass Calculation
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 cylinder in respect to the cube's scale
// Cylinder 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;
}
#endregion
private void setMass()
{
if (Body != (IntPtr) 0)
{
float newmass = CalculateMass();
//m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
d.BodySetMass(Body, ref pMass);
}
}
/// <summary>
/// Stop a prim from being subject to physics.
/// </summary>
internal void disableBody()
{
//this kills the body so things like 'mesh' can re-create it.
lock (this)
{
if (!childPrim)
{
if (Body != IntPtr.Zero)
{
_parent_scene.DeactivatePrim(this);
m_collisionCategories &= ~CollisionCategories.Body;
m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
d.BodyDestroy(Body);
lock (childrenPrim)
{
if (childrenPrim.Count > 0)
{
foreach (OdePrim prm in childrenPrim)
{
_parent_scene.DeactivatePrim(prm);
prm.Body = IntPtr.Zero;
}
}
}
Body = IntPtr.Zero;
}
}
else
{
_parent_scene.DeactivatePrim(this);
m_collisionCategories &= ~CollisionCategories.Body;
m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
Body = IntPtr.Zero;
}
}
m_disabled = true;
m_collisionscore = 0;
}
private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
private void setMesh(OdeScene parent_scene, IMesh mesh)
{
// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
// This sleeper is there to moderate how long it takes between
// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
//Thread.Sleep(10);
//Kill Body so that mesh can re-make the geom
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
if (m_MeshToTriMeshMap.ContainsKey(mesh))
{
_triMeshData = m_MeshToTriMeshMap[mesh];
}
else
{
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
m_MeshToTriMeshMap[mesh] = _triMeshData;
}
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
if (prim_geom == IntPtr.Zero)
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
}
}
catch (AccessViolationException)
{
m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
return;
}
// if (IsPhysical && Body == (IntPtr) 0)
// {
// Recreate the body
// m_interpenetrationcount = 0;
// m_collisionscore = 0;
// enableBody();
// }
}
internal void ProcessTaints()
{
#if SPAM
Console.WriteLine("ZProcessTaints for " + Name);
#endif
if (m_taintadd)
{
changeadd();
}
if (prim_geom != IntPtr.Zero)
{
if (!_position.ApproxEquals(m_taintposition, 0f))
changemove();
if (m_taintrot != _orientation)
{
if (childPrim && IsPhysical) // For physical child prim...
{
rotate();
// KF: ODE will also rotate the parent prim!
// so rotate the root back to where it was
OdePrim parent = (OdePrim)_parent;
parent.rotate();
}
else
{
//Just rotate the prim
rotate();
}
}
if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
changePhysicsStatus();
if (!_size.ApproxEquals(m_taintsize, 0f))
changesize();
if (m_taintshape)
changeshape();
if (m_taintforce)
changeAddForce();
if (m_taintaddangularforce)
changeAddAngularForce();
if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
changeSetTorque();
if (m_taintdisable)
changedisable();
if (m_taintselected != m_isSelected)
changeSelectedStatus();
if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
changevelocity();
if (m_taintparent != _parent)
changelink();
if (m_taintCollidesWater != m_collidesWater)
changefloatonwater();
if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
changeAngularLock();
}
else
{
m_log.ErrorFormat("[PHYSICS]: The scene reused a disposed PhysActor for {0}! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)", Name);
}
}
/// <summary>
/// Change prim in response to an angular lock taint.
/// </summary>
private void changeAngularLock()
{
// do we have a Physical object?
if (Body != IntPtr.Zero)
{
//Check that we have a Parent
//If we have a parent then we're not authorative here
if (_parent == null)
{
if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
{
//d.BodySetFiniteRotationMode(Body, 0);
//d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
createAMotor(m_taintAngularLock);
}
else
{
if (Amotor != IntPtr.Zero)
{
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
}
}
}
// Store this for later in case we get turned into a separate body
m_angularlock = m_taintAngularLock;
}
/// <summary>
/// Change prim in response to a link taint.
/// </summary>
private void changelink()
{
// If the newly set parent is not null
// create link
if (_parent == null && m_taintparent != null)
{
if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
{
OdePrim obj = (OdePrim)m_taintparent;
//obj.disableBody();
//Console.WriteLine("changelink calls ParentPrim");
obj.AddChildPrim(this);
/*
if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
{
_linkJointGroup = d.JointGroupCreate(0);
m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
d.JointAttach(m_linkJoint, obj.Body, Body);
d.JointSetFixed(m_linkJoint);
}
*/
}
}
// If the newly set parent is null
// destroy link
else if (_parent != null && m_taintparent == null)
{
//Console.WriteLine(" changelink B");
if (_parent is OdePrim)
{
OdePrim obj = (OdePrim)_parent;
obj.ChildDelink(this);
childPrim = false;
//_parent = null;
}
/*
if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
d.JointGroupDestroy(_linkJointGroup);
_linkJointGroup = (IntPtr)0;
m_linkJoint = (IntPtr)0;
*/
}
_parent = m_taintparent;
m_taintPhysics = IsPhysical;
}
/// <summary>
/// Add a child prim to this parent prim.
/// </summary>
/// <param name="prim">Child prim</param>
private 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.ActivatePrim(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.ActivatePrim(this);
}
}
}
}
}
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.DeactivatePrim(this);
}
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildSetGeom calls ParentPrim");
AddChildPrim(prm);
}
}
}
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.DeactivatePrim(this);
}
lock (childrenPrim)
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildDelink calls ParentPrim");
AddChildPrim(prm);
}
}
}
/// <summary>
/// Change prim in response to a selection taint.
/// </summary>
private void changeSelectedStatus()
{
if (m_taintselected)
{
m_collisionCategories = CollisionCategories.Selected;
m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
// We do the body disable soft twice because 'in theory' a collision could have happened
// in between the disabling and the collision properties setting
// which would wake the physical body up from a soft disabling and potentially cause it to fall
// through the ground.
// NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
// just one part of the assembly, the rest of the assembly is non-selected and still simulating,
// so that causes the selected part to wake up and continue moving.
// even if you select all parts of a jointed assembly, it is not guaranteed that the entire
// assembly will stop simulating during the selection, because of the lack of atomicity
// of select operations (their processing could be interrupted by a thread switch, causing
// simulation to continue before all of the selected object notifications trickle down to
// the physics engine).
// e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
// selected and disabled. then, due to a thread switch, the selection processing is
// interrupted and the physics engine continues to simulate, so the last 50 items, whose
// selection was not yet processed, continues to simulate. this wakes up ALL of the
// first 50 again. then the last 50 are disabled. then the first 50, which were just woken
// up, start simulating again, which in turn wakes up the last 50.
if (IsPhysical)
{
disableBodySoft();
}
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
if (IsPhysical)
{
disableBodySoft();
}
}
else
{
m_collisionCategories = CollisionCategories.Geom;
if (IsPhysical)
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags = m_default_collisionFlags;
if (m_collidesLand)
m_collisionFlags |= CollisionCategories.Land;
if (m_collidesWater)
m_collisionFlags |= CollisionCategories.Water;
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
if (IsPhysical)
{
if (Body != IntPtr.Zero)
{
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
}
}
resetCollisionAccounting();
m_isSelected = m_taintselected;
}//end changeSelectedStatus
internal void ResetTaints()
{
m_taintposition = _position;
m_taintrot = _orientation;
m_taintPhysics = IsPhysical;
m_taintselected = m_isSelected;
m_taintsize = _size;
m_taintshape = false;
m_taintforce = false;
m_taintdisable = false;
m_taintVelocity = Vector3.Zero;
}
/// <summary>
/// Create a geometry for the given mesh in the given target space.
/// </summary>
/// <param name="m_targetSpace"></param>
/// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
{
#if SPAM
Console.WriteLine("CreateGeom:");
#endif
if (mesh != null)
{
setMesh(_parent_scene, mesh);
}
else
{
if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
{
if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
{
if (((_size.X / 2f) > 0f))
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 1");
SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
}
catch (AccessViolationException)
{
m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
return;
}
}
else
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 2");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
{
m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
return;
}
}
}
else
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 3");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
{
m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
return;
}
}
}
else
{
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 4");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
{
m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
return;
}
}
}
}
/// <summary>
/// Remove the existing geom from this prim.
/// </summary>
/// <param name="m_targetSpace"></param>
/// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
/// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
internal bool RemoveGeom()
{
if (prim_geom != IntPtr.Zero)
{
try
{
_parent_scene.geom_name_map.Remove(prim_geom);
_parent_scene.actor_name_map.Remove(prim_geom);
d.GeomDestroy(prim_geom);
prim_geom = IntPtr.Zero;
}
catch (System.AccessViolationException)
{
prim_geom = IntPtr.Zero;
m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
return false;
}
return true;
}
else
{
return false;
}
}
/// <summary>
/// Add prim in response to an add taint.
/// </summary>
private 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;
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);
}
lock (_parent_scene.OdeLock)
{
#if SPAM
Console.WriteLine("changeadd 1");
#endif
CreateGeom(m_targetSpace, mesh);
if (prim_geom != IntPtr.Zero)
{
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;
}
/// <summary>
/// Move prim in response to a move taint.
/// </summary>
private void changemove()
{
if (IsPhysical)
{
if (!m_disabled && !m_taintremove && !childPrim)
{
if (Body == IntPtr.Zero)
enableBody();
//Prim auto disable after 20 frames,
//if you move it, re-enable the prim manually.
if (_parent != null)
{
if (m_linkJoint != IntPtr.Zero)
{
d.JointDestroy(m_linkJoint);
m_linkJoint = IntPtr.Zero;
}
}
if (Body != IntPtr.Zero)
{
d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
if (_parent != null)
{
OdePrim odParent = (OdePrim)_parent;
if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
{
// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
Console.WriteLine(" JointCreateFixed");
m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
d.JointAttach(m_linkJoint, Body, odParent.Body);
d.JointSetFixed(m_linkJoint);
}
}
d.BodyEnable(Body);
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
m_vehicle.Enable(Body, _parent_scene);
}
}
else
{
m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
}
}
//else
// {
//m_log.Debug("[BUG]: race!");
//}
}
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, _position, m_targetSpace);
m_targetSpace = tempspace;
_parent_scene.waitForSpaceUnlock(m_targetSpace);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
_parent_scene.waitForSpaceUnlock(m_targetSpace);
d.SpaceAdd(m_targetSpace, prim_geom);
}
}
changeSelectedStatus();
resetCollisionAccounting();
m_taintposition = _position;
}
internal void Move(float timestep)
{
float fx = 0;
float fy = 0;
float fz = 0;
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
{
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(timestep, _parent_scene);
}
else
{
//Console.WriteLine("Move " + Name);
if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
// NON-'VEHICLES' are dealt with here
// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
// {
// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
// /*
// if (m_angularlock.X == 1)
// avel2.X = 0;
// if (m_angularlock.Y == 1)
// avel2.Y = 0;
// if (m_angularlock.Z == 1)
// avel2.Z = 0;
// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
// */
// }
//float PID_P = 900.0f;
float m_mass = CalculateMass();
// fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
if (m_usePID)
{
//Console.WriteLine("PID " + Name);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if ((m_PIDTau < 1) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 pos = d.BodyGetPosition(Body);
_target_velocity =
new Vector3(
(m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
//fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
//fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
//fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fx = ((_target_velocity.X) - vel.X) * (PID_D);
fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
} // end if (m_usePID)
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + Name);
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if ((m_PIDTau < 1))
{
PID_G = PID_G / m_PIDTau;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
// Where are we, and where are we headed?
d.Vector3 pos = d.BodyGetPosition(Body);
d.Vector3 vel = d.BodyGetLinearVel(Body);
// Non-Vehicles have a limited set of Hover options.
// determine what our target height really is based on HoverType
switch (m_PIDHoverType)
{
case PIDHoverType.Ground:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_waterHeight = _parent_scene.GetWaterLevel();
if (m_groundHeight > m_waterHeight)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
} // end switch (m_PIDHoverType)
_target_velocity =
new Vector3(0.0f, 0.0f,
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
if (!d.BodyIsEnabled(Body))
{
// A physical body at rest on a surface will auto-disable after a while,
// this appears to re-enable it incase the surface it is upon vanishes,
// and the body should fall again.
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
// 35x10 = 350n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
d.BodyAddForce(Body, fx, fy, fz);
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
}
}
}
else
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + Name);
}
}
private void rotate()
{
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 (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;
}
private void resetCollisionAccounting()
{
m_collisionscore = 0;
m_interpenetrationcount = 0;
m_disabled = false;
}
/// <summary>
/// Change prim in response to a disable taint.
/// </summary>
private void changedisable()
{
m_disabled = true;
if (Body != IntPtr.Zero)
{
d.BodyDisable(Body);
Body = IntPtr.Zero;
}
m_taintdisable = false;
}
/// <summary>
/// Change prim in response to a physics status taint
/// </summary>
private void changePhysicsStatus()
{
if (IsPhysical)
{
if (Body == IntPtr.Zero)
{
if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
{
changeshape();
}
else
{
enableBody();
}
}
}
else
{
if (Body != IntPtr.Zero)
{
if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
{
RemoveGeom();
//Console.WriteLine("changePhysicsStatus for " + Name);
changeadd();
}
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
}
changeSelectedStatus();
resetCollisionAccounting();
m_taintPhysics = IsPhysical;
}
/// <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);
}
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>
/// Change prim in response to a float on water taint.
/// </summary>
/// <param name="timestep"></param>
private void changefloatonwater()
{
m_collidesWater = m_taintCollidesWater;
if (prim_geom != IntPtr.Zero)
{
if (m_collidesWater)
{
m_collisionFlags |= CollisionCategories.Water;
}
else
{
m_collisionFlags &= ~CollisionCategories.Water;
}
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
}
}
/// <summary>
/// Change prim in response to a shape taint.
/// </summary>
private void changeshape()
{
// 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);
}
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 an add force taint.
/// </summary>
private void changeAddForce()
{
if (!m_isSelected)
{
lock (m_forcelist)
{
//m_log.Info("[PHYSICS]: dequeing forcelist");
if (IsPhysical)
{
Vector3 iforce = Vector3.Zero;
int i = 0;
try
{
for (i = 0; i < m_forcelist.Count; i++)
{
iforce = iforce + (m_forcelist[i] * 100);
}
}
catch (IndexOutOfRangeException)
{
m_forcelist = new List<Vector3>();
m_collisionscore = 0;
m_interpenetrationcount = 0;
m_taintforce = false;
return;
}
catch (ArgumentOutOfRangeException)
{
m_forcelist = new List<Vector3>();
m_collisionscore = 0;
m_interpenetrationcount = 0;
m_taintforce = false;
return;
}
d.BodyEnable(Body);
d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
}
m_forcelist.Clear();
}
m_collisionscore = 0;
m_interpenetrationcount = 0;
}
m_taintforce = false;
}
/// <summary>
/// Change prim in response to a torque taint.
/// </summary>
private void changeSetTorque()
{
if (!m_isSelected)
{
if (IsPhysical && Body != IntPtr.Zero)
{
d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
}
}
m_taintTorque = Vector3.Zero;
}
/// <summary>
/// Change prim in response to an angular force taint.
/// </summary>
private void changeAddAngularForce()
{
if (!m_isSelected)
{
lock (m_angularforcelist)
{
//m_log.Info("[PHYSICS]: dequeing forcelist");
if (IsPhysical)
{
Vector3 iforce = Vector3.Zero;
for (int i = 0; i < m_angularforcelist.Count; i++)
{
iforce = iforce + (m_angularforcelist[i] * 100);
}
d.BodyEnable(Body);
d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
}
m_angularforcelist.Clear();
}
m_collisionscore = 0;
m_interpenetrationcount = 0;
}
m_taintaddangularforce = false;
}
/// <summary>
/// Change prim in response to a velocity taint.
/// </summary>
private void changevelocity()
{
if (!m_isSelected)
{
Thread.Sleep(20);
if (IsPhysical)
{
if (Body != IntPtr.Zero)
{
d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
}
}
//resetCollisionAccounting();
}
m_taintVelocity = Vector3.Zero;
}
internal void setPrimForRemoval()
{
m_taintremove = true;
}
public override bool Flying
{
// no flying prims for you
get { return false; }
set { }
}
public override bool IsColliding
{
get { return iscolliding; }
set { iscolliding = value; }
}
public override bool CollidingGround
{
get { return false; }
set { return; }
}
public override bool CollidingObj
{
get { return false; }
set { return; }
}
public override bool ThrottleUpdates
{
get { return m_throttleUpdates; }
set { m_throttleUpdates = value; }
}
public override bool Stopped
{
get { return _zeroFlag; }
}
public override Vector3 Position
{
get { return _position; }
set { _position = value;
//m_log.Info("[PHYSICS]: " + _position.ToString());
}
}
public override Vector3 Size
{
get { return _size; }
set
{
if (value.IsFinite())
{
_size = value;
// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
}
}
}
public override float Mass
{
get { return CalculateMass(); }
}
public override Vector3 Force
{
//get { return Vector3.Zero; }
get { return m_force; }
set
{
if (value.IsFinite())
{
m_force = value;
}
else
{
m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
}
}
}
public override int VehicleType
{
get { return (int)m_vehicle.Type; }
set { m_vehicle.ProcessTypeChange((Vehicle)value); }
}
public override void VehicleFloatParam(int param, float value)
{
m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
}
public override void VehicleVectorParam(int param, Vector3 value)
{
m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
}
public override void VehicleRotationParam(int param, Quaternion rotation)
{
m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
}
public override void VehicleFlags(int param, bool remove)
{
m_vehicle.ProcessVehicleFlags(param, remove);
}
public override void SetVolumeDetect(int param)
{
// We have to lock the scene here so that an entire simulate loop either uses volume detect for all
// possible collisions with this prim or for none of them.
lock (_parent_scene.OdeLock)
{
m_isVolumeDetect = (param != 0);
}
}
public override Vector3 CenterOfMass
{
get { return Vector3.Zero; }
}
public override Vector3 GeometricCenter
{
get { return Vector3.Zero; }
}
public override PrimitiveBaseShape Shape
{
set
{
_pbs = value;
m_taintshape = true;
}
}
public override Vector3 Velocity
{
get
{
// Averate previous velocity with the new one so
// client object interpolation works a 'little' better
if (_zeroFlag)
return Vector3.Zero;
Vector3 returnVelocity = Vector3.Zero;
returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
return returnVelocity;
}
set
{
if (value.IsFinite())
{
_velocity = value;
m_taintVelocity = value;
_parent_scene.AddPhysicsActorTaint(this);
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
}
}
}
public override Vector3 Torque
{
get
{
if (!IsPhysical || Body == IntPtr.Zero)
return Vector3.Zero;
return _torque;
}
set
{
if (value.IsFinite())
{
m_taintTorque = value;
_parent_scene.AddPhysicsActorTaint(this);
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
}
}
}
public override float CollisionScore
{
get { return m_collisionscore; }
set { m_collisionscore = value; }
}
public override bool Kinematic
{
get { return false; }
set { }
}
public override Quaternion Orientation
{
get { return _orientation; }
set
{
if (QuaternionIsFinite(value))
_orientation = value;
else
m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
}
}
private static bool QuaternionIsFinite(Quaternion q)
{
if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
return false;
if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
return false;
if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
return false;
if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
return false;
return true;
}
public override Vector3 Acceleration
{
get { return _acceleration; }
}
public override void AddForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
lock (m_forcelist)
m_forcelist.Add(force);
m_taintforce = true;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
}
//m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
}
public override void AddAngularForce(Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
m_angularforcelist.Add(force);
m_taintaddangularforce = true;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
}
}
public override Vector3 RotationalVelocity
{
get
{
Vector3 pv = Vector3.Zero;
if (_zeroFlag)
return pv;
m_lastUpdateSent = false;
if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
return pv;
return m_rotationalVelocity;
}
set
{
if (value.IsFinite())
{
m_rotationalVelocity = value;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
}
}
}
public override void CrossingFailure()
{
m_crossingfailures++;
if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
base.RaiseOutOfBounds(_position);
return;
}
else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
}
}
public override float Buoyancy
{
get { return m_buoyancy; }
set { m_buoyancy = value; }
}
public override void link(PhysicsActor obj)
{
m_taintparent = obj;
}
public override void delink()
{
m_taintparent = null;
}
public override void LockAngularMotion(Vector3 axis)
{
// reverse the zero/non zero values for ODE.
if (axis.IsFinite())
{
axis.X = (axis.X > 0) ? 1f : 0f;
axis.Y = (axis.Y > 0) ? 1f : 0f;
axis.Z = (axis.Z > 0) ? 1f : 0f;
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
m_taintAngularLock = axis;
}
else
{
m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
}
}
internal void UpdatePositionAndVelocity()
{
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
if (_parent == null)
{
Vector3 pv = Vector3.Zero;
bool lastZeroFlag = _zeroFlag;
if (Body != (IntPtr)0) // FIXME -> or if it is a joint
{
d.Vector3 vec = d.BodyGetPosition(Body);
d.Quaternion ori = d.BodyGetQuaternion(Body);
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 rotvel = d.BodyGetAngularVel(Body);
d.Vector3 torque = d.BodyGetTorque(Body);
_torque = new Vector3(torque.X, torque.Y, torque.Z);
Vector3 l_position = Vector3.Zero;
Quaternion l_orientation = Quaternion.Identity;
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
//if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
//if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
//if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
//if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
m_lastposition = _position;
m_lastorientation = _orientation;
l_position.X = vec.X;
l_position.Y = vec.Y;
l_position.Z = vec.Z;
l_orientation.X = ori.X;
l_orientation.Y = ori.Y;
l_orientation.Z = ori.Z;
l_orientation.W = ori.W;
if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
{
//base.RaiseOutOfBounds(l_position);
if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
_position = l_position;
//_parent_scene.remActivePrim(this);
if (_parent == null)
base.RequestPhysicsterseUpdate();
return;
}
else
{
if (_parent == null)
base.RaiseOutOfBounds(l_position);
return;
}
}
if (l_position.Z < 0)
{
// This is so prim that get lost underground don't fall forever and suck up
//
// Sim resources and memory.
// Disables the prim's movement physics....
// It's a hack and will generate a console message if it fails.
//IsPhysical = false;
if (_parent == null)
base.RaiseOutOfBounds(_position);
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_throttleUpdates = false;
throttleCounter = 0;
_zeroFlag = true;
//outofBounds = true;
}
//float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
//Console.WriteLine("Adiff " + Name + " = " + Adiff);
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
&& (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
{
_zeroFlag = true;
//Console.WriteLine("ZFT 2");
m_throttleUpdates = false;
}
else
{
//m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
_zeroFlag = false;
m_lastUpdateSent = false;
//m_throttleUpdates = false;
}
if (_zeroFlag)
{
_velocity.X = 0.0f;
_velocity.Y = 0.0f;
_velocity.Z = 0.0f;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
//_orientation.w = 0f;
//_orientation.X = 0f;
//_orientation.Y = 0f;
//_orientation.Z = 0f;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (!m_lastUpdateSent)
{
m_throttleUpdates = false;
throttleCounter = 0;
m_rotationalVelocity = pv;
if (_parent == null)
{
base.RequestPhysicsterseUpdate();
}
m_lastUpdateSent = true;
}
}
else
{
if (lastZeroFlag != _zeroFlag)
{
if (_parent == null)
{
base.RequestPhysicsterseUpdate();
}
}
m_lastVelocity = _velocity;
_position = l_position;
_velocity.X = vel.X;
_velocity.Y = vel.Y;
_velocity.Z = vel.Z;
_acceleration = ((_velocity - m_lastVelocity) / 0.1f);
_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
if (_velocity.ApproxEquals(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
}
//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
_orientation.X = ori.X;
_orientation.Y = ori.Y;
_orientation.Z = ori.Z;
_orientation.W = ori.W;
m_lastUpdateSent = false;
if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
{
if (_parent == null)
{
base.RequestPhysicsterseUpdate();
}
}
else
{
throttleCounter++;
}
}
m_lastposition = l_position;
}
else
{
// Not a body.. so Make sure the client isn't interpolating
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
_zeroFlag = true;
}
}
}
public override bool FloatOnWater
{
set {
m_taintCollidesWater = value;
_parent_scene.AddPhysicsActorTaint(this);
}
}
public override void SetMomentum(Vector3 momentum)
{
}
public override Vector3 PIDTarget
{
set
{
if (value.IsFinite())
{
m_PIDTarget = value;
}
else
m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
}
}
public override bool PIDActive { set { m_usePID = value; } }
public override float PIDTau { set { m_PIDTau = value; } }
public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
public override bool PIDHoverActive { set { m_useHoverPID = value; } }
public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
public override Quaternion APIDTarget{ set { return; } }
public override bool APIDActive{ set { return; } }
public override float APIDStrength{ set { return; } }
public override float APIDDamping{ set { return; } }
private void createAMotor(Vector3 axis)
{
if (Body == IntPtr.Zero)
return;
if (Amotor != IntPtr.Zero)
{
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
float axisnum = 3;
axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
// PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
// Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
d.Mass objMass;
d.MassSetZero(out objMass);
DMassCopy(ref pMass, ref objMass);
//m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
Matrix4 dMassMat = FromDMass(objMass);
Matrix4 mathmat = Inverse(dMassMat);
/*
//m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
mathmat = Inverse(mathmat);
objMass = FromMatrix4(mathmat, ref objMass);
//m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
mathmat = Inverse(mathmat);
*/
if (axis.X == 0)
{
mathmat.M33 = 50.0000001f;
//objMass.I.M22 = 0;
}
if (axis.Y == 0)
{
mathmat.M22 = 50.0000001f;
//objMass.I.M11 = 0;
}
if (axis.Z == 0)
{
mathmat.M11 = 50.0000001f;
//objMass.I.M00 = 0;
}
mathmat = Inverse(mathmat);
objMass = FromMatrix4(mathmat, ref objMass);
//m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
//return;
if (d.MassCheck(ref objMass))
{
d.BodySetMass(Body, ref objMass);
}
else
{
//m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
}
if (axisnum <= 0)
return;
// int dAMotorEuler = 1;
Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
d.JointAttach(Amotor, Body, IntPtr.Zero);
d.JointSetAMotorMode(Amotor, 0);
d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
int i = 0;
if (axis.X == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
i++;
}
if (axis.Y == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
i++;
}
if (axis.Z == 0)
{
d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
i++;
}
for (int j = 0; j < (int)axisnum; j++)
{
//d.JointSetAMotorAngle(Amotor, j, 0);
}
//d.JointSetAMotorAngle(Amotor, 1, 0);
//d.JointSetAMotorAngle(Amotor, 2, 0);
// These lowstops and high stops are effectively (no wiggle room)
d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
//d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
}
private Matrix4 FromDMass(d.Mass pMass)
{
Matrix4 obj;
obj.M11 = pMass.I.M00;
obj.M12 = pMass.I.M01;
obj.M13 = pMass.I.M02;
obj.M14 = 0;
obj.M21 = pMass.I.M10;
obj.M22 = pMass.I.M11;
obj.M23 = pMass.I.M12;
obj.M24 = 0;
obj.M31 = pMass.I.M20;
obj.M32 = pMass.I.M21;
obj.M33 = pMass.I.M22;
obj.M34 = 0;
obj.M41 = 0;
obj.M42 = 0;
obj.M43 = 0;
obj.M44 = 1;
return obj;
}
private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
{
obj.I.M00 = pMat[0, 0];
obj.I.M01 = pMat[0, 1];
obj.I.M02 = pMat[0, 2];
obj.I.M10 = pMat[1, 0];
obj.I.M11 = pMat[1, 1];
obj.I.M12 = pMat[1, 2];
obj.I.M20 = pMat[2, 0];
obj.I.M21 = pMat[2, 1];
obj.I.M22 = pMat[2, 2];
return obj;
}
public override void SubscribeEvents(int ms)
{
m_eventsubscription = ms;
_parent_scene.AddCollisionEventReporting(this);
}
public override void UnSubscribeEvents()
{
_parent_scene.RemoveCollisionEventReporting(this);
m_eventsubscription = 0;
}
public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
{
if (CollisionEventsThisFrame == null)
CollisionEventsThisFrame = new CollisionEventUpdate();
CollisionEventsThisFrame.addCollider(CollidedWith, contact);
}
public void SendCollisions()
{
if (CollisionEventsThisFrame == null)
return;
base.SendCollisionUpdate(CollisionEventsThisFrame);
if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
CollisionEventsThisFrame = null;
else
CollisionEventsThisFrame = new CollisionEventUpdate();
}
public override bool SubscribedEvents()
{
if (m_eventsubscription > 0)
return true;
return false;
}
public static Matrix4 Inverse(Matrix4 pMat)
{
if (determinant3x3(pMat) == 0)
{
return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
}
return (Adjoint(pMat) / determinant3x3(pMat));
}
public static Matrix4 Adjoint(Matrix4 pMat)
{
Matrix4 adjointMatrix = new Matrix4();
for (int i=0; i<4; i++)
{
for (int j=0; j<4; j++)
{
Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
}
}
adjointMatrix = Transpose(adjointMatrix);
return adjointMatrix;
}
public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
{
Matrix4 minor = new Matrix4();
int m = 0, n = 0;
for (int i = 0; i < 4; i++)
{
if (i == iRow)
continue;
n = 0;
for (int j = 0; j < 4; j++)
{
if (j == iCol)
continue;
Matrix4SetValue(ref minor, m,n, matrix[i, j]);
n++;
}
m++;
}
return minor;
}
public static Matrix4 Transpose(Matrix4 pMat)
{
Matrix4 transposeMatrix = new Matrix4();
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
return transposeMatrix;
}
public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
{
switch (r)
{
case 0:
switch (c)
{
case 0:
pMat.M11 = val;
break;
case 1:
pMat.M12 = val;
break;
case 2:
pMat.M13 = val;
break;
case 3:
pMat.M14 = val;
break;
}
break;
case 1:
switch (c)
{
case 0:
pMat.M21 = val;
break;
case 1:
pMat.M22 = val;
break;
case 2:
pMat.M23 = val;
break;
case 3:
pMat.M24 = val;
break;
}
break;
case 2:
switch (c)
{
case 0:
pMat.M31 = val;
break;
case 1:
pMat.M32 = val;
break;
case 2:
pMat.M33 = val;
break;
case 3:
pMat.M34 = val;
break;
}
break;
case 3:
switch (c)
{
case 0:
pMat.M41 = val;
break;
case 1:
pMat.M42 = val;
break;
case 2:
pMat.M43 = val;
break;
case 3:
pMat.M44 = val;
break;
}
break;
}
}
private static float determinant3x3(Matrix4 pMat)
{
float det = 0;
float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
return det;
}
private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
{
dst.c.W = src.c.W;
dst.c.X = src.c.X;
dst.c.Y = src.c.Y;
dst.c.Z = src.c.Z;
dst.mass = src.mass;
dst.I.M00 = src.I.M00;
dst.I.M01 = src.I.M01;
dst.I.M02 = src.I.M02;
dst.I.M10 = src.I.M10;
dst.I.M11 = src.I.M11;
dst.I.M12 = src.I.M12;
dst.I.M20 = src.I.M20;
dst.I.M21 = src.I.M21;
dst.I.M22 = src.I.M22;
}
public override void SetMaterial(int pMaterial)
{
m_material = pMaterial;
}
}
}
Reference in New Issue View Git Blame Copy Permalink