/* * 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. */ #region References using System; using System.Collections.Generic; using OpenMetaverse; using MonoXnaCompactMaths; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; using XnaDevRu.BulletX; using XnaDevRu.BulletX.Dynamics; using Nini.Config; using Vector3 = MonoXnaCompactMaths.Vector3; using Quaternion = MonoXnaCompactMaths.Quaternion; #endregion namespace OpenSim.Region.Physics.BulletXPlugin { /// <summary> /// BulletXConversions are called now BulletXMaths /// This Class converts objects and types for BulletX and give some operations /// </summary> public class BulletXMaths { //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); //Vector3 public static Vector3 PhysicsVectorToXnaVector3(OpenMetaverse.Vector3 physicsVector) { return new Vector3(physicsVector.X, physicsVector.Y, physicsVector.Z); } public static OpenMetaverse.Vector3 XnaVector3ToPhysicsVector(Vector3 xnaVector3) { return new OpenMetaverse.Vector3(xnaVector3.X, xnaVector3.Y, xnaVector3.Z); } //Quaternion public static Quaternion QuaternionToXnaQuaternion(OpenMetaverse.Quaternion quaternion) { return new Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W); } public static OpenMetaverse.Quaternion XnaQuaternionToQuaternion(Quaternion xnaQuaternion) { return new OpenMetaverse.Quaternion(xnaQuaternion.W, xnaQuaternion.X, xnaQuaternion.Y, xnaQuaternion.Z); } //Next methods are extracted from XnaDevRu.BulletX(See 3rd party license): //- SetRotation (class MatrixOperations) //- GetRotation (class MatrixOperations) //- GetElement (class MathHelper) //- SetElement (class MathHelper) internal static void SetRotation(ref Matrix m, Quaternion q) { float d = q.LengthSquared(); float s = 2f/d; float xs = q.X*s, ys = q.Y*s, zs = q.Z*s; float wx = q.W*xs, wy = q.W*ys, wz = q.W*zs; float xx = q.X*xs, xy = q.X*ys, xz = q.X*zs; float yy = q.Y*ys, yz = q.Y*zs, zz = q.Z*zs; m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, xy + wz, 1 - (xx + zz), yz - wx, 0, xz - wy, yz + wx, 1 - (xx + yy), 0, m.M41, m.M42, m.M43, 1); } internal static Quaternion GetRotation(Matrix m) { Quaternion q; float trace = m.M11 + m.M22 + m.M33; if (trace > 0) { float s = (float) Math.Sqrt(trace + 1); q.W = s*0.5f; s = 0.5f/s; q.X = (m.M32 - m.M23)*s; q.Y = (m.M13 - m.M31)*s; q.Z = (m.M21 - m.M12)*s; } else { q.X = q.Y = q.Z = q.W = 0f; int i = m.M11 < m.M22 ? (m.M22 < m.M33 ? 2 : 1) : (m.M11 < m.M33 ? 2 : 0); int j = (i + 1)%3; int k = (i + 2)%3; float s = (float) Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1); SetElement(ref q, i, s*0.5f); s = 0.5f/s; q.W = (GetElement(m, k, j) - GetElement(m, j, k))*s; SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j))*s); SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k))*s); } return q; } internal static float SetElement(ref Quaternion q, int index, float value) { switch (index) { case 0: q.X = value; break; case 1: q.Y = value; break; case 2: q.Z = value; break; case 3: q.W = value; break; } return 0; } internal static float GetElement(Matrix mat, int row, int col) { switch (row) { case 0: switch (col) { case 0: return mat.M11; case 1: return mat.M12; case 2: return mat.M13; } break; case 1: switch (col) { case 0: return mat.M21; case 1: return mat.M22; case 2: return mat.M23; } break; case 2: switch (col) { case 0: return mat.M31; case 1: return mat.M32; case 2: return mat.M33; } break; } return 0; } } /// <summary> /// PhysicsPlugin Class for BulletX /// </summary> public class BulletXPlugin : IPhysicsPlugin { private BulletXScene _mScene; public BulletXPlugin() { } public bool Init() { return true; } public PhysicsScene GetScene(string sceneIdentifier) { if (_mScene == null) { _mScene = new BulletXScene(sceneIdentifier); } return (_mScene); } public string GetName() { return ("modified_BulletX"); //Changed!! "BulletXEngine" To "modified_BulletX" } public void Dispose() { } } // Class to detect and debug collisions // Mainly used for debugging purposes internal class CollisionDispatcherLocal : CollisionDispatcher { private BulletXScene relatedScene; public CollisionDispatcherLocal(BulletXScene s) : base() { relatedScene = s; } public override bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) { RigidBody rb; BulletXCharacter bxcA = null; BulletXPrim bxpA = null; Type t = bodyA.GetType(); if (t == typeof (RigidBody)) { rb = (RigidBody) bodyA; relatedScene._characters.TryGetValue(rb, out bxcA); relatedScene._prims.TryGetValue(rb, out bxpA); } // String nameA; // if (bxcA != null) // nameA = bxcA._name; // else if (bxpA != null) // nameA = bxpA._name; // else // nameA = "null"; BulletXCharacter bxcB = null; BulletXPrim bxpB = null; t = bodyB.GetType(); if (t == typeof (RigidBody)) { rb = (RigidBody) bodyB; relatedScene._characters.TryGetValue(rb, out bxcB); relatedScene._prims.TryGetValue(rb, out bxpB); } // String nameB; // if (bxcB != null) // nameB = bxcB._name; // else if (bxpB != null) // nameB = bxpB._name; // else // nameB = "null"; bool needsCollision;// = base.NeedsCollision(bodyA, bodyB); int c1 = 3; int c2 = 3; //////////////////////////////////////////////////////// //BulletX Mesh Collisions //added by Jed zhu //data: May 07,2005 //////////////////////////////////////////////////////// #region BulletXMeshCollisions Fields if (bxcA != null && bxpB != null) c1 = Collision(bxcA, bxpB); if (bxpA != null && bxcB != null) c2 = Collision(bxcB, bxpA); if (c1 < 2) needsCollision = (c1 > 0) ? true : false; else if (c2 < 2) needsCollision = (c2 > 0) ? true : false; else needsCollision = base.NeedsCollision(bodyA, bodyB); #endregion //m_log.DebugFormat("[BulletX]: A collision was detected between {0} and {1} --> {2}", nameA, nameB, //needsCollision); return needsCollision; } //added by jed zhu //calculas the collision between the Prim and Actor // private int Collision(BulletXCharacter actorA, BulletXPrim primB) { int[] indexBase; Vector3[] vertexBase; Vector3 vNormal; // Vector3 vP1; // Vector3 vP2; // Vector3 vP3; IMesh mesh = primB.GetMesh(); float fdistance; if (primB == null) return 3; if (mesh == null) return 2; if (actorA == null) return 3; int iVertexCount = mesh.getVertexList().Count; int iIndexCount = mesh.getIndexListAsInt().Length; if (iVertexCount == 0) return 3; if (iIndexCount == 0) return 3; lock (BulletXScene.BulletXLock) { indexBase = mesh.getIndexListAsInt(); vertexBase = new Vector3[iVertexCount]; for (int i = 0; i < iVertexCount; i++) { OpenMetaverse.Vector3 v = mesh.getVertexList()[i]; if (v != null) // Note, null has special meaning. See meshing code for details vertexBase[i] = BulletXMaths.PhysicsVectorToXnaVector3(v); else vertexBase[i] = Vector3.Zero; } for (int ix = 0; ix < iIndexCount; ix += 3) { int ia = indexBase[ix + 0]; int ib = indexBase[ix + 1]; int ic = indexBase[ix + 2]; // Vector3 v1 = vertexBase[ib] - vertexBase[ia]; Vector3 v2 = vertexBase[ic] - vertexBase[ia]; Vector3.Cross(ref v1, ref v2, out vNormal); Vector3.Normalize(ref vNormal, out vNormal); fdistance = Vector3.Dot(vNormal, vertexBase[ia]) + 0.50f; if (preCheckCollision(actorA, vNormal, fdistance) == 1) { if (CheckCollision(actorA, ia, ib, ic, vNormal, vertexBase) == 1) { //PhysicsVector v = actorA.Position; //Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v); //Vector3 vp = vNormal * (fdistance - Vector3.Dot(vNormal, v3) + 0.2f); //actorA.Position += BulletXMaths.XnaVector3ToPhysicsVector(vp); return 1; } } } } return 0; } //added by jed zhu //return value 1: need second check //return value 0: no need check private int preCheckCollision(BulletXActor actA, Vector3 vNormal, float fDist) { float fstartSide; OpenMetaverse.Vector3 v = actA.Position; Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v); fstartSide = Vector3.Dot(vNormal, v3) - fDist; if (fstartSide > 0) return 0; else return 1; } //added by jed zhu private int CheckCollision(BulletXActor actA, int ia, int ib, int ic, Vector3 vNormal, Vector3[] vertBase) { Vector3 perPlaneNormal; float fPerPlaneDist; OpenMetaverse.Vector3 v = actA.Position; Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v); //check AB Vector3 v1; v1 = vertBase[ib] - vertBase[ia]; Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal); Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal); if (Vector3.Dot((vertBase[ic] - vertBase[ia]), perPlaneNormal) < 0) perPlaneNormal = -perPlaneNormal; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) - 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0) return 0; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) + 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0) return 0; //check BC v1 = vertBase[ic] - vertBase[ib]; Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal); Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal); if (Vector3.Dot((vertBase[ia] - vertBase[ib]), perPlaneNormal) < 0) perPlaneNormal = -perPlaneNormal; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) - 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0) return 0; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) + 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0) return 0; //check CA v1 = vertBase[ia] - vertBase[ic]; Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal); Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal); if (Vector3.Dot((vertBase[ib] - vertBase[ic]), perPlaneNormal) < 0) perPlaneNormal = -perPlaneNormal; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) - 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0) return 0; fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) + 0.50f; if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0) return 0; return 1; } } /// <summary> /// PhysicsScene Class for BulletX /// </summary> public class BulletXScene : PhysicsScene { #region BulletXScene Fields public DiscreteDynamicsWorld ddWorld; private CollisionDispatcher cDispatcher; private OverlappingPairCache opCache; private SequentialImpulseConstraintSolver sicSolver; public static Object BulletXLock = new Object(); private const int minXY = 0; private const int minZ = 0; private const int maxXY = (int)Constants.RegionSize; private const int maxZ = 4096; private const int maxHandles = 32766; //Why? I don't know private const float gravity = 9.8f; private const float heightLevel0 = 77.0f; private const float heightLevel1 = 200.0f; private const float lowGravityFactor = 0.2f; //OpenSim calls Simulate 10 times per seconds. So FPS = "Simulate Calls" * simulationSubSteps = 100 FPS private const int simulationSubSteps = 10; //private float[] _heightmap; private BulletXPlanet _simFlatPlanet; internal Dictionary<RigidBody, BulletXCharacter> _characters = new Dictionary<RigidBody, BulletXCharacter>(); internal Dictionary<RigidBody, BulletXPrim> _prims = new Dictionary<RigidBody, BulletXPrim>(); public IMesher mesher; // private IConfigSource m_config; // protected internal String identifier; public BulletXScene(String sceneIdentifier) { //identifier = sceneIdentifier; cDispatcher = new CollisionDispatcherLocal(this); Vector3 worldMinDim = new Vector3((float)minXY, (float)minXY, (float)minZ); Vector3 worldMaxDim = new Vector3((float)maxXY, (float)maxXY, (float)maxZ); opCache = new AxisSweep3(worldMinDim, worldMaxDim, maxHandles); sicSolver = new SequentialImpulseConstraintSolver(); lock (BulletXLock) { ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver); ddWorld.Gravity = new Vector3(0, 0, -gravity); } //this._heightmap = new float[65536]; } public static float Gravity { get { return gravity; } } public static float HeightLevel0 { get { return heightLevel0; } } public static float HeightLevel1 { get { return heightLevel1; } } public static float LowGravityFactor { get { return lowGravityFactor; } } public static int MaxXY { get { return maxXY; } } public static int MaxZ { get { return maxZ; } } private List<RigidBody> _forgottenRigidBodies = new List<RigidBody>(); internal string is_ex_message = "Can't remove rigidBody!: "; #endregion public BulletXScene() { cDispatcher = new CollisionDispatcherLocal(this); Vector3 worldMinDim = new Vector3((float) minXY, (float) minXY, (float) minZ); Vector3 worldMaxDim = new Vector3((float) maxXY, (float) maxXY, (float) maxZ); opCache = new AxisSweep3(worldMinDim, worldMaxDim, maxHandles); sicSolver = new SequentialImpulseConstraintSolver(); lock (BulletXLock) { ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver); ddWorld.Gravity = new Vector3(0, 0, -gravity); } //this._heightmap = new float[65536]; } public override void Initialise(IMesher meshmerizer, IConfigSource config) { mesher = meshmerizer; // m_config = config; } public override void Dispose() { } public override Dictionary<uint, float> GetTopColliders() { Dictionary<uint, float> returncolliders = new Dictionary<uint, float>(); return returncolliders; } public override void SetWaterLevel(float baseheight) { } public override PhysicsActor AddAvatar(string avName, OpenMetaverse.Vector3 position, OpenMetaverse.Vector3 size, bool isFlying) { OpenMetaverse.Vector3 pos = OpenMetaverse.Vector3.Zero; pos.X = position.X; pos.Y = position.Y; pos.Z = position.Z + 20; BulletXCharacter newAv = null; lock (BulletXLock) { newAv = new BulletXCharacter(avName, this, pos); _characters.Add(newAv.RigidBody, newAv); } newAv.Flying = isFlying; return newAv; } public override void RemoveAvatar(PhysicsActor actor) { if (actor is BulletXCharacter) { lock (BulletXLock) { try { ddWorld.RemoveRigidBody(((BulletXCharacter) actor).RigidBody); } catch (Exception ex) { BulletXMessage(is_ex_message + ex.Message, true); ((BulletXCharacter) actor).RigidBody.ActivationState = ActivationState.DisableSimulation; AddForgottenRigidBody(((BulletXCharacter) actor).RigidBody); } _characters.Remove(((BulletXCharacter) actor).RigidBody); } GC.Collect(); } } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation) { return AddPrimShape(primName, pbs, position, size, rotation, false); } public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, OpenMetaverse.Vector3 position, OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, bool isPhysical) { PhysicsActor result; switch (pbs.ProfileShape) { case ProfileShape.Square: /// support simple box & hollow box now; later, more shapes if (pbs.ProfileHollow == 0) { result = AddPrim(primName, position, size, rotation, null, null, isPhysical); } else { IMesh mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); } break; default: result = AddPrim(primName, position, size, rotation, null, null, isPhysical); break; } return result; } public PhysicsActor AddPrim(String name, OpenMetaverse.Vector3 position, OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool isPhysical) { BulletXPrim newPrim = null; lock (BulletXLock) { newPrim = new BulletXPrim(name, this, position, size, rotation, mesh, pbs, isPhysical); _prims.Add(newPrim.RigidBody, newPrim); } return newPrim; } public override void RemovePrim(PhysicsActor prim) { if (prim is BulletXPrim) { lock (BulletXLock) { try { ddWorld.RemoveRigidBody(((BulletXPrim) prim).RigidBody); } catch (Exception ex) { BulletXMessage(is_ex_message + ex.Message, true); ((BulletXPrim) prim).RigidBody.ActivationState = ActivationState.DisableSimulation; AddForgottenRigidBody(((BulletXPrim) prim).RigidBody); } _prims.Remove(((BulletXPrim) prim).RigidBody); } GC.Collect(); } } public override void AddPhysicsActorTaint(PhysicsActor prim) { } public override float Simulate(float timeStep) { float fps = 0; lock (BulletXLock) { //Try to remove garbage RemoveForgottenRigidBodies(); //End of remove MoveAPrimitives(timeStep); fps = (timeStep*simulationSubSteps); ddWorld.StepSimulation(timeStep, simulationSubSteps, timeStep); //Extra Heightmap Validation: BulletX's HeightFieldTerrain somestimes doesn't work so fine. ValidateHeightForAll(); //End heightmap validation. UpdateKineticsForAll(); } return fps; } private void MoveAPrimitives(float timeStep) { foreach (BulletXCharacter actor in _characters.Values) { actor.Move(timeStep); } } private void ValidateHeightForAll() { float _height; foreach (BulletXCharacter actor in _characters.Values) { //_height = HeightValue(actor.RigidBodyPosition); _height = _simFlatPlanet.HeightValue(actor.RigidBodyPosition); actor.ValidateHeight(_height); //if (_simFlatPlanet.heightIsNotValid(actor.RigidBodyPosition, out _height)) actor.ValidateHeight(_height); } foreach (BulletXPrim prim in _prims.Values) { //_height = HeightValue(prim.RigidBodyPosition); _height = _simFlatPlanet.HeightValue(prim.RigidBodyPosition); prim.ValidateHeight(_height); //if (_simFlatPlanet.heightIsNotValid(prim.RigidBodyPosition, out _height)) prim.ValidateHeight(_height); } //foreach (BulletXCharacter actor in _characters) //{ // actor.ValidateHeight(0); //} //foreach (BulletXPrim prim in _prims) //{ // prim.ValidateHeight(0); //} } private void UpdateKineticsForAll() { //UpdatePosition > UpdateKinetics. //Not only position will be updated, also velocity cause acceleration. foreach (BulletXCharacter actor in _characters.Values) { actor.UpdateKinetics(); } foreach (BulletXPrim prim in _prims.Values) { prim.UpdateKinetics(); } //if (this._simFlatPlanet!=null) this._simFlatPlanet.Restore(); } public override void GetResults() { } public override bool IsThreaded { get { return (false); // for now we won't be multithreaded } } public override void SetTerrain(float[] heightMap) { ////As the same as ODE, heightmap (x,y) must be swapped for BulletX //for (int i = 0; i < 65536; i++) //{ // // this._heightmap[i] = (double)heightMap[i]; // // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...) // int x = i & 0xff; // int y = i >> 8; // this._heightmap[i] = heightMap[x * 256 + y]; //} //float[] swappedHeightMap = new float[65536]; ////As the same as ODE, heightmap (x,y) must be swapped for BulletX //for (int i = 0; i < 65536; i++) //{ // // this._heightmap[i] = (double)heightMap[i]; // // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...) // int x = i & 0xff; // int y = i >> 8; // swappedHeightMap[i] = heightMap[x * 256 + y]; //} DeleteTerrain(); //There is a BulletXLock inside the constructor of BulletXPlanet //this._simFlatPlanet = new BulletXPlanet(this, swappedHeightMap); _simFlatPlanet = new BulletXPlanet(this, heightMap); //this._heightmap = heightMap; } public override void DeleteTerrain() { if (_simFlatPlanet != null) { lock (BulletXLock) { try { ddWorld.RemoveRigidBody(_simFlatPlanet.RigidBody); } catch (Exception ex) { BulletXMessage(is_ex_message + ex.Message, true); _simFlatPlanet.RigidBody.ActivationState = ActivationState.DisableSimulation; AddForgottenRigidBody(_simFlatPlanet.RigidBody); } } _simFlatPlanet = null; GC.Collect(); BulletXMessage("Terrain erased!", false); } //this._heightmap = null; } internal void AddForgottenRigidBody(RigidBody forgottenRigidBody) { _forgottenRigidBodies.Add(forgottenRigidBody); } private void RemoveForgottenRigidBodies() { RigidBody forgottenRigidBody; int nRigidBodies = _forgottenRigidBodies.Count; for (int i = nRigidBodies - 1; i >= 0; i--) { forgottenRigidBody = _forgottenRigidBodies[i]; try { ddWorld.RemoveRigidBody(forgottenRigidBody); _forgottenRigidBodies.Remove(forgottenRigidBody); BulletXMessage("Forgotten Rigid Body Removed", false); } catch (Exception ex) { BulletXMessage("Can't remove forgottenRigidBody!: " + ex.Message, false); } } GC.Collect(); } internal static void BulletXMessage(string message, bool isWarning) { PhysicsPluginManager.PhysicsPluginMessage("[Modified BulletX]:\t" + message, isWarning); } //temp //private float HeightValue(MonoXnaCompactMaths.Vector3 position) //{ // int li_x, li_y; // float height; // li_x = (int)Math.Round(position.X); if (li_x < 0) li_x = 0; // li_y = (int)Math.Round(position.Y); if (li_y < 0) li_y = 0; // height = this._heightmap[li_y * 256 + li_x]; // if (height < 0) height = 0; // else if (height > maxZ) height = maxZ; // return height; //} } /// <summary> /// Generic Physics Actor for BulletX inherit from PhysicActor /// </summary> public class BulletXActor : PhysicsActor { protected bool flying = false; protected bool _physical = false; protected OpenMetaverse.Vector3 _position; protected OpenMetaverse.Vector3 _velocity; protected OpenMetaverse.Vector3 _size; protected OpenMetaverse.Vector3 _acceleration; protected OpenMetaverse.Quaternion _orientation; protected OpenMetaverse.Vector3 m_rotationalVelocity; protected RigidBody rigidBody; protected int m_PhysicsActorType; private Boolean iscolliding = false; internal string _name; public BulletXActor(String name) { _name = name; } public override bool Stopped { get { return false; } } public override OpenMetaverse.Vector3 Position { get { return _position; } set { lock (BulletXScene.BulletXLock) { _position = value; Translate(); } } } public override OpenMetaverse.Vector3 RotationalVelocity { get { return m_rotationalVelocity; } set { m_rotationalVelocity = value; } } public override OpenMetaverse.Vector3 Velocity { get { return _velocity; } set { lock (BulletXScene.BulletXLock) { //Static objects don' have linear velocity if (_physical) { _velocity = value; Speed(); } else { _velocity = OpenMetaverse.Vector3.Zero; } } } } public override float CollisionScore { get { return 0f; } set { } } public override OpenMetaverse.Vector3 Size { get { return _size; } set { lock (BulletXScene.BulletXLock) { _size = value; } } } public override OpenMetaverse.Vector3 Force { get { return OpenMetaverse.Vector3.Zero; } set { return; } } public override int VehicleType { get { return 0; } set { return; } } public override void VehicleFloatParam(int param, float value) { } public override void VehicleVectorParam(int param, OpenMetaverse.Vector3 value) { } public override void VehicleRotationParam(int param, OpenMetaverse.Quaternion rotation) { } public override void VehicleFlags(int param, bool remove) { } public override void SetVolumeDetect(int param) { } public override OpenMetaverse.Vector3 CenterOfMass { get { return OpenMetaverse.Vector3.Zero; } } public override OpenMetaverse.Vector3 GeometricCenter { get { return OpenMetaverse.Vector3.Zero; } } public override PrimitiveBaseShape Shape { set { return; } } public override bool SetAlwaysRun { get { return false; } set { return; } } public override OpenMetaverse.Vector3 Acceleration { get { return _acceleration; } } public override OpenMetaverse.Quaternion Orientation { get { return _orientation; } set { lock (BulletXScene.BulletXLock) { _orientation = value; ReOrient(); } } } public override void link(PhysicsActor obj) { } public override void delink() { } public override void LockAngularMotion(OpenMetaverse.Vector3 axis) { } public override float Mass { get { return ActorMass; } } public virtual float ActorMass { get { return 0; } } public override int PhysicsActorType { get { return (int) m_PhysicsActorType; } set { m_PhysicsActorType = value; } } public RigidBody RigidBody { get { return rigidBody; } } public Vector3 RigidBodyPosition { get { return rigidBody.CenterOfMassPosition; } } public override bool IsPhysical { get { return _physical; } set { _physical = value; } } public override bool Flying { get { return flying; } set { flying = value; } } public override bool ThrottleUpdates { get { return false; } set { return; } } 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 uint LocalID { set { return; } } public override bool Grabbed { set { return; } } public override bool Selected { set { return; } } public override float Buoyancy { get { return 0f; } set { return; } } public override bool FloatOnWater { set { return; } } public virtual void SetAcceleration(OpenMetaverse.Vector3 accel) { lock (BulletXScene.BulletXLock) { _acceleration = accel; } } public override bool Kinematic { get { return false; } set { } } public override void AddForce(OpenMetaverse.Vector3 force, bool pushforce) { } public override OpenMetaverse.Vector3 Torque { get { return OpenMetaverse.Vector3.Zero; } set { return; } } public override void AddAngularForce(OpenMetaverse.Vector3 force, bool pushforce) { } public override void SetMomentum(OpenMetaverse.Vector3 momentum) { } internal virtual void ValidateHeight(float heighmapPositionValue) { } internal virtual void UpdateKinetics() { } #region Methods for updating values of RigidBody protected internal void Translate() { Translate(_position); } protected internal void Translate(OpenMetaverse.Vector3 _newPos) { Vector3 _translation; _translation = BulletXMaths.PhysicsVectorToXnaVector3(_newPos) - rigidBody.CenterOfMassPosition; rigidBody.Translate(_translation); } protected internal void Speed() { Speed(_velocity); } protected internal void Speed(OpenMetaverse.Vector3 _newSpeed) { Vector3 _speed; _speed = BulletXMaths.PhysicsVectorToXnaVector3(_newSpeed); rigidBody.LinearVelocity = _speed; } protected internal void ReOrient() { ReOrient(_orientation); } protected internal void ReOrient(OpenMetaverse.Quaternion _newOrient) { Quaternion _newOrientation; _newOrientation = BulletXMaths.QuaternionToXnaQuaternion(_newOrient); Matrix _comTransform = rigidBody.CenterOfMassTransform; BulletXMaths.SetRotation(ref _comTransform, _newOrientation); rigidBody.CenterOfMassTransform = _comTransform; } protected internal void ReSize() { ReSize(_size); } protected internal virtual void ReSize(OpenMetaverse.Vector3 _newSize) { } public virtual void ScheduleTerseUpdate() { base.RequestPhysicsterseUpdate(); } #endregion public override void CrossingFailure() { } public override OpenMetaverse.Vector3 PIDTarget { set { return; } } public override bool PIDActive { set { return; } } public override float PIDTau { set { return; } } public override float PIDHoverHeight { set { return; } } public override bool PIDHoverActive { set { return; } } public override PIDHoverType PIDHoverType { set { return; } } public override float PIDHoverTau { set { return; } } public override OpenMetaverse.Quaternion APIDTarget { set { return; } } public override bool APIDActive { set { return; } } public override float APIDStrength { set { return; } } public override float APIDDamping { set { return; } } public override void SubscribeEvents(int ms) { } public override void UnSubscribeEvents() { } public override bool SubscribedEvents() { return false; } } /// <summary> /// PhysicsActor Character Class for BulletX /// </summary> public class BulletXCharacter : BulletXActor { public BulletXCharacter(BulletXScene parent_scene, OpenMetaverse.Vector3 pos) : this(String.Empty, parent_scene, pos) { } public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos) : this(avName, parent_scene, pos, OpenMetaverse.Vector3.Zero, OpenMetaverse.Vector3.Zero, OpenMetaverse.Vector3.Zero, OpenMetaverse.Quaternion.Identity) { } public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity, OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation) : base(avName) { //This fields will be removed. They're temporal float _sizeX = 0.5f; float _sizeY = 0.5f; float _sizeZ = 1.6f; //. _position = pos; _velocity = velocity; _size = size; //--- _size.X = _sizeX; _size.Y = _sizeY; _size.Z = _sizeZ; //. _acceleration = acceleration; _orientation = orientation; _physical = true; float _mass = 50.0f; //This depends of avatar's dimensions //For RigidBody Constructor. The next values might change float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 0.5f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; lock (BulletXScene.BulletXLock) { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos); //CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f)); //For now, like ODE, collisionShape = sphere of radious = 1.0 CollisionShape _collisionShape = new SphereShape(1.0f); DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0 rigidBody = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //rigidBody.ActivationState = ActivationState.DisableDeactivation; //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; rigidBody.Translate(_vDebugTranslation); parent_scene.ddWorld.AddRigidBody(rigidBody); } } public override int PhysicsActorType { get { return (int) ActorTypes.Agent; } set { return; } } public override OpenMetaverse.Vector3 Position { get { return base.Position; } set { base.Position = value; } } public override OpenMetaverse.Vector3 Velocity { get { return base.Velocity; } set { base.Velocity = value; } } public override OpenMetaverse.Vector3 Size { get { return base.Size; } set { base.Size = value; } } public override OpenMetaverse.Vector3 Acceleration { get { return base.Acceleration; } } public override OpenMetaverse.Quaternion Orientation { get { return base.Orientation; } set { base.Orientation = value; } } public override bool Flying { get { return base.Flying; } set { base.Flying = value; } } public override bool IsColliding { get { return base.IsColliding; } set { base.IsColliding = value; } } public override bool Kinematic { get { return base.Kinematic; } set { base.Kinematic = value; } } public override void SetAcceleration(OpenMetaverse.Vector3 accel) { base.SetAcceleration(accel); } public override void AddForce(OpenMetaverse.Vector3 force, bool pushforce) { base.AddForce(force, pushforce); } public override void SetMomentum(OpenMetaverse.Vector3 momentum) { base.SetMomentum(momentum); } internal void Move(float timeStep) { Vector3 vec = new Vector3(); //At this point it's supossed that: //_velocity == rigidBody.LinearVelocity vec.X = _velocity.X; vec.Y = _velocity.Y; vec.Z = _velocity.Z; if ((vec.X != 0.0f) || (vec.Y != 0.0f) || (vec.Z != 0.0f)) rigidBody.Activate(); if (flying) { //Antigravity with movement if (_position.Z <= BulletXScene.HeightLevel0) { vec.Z += BulletXScene.Gravity*timeStep; } //Lowgravity with movement else if ((_position.Z > BulletXScene.HeightLevel0) && (_position.Z <= BulletXScene.HeightLevel1)) { vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor); } //Lowgravity with... else if (_position.Z > BulletXScene.HeightLevel1) { if (vec.Z > 0) //no movement vec.Z = BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor); else vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor); } } rigidBody.LinearVelocity = vec; } //This validation is very basic internal override void ValidateHeight(float heighmapPositionValue) { if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z/2.0f) { Matrix m = rigidBody.WorldTransform; Vector3 v3 = m.Translation; v3.Z = heighmapPositionValue + _size.Z/2.0f; m.Translation = v3; rigidBody.WorldTransform = m; //When an Avie touch the ground it's vertical velocity it's reduced to ZERO Speed(new OpenMetaverse.Vector3(rigidBody.LinearVelocity.X, rigidBody.LinearVelocity.Y, 0.0f)); } } internal override void UpdateKinetics() { _position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition); _velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity); //Orientation it seems that it will be the default. ReOrient(); } } /// <summary> /// PhysicsActor Prim Class for BulletX /// </summary> public class BulletXPrim : BulletXActor { //Density it will depends of material. //For now all prims have the same density, all prims are made of water. Be water my friend! :D private const float _density = 1000.0f; private BulletXScene _parent_scene; private OpenMetaverse.Vector3 m_prev_position; private bool m_lastUpdateSent = false; //added by jed zhu private IMesh _mesh; public IMesh GetMesh() { return _mesh; } public BulletXPrim(String primName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 size, OpenMetaverse.Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool isPhysical) : this( primName, parent_scene, pos, OpenMetaverse.Vector3.Zero, size, OpenMetaverse.Vector3.Zero, rotation, mesh, pbs, isPhysical) { } public BulletXPrim(String primName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity, OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool isPhysical) : base(primName) { if ((size.X == 0) || (size.Y == 0) || (size.Z == 0)) throw new Exception("Size 0"); if (OpenMetaverse.Quaternion.Normalize(rotation).Length() == 0f) rotation = OpenMetaverse.Quaternion.Identity; _position = pos; _physical = isPhysical; _velocity = _physical ? velocity : OpenMetaverse.Vector3.Zero; _size = size; _acceleration = acceleration; _orientation = rotation; _parent_scene = parent_scene; CreateRigidBody(parent_scene, mesh, pos, size); } public override int PhysicsActorType { get { return (int) ActorTypes.Prim; } set { return; } } public override OpenMetaverse.Vector3 Position { get { return base.Position; } set { base.Position = value; } } public override OpenMetaverse.Vector3 Velocity { get { return base.Velocity; } set { base.Velocity = value; } } public override OpenMetaverse.Vector3 Size { get { return _size; } set { lock (BulletXScene.BulletXLock) { _size = value; ReSize(); } } } public override OpenMetaverse.Vector3 Acceleration { get { return base.Acceleration; } } public override OpenMetaverse.Quaternion Orientation { get { return base.Orientation; } set { base.Orientation = value; } } public override float ActorMass { get { //For now all prims are boxes return (_physical ? 1 : 0)*_density*_size.X*_size.Y*_size.Z; } } public override bool IsPhysical { get { return base.IsPhysical; } set { base.IsPhysical = value; if (value) { //--- PhysicsPluginManager.PhysicsPluginMessage("Physical - Recreate", true); //--- ReCreateRigidBody(_size); } else { //--- PhysicsPluginManager.PhysicsPluginMessage("Physical - SetMassProps", true); //--- rigidBody.SetMassProps(Mass, new Vector3()); } } } public override bool Flying { get { return base.Flying; } set { base.Flying = value; } } public override bool IsColliding { get { return base.IsColliding; } set { base.IsColliding = value; } } public override bool Kinematic { get { return base.Kinematic; } set { base.Kinematic = value; } } public override void SetAcceleration(OpenMetaverse.Vector3 accel) { lock (BulletXScene.BulletXLock) { _acceleration = accel; } } public override void AddForce(OpenMetaverse.Vector3 force, bool pushforce) { base.AddForce(force,pushforce); } public override void SetMomentum(OpenMetaverse.Vector3 momentum) { base.SetMomentum(momentum); } internal override void ValidateHeight(float heighmapPositionValue) { if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z/2.0f) { Matrix m = rigidBody.WorldTransform; Vector3 v3 = m.Translation; v3.Z = heighmapPositionValue + _size.Z/2.0f; m.Translation = v3; rigidBody.WorldTransform = m; //When a Prim touch the ground it's vertical velocity it's reduced to ZERO //Static objects don't have linear velocity if (_physical) Speed(new OpenMetaverse.Vector3(rigidBody.LinearVelocity.X, rigidBody.LinearVelocity.Y, 0.0f)); } } internal override void UpdateKinetics() { if (_physical) //Updates properties. Prim updates its properties physically { _position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition); _velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity); _orientation = BulletXMaths.XnaQuaternionToQuaternion(rigidBody.Orientation); if ((Math.Abs(m_prev_position.X - _position.X) < 0.03) && (Math.Abs(m_prev_position.Y - _position.Y) < 0.03) && (Math.Abs(m_prev_position.Z - _position.Z) < 0.03)) { if (!m_lastUpdateSent) { _velocity = OpenMetaverse.Vector3.Zero; base.ScheduleTerseUpdate(); m_lastUpdateSent = true; } } else { m_lastUpdateSent = false; base.ScheduleTerseUpdate(); } m_prev_position = _position; } else //Doesn't updates properties. That's a cancel { Translate(); //Speed(); //<- Static objects don't have linear velocity ReOrient(); } } #region Methods for updating values of RigidBody protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 size) { //For RigidBody Constructor. The next values might change float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 1.0f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; //added by jed zhu _mesh = mesh; lock (BulletXScene.BulletXLock) { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos); //For now all prims are boxes CollisionShape _collisionShape; if (mesh == null) { _collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f); } else { int iVertexCount = mesh.getVertexList().Count; int[] indices = mesh.getIndexListAsInt(); Vector3[] v3Vertices = new Vector3[iVertexCount]; for (int i = 0; i < iVertexCount; i++) { OpenMetaverse.Vector3 v = mesh.getVertexList()[i]; if (v != null) // Note, null has special meaning. See meshing code for details v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v); else v3Vertices[i] = Vector3.Zero; } TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices); _collisionShape = new TriangleMeshShape(triMesh); } DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0 rigidBody = new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //rigidBody.ActivationState = ActivationState.DisableDeactivation; //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; rigidBody.Translate(_vDebugTranslation); //--- parent_scene.ddWorld.AddRigidBody(rigidBody); } } protected internal void ReCreateRigidBody(OpenMetaverse.Vector3 size) { //There is a bug when trying to remove a rigidBody that is colliding with something.. try { _parent_scene.ddWorld.RemoveRigidBody(rigidBody); } catch (Exception ex) { BulletXScene.BulletXMessage(_parent_scene.is_ex_message + ex.Message, true); rigidBody.ActivationState = ActivationState.DisableSimulation; _parent_scene.AddForgottenRigidBody(rigidBody); } CreateRigidBody(_parent_scene, null, _position, size); // Note, null for the meshing definitely is wrong. It's here for the moment to apease the compiler if (_physical) Speed(); //Static objects don't have linear velocity ReOrient(); GC.Collect(); } protected internal override void ReSize(OpenMetaverse.Vector3 _newSize) { //I wonder to know how to resize with a simple instruction in BulletX. It seems that for now there isn't //so i have to do it manually. That's recreating rigidbody ReCreateRigidBody(_newSize); } #endregion } /// <summary> /// This Class manage a HeighField as a RigidBody. This is for to be added in the BulletXScene /// </summary> internal class BulletXPlanet { private OpenMetaverse.Vector3 _staticPosition; // private Vector3 _staticVelocity; // private OpenMetaverse.Quaternion _staticOrientation; private float _mass; // private BulletXScene _parentscene; internal float[] _heightField; private RigidBody _flatPlanet; internal RigidBody RigidBody { get { return _flatPlanet; } } internal BulletXPlanet(BulletXScene parent_scene, float[] heightField) { _staticPosition = new OpenMetaverse.Vector3(BulletXScene.MaxXY / 2, BulletXScene.MaxXY / 2, 0); // _staticVelocity = new PhysicsVector(); // _staticOrientation = OpenMetaverse.Quaternion.Identity; _mass = 0; //No active // _parentscene = parent_scene; _heightField = heightField; float _linearDamping = 0.0f; float _angularDamping = 0.0f; float _friction = 0.5f; float _restitution = 0.0f; Matrix _startTransform = Matrix.Identity; Matrix _centerOfMassOffset = Matrix.Identity; lock (BulletXScene.BulletXLock) { try { _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition); CollisionShape _collisionShape = new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField, (float) BulletXScene.MaxZ, 2, true, false); DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); Vector3 _localInertia = new Vector3(); //_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0 _flatPlanet = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition Vector3 _vDebugTranslation; _vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition; _flatPlanet.Translate(_vDebugTranslation); parent_scene.ddWorld.AddRigidBody(_flatPlanet); } catch (Exception ex) { BulletXScene.BulletXMessage(ex.Message, true); } } BulletXScene.BulletXMessage("BulletXPlanet created.", false); } internal float HeightValue(Vector3 position) { int li_x, li_y; float height; li_x = (int) Math.Round(position.X); if (li_x < 0) li_x = 0; if (li_x >= BulletXScene.MaxXY) li_x = BulletXScene.MaxXY - 1; li_y = (int) Math.Round(position.Y); if (li_y < 0) li_y = 0; if (li_y >= BulletXScene.MaxXY) li_y = BulletXScene.MaxXY - 1; height = ((HeightfieldTerrainShape) _flatPlanet.CollisionShape).getHeightFieldValue(li_x, li_y); if (height < 0) height = 0; else if (height > BulletXScene.MaxZ) height = BulletXScene.MaxZ; return height; } } }