diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index af13d0211e..705eb38285 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -30,18 +30,28 @@
#region References
using System;
using System.Collections.Generic;
-using OpenSim.Framework.Types;
using OpenSim.Region.Physics.Manager;
+using OpenSim.Framework.Types;
using Axiom.Math;
using AxiomQuaternion = Axiom.Math.Quaternion;
//Specific References for BulletXPlugin
-using MonoXnaCompactMaths; //Called as MXCM
+using MonoXnaCompactMaths;
using XnaDevRu.BulletX;
using XnaDevRu.BulletX.Dynamics;
+
#endregion
namespace OpenSim.Region.Physics.BulletXPlugin
{
+ ///
+ /// This class is only here for compilations reasons
+ ///
+ public class Mesh
+ {
+ public Mesh()
+ {
+ }
+ }
///
/// BulletXConversions are called now BulletXMaths
/// This Class converts objects and types for BulletX and give some operations
@@ -222,12 +232,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
private const int maxXY = 256;
private const int maxZ = 4096;
private const int maxHandles = 32766; //Why? I don't know
- private static float gravity = 9.8f;
- private static float heightLevel0 = 77.0f;
- private static float heightLevel1 = 200.0f;
- private static float lowGravityFactor = 0.2f;
-
- private float[] _heightmap;
+ 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;
private List _characters = new List();
private List _prims = new List();
@@ -235,6 +247,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
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 _forgottenRigidBodies = new List();
+ internal string is_ex_message = "Can't remove rigidBody!: ";
#endregion
public BulletXScene()
@@ -250,10 +267,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
ddWorld.Gravity = new MonoXnaCompactMaths.Vector3(0, 0, -gravity);
}
-
- this._heightmap = new float[65536];
+ //this._heightmap = new float[65536];
}
- public override PhysicsActor AddAvatar(string avName, PhysicsVector position)
+ public override PhysicsActor AddAvatar(string avName, PhysicsVector position)
{
PhysicsVector pos = new PhysicsVector();
pos.X = position.X;
@@ -262,7 +278,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
BulletXCharacter newAv = null;
lock (BulletXLock)
{
- newAv = new BulletXCharacter(this, pos);
+ newAv = new BulletXCharacter(avName, this, pos);
_characters.Add(newAv);
}
return newAv;
@@ -273,48 +289,98 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{
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);
}
+ GC.Collect();
}
}
- PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
+ public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
+ {
+ 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);
+ }
+ else
+ {
+ Mesh mesh = null;
+ result = AddPrim(primName, position, size, rotation, mesh, pbs);
+ }
+ break;
+
+ default:
+ result = AddPrim(primName, position, size, rotation, null, null);
+ break;
+ }
+
+ return result;
+ }
+ public PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, Axiom.Math.Quaternion rotation)
+ {
+ return AddPrim("", position, size, rotation, null, null);
+ }
+ public PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
{
BulletXPrim newPrim = null;
lock (BulletXLock)
{
- newPrim = new BulletXPrim(this, position, size, rotation);
+ newPrim = new BulletXPrim(name, this, position, size, rotation, mesh, pbs);
_prims.Add(newPrim);
}
return newPrim;
}
- public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation)
- {
- return AddPrim(position, size, rotation);
- }
-
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);
}
+ GC.Collect();
}
}
public override void Simulate(float timeStep)
{
lock (BulletXLock)
{
- BXSMove(timeStep);
- ddWorld.StepSimulation(timeStep, 0, timeStep);
- //Heightmap Validation:
- BXSValidateHeight();
+ //Try to remove garbage
+ RemoveForgottenRigidBodies();
+ //End of remove
+ MoveAllObjects(timeStep);
+ ddWorld.StepSimulation(timeStep, simulationSubSteps, timeStep);
+ //Extra Heightmap Validation: BulletX's HeightFieldTerrain somestimes doesn't work so fine.
+ ValidateHeightForAll();
//End heightmap validation.
- BXSUpdateKinetics();
+ UpdateKineticsForAll();
}
}
- private void BXSMove(float timeStep)
+ private void MoveAllObjects(float timeStep)
{
foreach (BulletXCharacter actor in _characters)
{
@@ -324,39 +390,33 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{
}
}
- private void BXSValidateHeight()
+ private void ValidateHeightForAll()
{
float _height;
foreach (BulletXCharacter actor in _characters)
{
- if ((actor.RigidBodyHorizontalPosition.x < 0) || (actor.RigidBodyHorizontalPosition.y < 0))
- {
- _height = 0;
- }
- else
- {
- _height = this._heightmap[
- (int)Math.Round(actor.RigidBodyHorizontalPosition.x) * 256
- + (int)Math.Round(actor.RigidBodyHorizontalPosition.y)];
- }
+ //_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)
{
- if ((prim.RigidBodyHorizontalPosition.x < 0) || (prim.RigidBodyHorizontalPosition.y < 0))
- {
- _height = 0;
- }
- else
- {
- _height = this._heightmap[
- (int)Math.Round(prim.RigidBodyHorizontalPosition.x) * 256
- + (int)Math.Round(prim.RigidBodyHorizontalPosition.y)];
- }
+ //_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 BXSUpdateKinetics()
+ private void UpdateKineticsForAll()
{
//UpdatePosition > UpdateKinetics.
//Not only position will be updated, also velocity cause acceleration.
@@ -368,6 +428,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{
prim.UpdateKinetics();
}
+ //if(this._simFlatPlanet!=null) this._simFlatPlanet.Restore();
}
public override void GetResults()
{
@@ -382,24 +443,97 @@ namespace OpenSim.Region.Physics.BulletXPlugin
}
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];
- }
- lock (BulletXLock)
- {
- //Updating BulletX 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);
+ this._simFlatPlanet = new BulletXPlanet(this, heightMap);
+ //this._heightmap = heightMap;
}
public override void DeleteTerrain()
{
-
+ if (this._simFlatPlanet != null)
+ {
+ lock (BulletXLock)
+ {
+ try
+ {
+ ddWorld.RemoveRigidBody(this._simFlatPlanet.RigidBody);
+ }
+ catch (Exception ex)
+ {
+ BulletXMessage(is_ex_message + ex.Message, true);
+ this._simFlatPlanet.RigidBody.ActivationState = ActivationState.DisableSimulation;
+ AddForgottenRigidBody(this._simFlatPlanet.RigidBody);
+ }
+ }
+ this._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 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;
+ //}
}
///
/// PhysicsActor Character Class for BulletX
@@ -414,19 +548,20 @@ namespace OpenSim.Region.Physics.BulletXPlugin
private bool flying;
private RigidBody rigidBody;
- public Axiom.Math.Vector2 RigidBodyHorizontalPosition
+ public MonoXnaCompactMaths.Vector3 RigidBodyPosition
{
- get
- {
- return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
- }
+ get { return this.rigidBody.CenterOfMassPosition; }
}
public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos)
- : this(parent_scene, pos, new PhysicsVector(), new PhysicsVector(), new PhysicsVector(),
+ : this("", parent_scene, pos)
+ {
+ }
+ public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos)
+ : this(avName, parent_scene, pos, new PhysicsVector(), new PhysicsVector(), new PhysicsVector(),
AxiomQuaternion.Identity)
{
}
- public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
+ public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
PhysicsVector size, PhysicsVector acceleration, AxiomQuaternion orientation)
{
//This fields will be removed. They're temporal
@@ -462,7 +597,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.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;
+ //rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
MonoXnaCompactMaths.Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
@@ -535,6 +670,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin
}
}
}
+ public RigidBody RigidBody
+ {
+ get
+ {
+ return rigidBody;
+ }
+ }
public override bool Flying
{
get
@@ -579,8 +721,8 @@ namespace OpenSim.Region.Physics.BulletXPlugin
//_velocity == rigidBody.LinearVelocity
vec.X = this._velocity.X;
vec.Y = this._velocity.Y;
- vec.Z = this._velocity.Z;
-
+ vec.Z = this._velocity.Z;
+ if ((vec.X != 0.0f) || (vec.Y != 0.0f) || (vec.Z != 0.0f)) rigidBody.Activate();
if (flying)
{
//Antigravity with movement
@@ -677,33 +819,37 @@ namespace OpenSim.Region.Physics.BulletXPlugin
//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 RigidBody rigidBody;
+ private BulletXScene _parent_scene;
//_physical value will be linked with the prim object value
private Boolean _physical = false;
- public Axiom.Math.Vector2 RigidBodyHorizontalPosition
+ public MonoXnaCompactMaths.Vector3 RigidBodyPosition
{
- get
- {
- return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
- }
+ get { return this.rigidBody.CenterOfMassPosition; }
}
public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size, AxiomQuaternion rotation)
- : this(parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation)
+ : this("", parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation, null, null)
{
}
- public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity, PhysicsVector size,
- PhysicsVector aceleration, AxiomQuaternion rotation)
+ public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size,
+ AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
+ : this(primName, parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation, mesh, pbs)
+ {
+ }
+ public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity, PhysicsVector size,
+ PhysicsVector aceleration, AxiomQuaternion rotation, Mesh mesh, PrimitiveBaseShape pbs)
{
- _position = pos;
- _velocity = velocity;
- _size = size;
if ((size.X == 0) || (size.Y == 0) || (size.Z == 0)) throw new Exception("Size 0");
+ if (rotation.Norm == 0f) rotation = AxiomQuaternion.Identity;
+ _position = pos;
+ if (_physical) _velocity = velocity; else _velocity = new PhysicsVector();
+ _size = size;
_acceleration = aceleration;
- //Because a bug, orientation will be fixed to AxiomQuaternion.Identity
- _orientation = AxiomQuaternion.Identity;
- //_orientation = rotation;
- //---
+ _orientation = rotation;
+
+ _parent_scene = parent_scene;
+
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
@@ -718,9 +864,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
CollisionShape _collisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_size) / 2.0f);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
- _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
+ 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;
+ //rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
MonoXnaCompactMaths.Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
@@ -754,8 +900,16 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{
lock (BulletXScene.BulletXLock)
{
- _velocity = value;
- Speed();
+ //Static objects don' have linear velocity
+ if (_physical)
+ {
+ _velocity = value;
+ Speed();
+ }
+ else
+ {
+ _velocity = new PhysicsVector();
+ }
}
}
}
@@ -801,7 +955,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
get
{
//For now all prims are boxes
- return _density * _size.X * _size.Y * _size.Z;
+ return (_physical ? 1 : 0) * _density * _size.X * _size.Y * _size.Z;
+ }
+ }
+ public RigidBody RigidBody
+ {
+ get
+ {
+ return rigidBody;
}
}
public override bool Flying
@@ -863,7 +1024,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin
m.Translation = v3;
rigidBody.WorldTransform = m;
//When a Prim touch the ground it's vertical velocity it's reduced to ZERO
- Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
+ //Static objects don't have linear velocity
+ if(_physical)
+ Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
}
}
internal void UpdateKinetics()
@@ -872,18 +1035,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{
this._position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
this._velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
- //Orientation is not implemented yet in MonoXnaCompactMaths
- //this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation); < Good
- //ReOrient();
- //---
- ReOrient();
+ this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation);
}
else //Doesn't updates properties. That's a cancel
{
Translate();
- Speed();
- //Orientation is not implemented yet in MonoXnaCompactMaths
- //ReOrient();
+ //Speed(); //<- Static objects don't have linear velocity
ReOrient();
}
}
@@ -915,10 +1072,49 @@ namespace OpenSim.Region.Physics.BulletXPlugin
}
private void ReSize(PhysicsVector _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
MonoXnaCompactMaths.Vector3 _newsize;
_newsize = BulletXMaths.PhysicsVectorToXnaVector3(_newSize);
- //For now all prims are Boxes
- rigidBody.CollisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
+ if ((_newsize.X == 0) || (_newsize.Y == 0) || (_newsize.Z == 0)) throw new Exception("Size 0");
+
+ //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;
+ RigidBody _tmpRigidBody;
+ _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(this._position);
+ //For now all prims are boxes
+ CollisionShape _collisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
+ DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
+ MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
+ if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
+ _tmpRigidBody = 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
+ MonoXnaCompactMaths.Vector3 _vDebugTranslation;
+ _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
+ _tmpRigidBody.Translate(_vDebugTranslation);
+ //---
+ //There is a bug when trying to remove a rigidBody that is colliding with something..
+ try
+ {
+ this._parent_scene.ddWorld.RemoveRigidBody(rigidBody);
+ }
+ catch(Exception ex)
+ {
+ this._parent_scene.BulletXMessage(this._parent_scene.is_ex_message + ex.Message, true);
+ rigidBody.ActivationState = ActivationState.DisableSimulation;
+ this._parent_scene.AddForgottenRigidBody(rigidBody);
+ }
+ rigidBody = _tmpRigidBody;
+ this._parent_scene.ddWorld.AddRigidBody(rigidBody);
+ if (_physical) Speed();//Static objects don't have linear velocity
+ ReOrient();
+ GC.Collect();
}
private void ReOrient()
{
@@ -935,4 +1131,74 @@ namespace OpenSim.Region.Physics.BulletXPlugin
#endregion
}
+ ///
+ /// This Class manage a HeighField as a RigidBody. This is for to be added in the BulletXScene
+ ///
+ internal class BulletXPlanet
+ {
+ private PhysicsVector _staticPosition;
+ private PhysicsVector _staticVelocity;
+ private AxiomQuaternion _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 PhysicsVector(BulletXScene.MaxXY / 2, BulletXScene.MaxXY/2, 0);
+ _staticVelocity = new PhysicsVector();
+ _staticOrientation = AxiomQuaternion.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);
+ MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.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
+ MonoXnaCompactMaths.Vector3 _vDebugTranslation;
+ _vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition;
+ _flatPlanet.Translate(_vDebugTranslation);
+ parent_scene.ddWorld.AddRigidBody(_flatPlanet);
+ }
+ catch (Exception ex)
+ {
+ this._parentscene.BulletXMessage(ex.Message, true);
+ }
+ }
+ this._parentscene.BulletXMessage("BulletXPlanet created.", false);
+ }
+ internal 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;
+ 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)this._flatPlanet.CollisionShape).getHeightFieldValue(li_x, li_y);
+ if (height < 0) height = 0;
+ else if (height > BulletXScene.MaxZ) height = BulletXScene.MaxZ;
+
+ return height;
+ }
+ }
}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
index 15645b12a7..87b6d34cb5 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
@@ -104,6 +104,19 @@ namespace OpenSim.Region.Physics.Manager
pluginAssembly = null;
}
+ //---
+ public static void PhysicsPluginMessage(string message, bool isWarning)
+ {
+ if (isWarning)
+ {
+ MainLog.Instance.Warn("PHYSICS", message);
+ }
+ else
+ {
+ MainLog.Instance.Verbose("PHYSICS", message);
+ }
+ }
+ //---
}
public interface IPhysicsPlugin
diff --git a/ThirdPartyLicenses/BulletLicense.txt b/ThirdPartyLicenses/BulletLicense.txt
new file mode 100644
index 0000000000..c3ec68c21f
--- /dev/null
+++ b/ThirdPartyLicenses/BulletLicense.txt
@@ -0,0 +1,17 @@
+/*
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+Free for commercial use, but please mail bullet@erwincoumans.com to report projects, and join the forum at
+www.continuousphysics.com/Bullet/phpBB2
diff --git a/bin/Modified.XnaDevRu.BulletX.dll b/bin/Modified.XnaDevRu.BulletX.dll
index 8de2a5f71a..496a99af92 100644
Binary files a/bin/Modified.XnaDevRu.BulletX.dll and b/bin/Modified.XnaDevRu.BulletX.dll differ
diff --git a/bin/MonoXnaCompactMaths.dll b/bin/MonoXnaCompactMaths.dll
index a4e345abd8..1372653b00 100644
Binary files a/bin/MonoXnaCompactMaths.dll and b/bin/MonoXnaCompactMaths.dll differ
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.suo b/libraries/ModifiedBulletX/ModifiedBulletX.suo
index 4f0acd9652..6244108c88 100644
Binary files a/libraries/ModifiedBulletX/ModifiedBulletX.suo and b/libraries/ModifiedBulletX/ModifiedBulletX.suo differ
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs
new file mode 100644
index 0000000000..0f30f1f2e8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs
@@ -0,0 +1,360 @@
+/*
+ * WARNING!: this class is not in the original BulletX
+ * By the way it's based on the Bullet btHeightfieldTerrainShape:
+ * http://www.continuousphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html
+ *****************************************************************************************
+ * 3RD PARTY LICENSE. The next it's the original 3rd party lincense of Bullet:
+ * ----------------------------------------------------------------------------
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+ * ------------------------------------------------------------------------------
+*/
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+ public class HeightfieldTerrainShape : ConcaveShape
+ {
+ private Vector3 _localAabbMin;
+ private Vector3 _localAabbMax;
+ private Vector3 _localScaling = new Vector3(1f,1f,1f);
+ private int _width;
+ private int _length;
+ private float[] _heightfieldData;
+ private float _maxHeight;
+ private int _upAxis;
+ private bool _useFloatData;
+ private bool _flipQuadEdges;
+ private bool _useDiamondSubdivision = false;
+ private float _defaultCollisionMargin = 0.6f;
+
+ public HeightfieldTerrainShape(int width, int length, float[] heightfieldData, float maxHeight,
+ int upAxis, bool useFloatData, bool flipQuadEdges)
+ {
+ _width = width;
+ _length = length;
+ _heightfieldData = heightfieldData;
+ _maxHeight = maxHeight;
+ _upAxis = upAxis;
+ _useFloatData = useFloatData;
+ _flipQuadEdges = flipQuadEdges;
+ this.Margin = _defaultCollisionMargin;
+
+ float quantizationMargin = 1f;
+
+ //enlarge the AABB to avoid division by zero when initializing the quantization value
+ Vector3 clampValue = new Vector3(quantizationMargin, quantizationMargin, quantizationMargin);
+ Vector3 halfExtents = new Vector3(0, 0, 0);
+
+ switch (_upAxis)
+ {
+ case 0:
+ halfExtents.X = _maxHeight;
+ halfExtents.Y = _width;
+ halfExtents.Z = _length;
+ break;
+ case 1:
+ halfExtents.X = _width;
+ halfExtents.Y = _maxHeight;
+ halfExtents.Z = _length;
+ break;
+ case 2:
+ halfExtents.X = _width;
+ halfExtents.Y = _length;
+ halfExtents.Z = _maxHeight;
+ break;
+ default:
+ //need to get valid _upAxis
+ //btAssert(0);
+ throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+ }
+
+ halfExtents *= 0.5f;
+
+ _localAabbMin = -halfExtents - clampValue;
+ _localAabbMax = halfExtents + clampValue;
+ //Vector3 aabbSize = new Vector3();
+ //aabbSize = m_localAabbMax - m_localAabbMin;
+
+ }
+
+ protected Vector3 LocalAabbMin
+ { get { return _localAabbMin; } set { _localAabbMin = value; } }
+ protected Vector3 LocalAabbMax
+ { get { return _localAabbMax; } set { _localAabbMax = value; } }
+ public override string Name
+ {
+ get
+ {
+ return "HeightfieldTerrain";
+ }
+ }
+ public override Vector3 LocalScaling
+ {
+ get
+ {
+ return _localScaling;
+ }
+ set
+ {
+ _localScaling = value;
+ }
+ }
+ public override float Margin
+ {
+ get
+ {
+ return base.Margin;
+ }
+ set
+ {
+ base.Margin = value;
+ }
+ }
+ public override BroadphaseNativeTypes ShapeType
+ {
+ get { return BroadphaseNativeTypes.Terrain; }
+ }
+ public Vector3 HalfExtents
+ {
+ get
+ {
+ Vector3 halfExtents = new Vector3();
+ switch (_upAxis)
+ {
+ case 0:
+ halfExtents.X = 2f;//_maxHeight;
+ halfExtents.Y = _width;
+ halfExtents.Z = _length;
+ break;
+ case 1:
+ halfExtents.X = _width;
+ halfExtents.Y = 2f;// _maxHeight;
+ halfExtents.Z = _length;
+ break;
+ case 2:
+ halfExtents.X = _width;
+ halfExtents.Y = _length;
+ halfExtents.Z = 2f;// _maxHeight;
+ break;
+ default:
+ //need to get valid m_upAxis
+ //btAssert(0);
+ throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+ //break;
+ }
+ halfExtents *= 0.5f;
+ return halfExtents;
+ }
+ }
+
+ public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+ {
+ //(void)callback;
+ //(void)aabbMax;
+ //(void)aabbMin;
+
+ //quantize the aabbMin and aabbMax, and adjust the start/end ranges
+
+ int[] quantizedAabbMin = new int[3];
+ int[] quantizedAabbMax = new int[3];
+
+ Vector3 localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z );
+ Vector3 localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z);
+
+ quantizeWithClamp(ref quantizedAabbMin, localAabbMin);
+ quantizeWithClamp(ref quantizedAabbMax, localAabbMax);
+
+
+
+ int startX=0;
+ int endX=_width-1;
+ int startJ=0;
+ int endJ=_length-1;
+
+ switch(_upAxis)
+ {
+ case 0:
+ quantizedAabbMin[1]+=_width/2-1;
+ quantizedAabbMax[1]+=_width/2+1;
+ quantizedAabbMin[2]+=_length/2-1;
+ quantizedAabbMax[2]+=_length/2+1;
+
+ if (quantizedAabbMin[1]>startX)
+ startX = quantizedAabbMin[1];
+ if (quantizedAabbMax[1]startJ)
+ startJ = quantizedAabbMin[2];
+ if (quantizedAabbMax[2]startX)
+ startX = quantizedAabbMin[0];
+ if (quantizedAabbMax[0]startJ)
+ startJ = quantizedAabbMin[2];
+ if (quantizedAabbMax[2]startX)
+ startX = quantizedAabbMin[0];
+ if (quantizedAabbMax[0]startJ)
+ startJ = quantizedAabbMin[1];
+ if (quantizedAabbMax[1] 0)))
+ {
+ //first triangle
+ getVertex(x,j,ref vertices[0]);
+ getVertex(x+1,j,ref vertices[1]);
+ getVertex(x+1,j+1,ref vertices[2]);
+ //callback->processTriangle(vertices,x,j);
+ callback.ProcessTriangle(vertices,x,j);
+
+ //second triangle
+ getVertex(x,j,ref vertices[0]);
+ getVertex(x+1,j+1,ref vertices[1]);
+ getVertex(x,j+1,ref vertices[2]);
+ //callback->processTriangle(vertices,x,j);
+ callback.ProcessTriangle(vertices, x, j);
+ }
+ else
+ {
+ //first triangle
+ getVertex(x,j,ref vertices[0]);
+ getVertex(x,j+1,ref vertices[1]);
+ getVertex(x+1,j,ref vertices[2]);
+ //callback->processTriangle(vertices,x,j);
+ callback.ProcessTriangle(vertices,x,j);
+
+ //second triangle
+ getVertex(x+1,j,ref vertices[0]);
+ getVertex(x,j+1,ref vertices[1]);
+ getVertex(x+1,j+1,ref vertices[2]);
+ //callback->processTriangle(vertices,x,j);
+ callback.ProcessTriangle(vertices,x,j);
+ }
+ }
+ }
+ }
+ public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+ {
+ //aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+ //aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+
+ Vector3 halfExtents = (_localAabbMax - _localAabbMin) * _localScaling * 0.5f;
+
+ Vector3 center = t.Translation;
+ Vector3 extent = new Vector3(Math.Abs(halfExtents.X), Math.Abs(halfExtents.Y), Math.Abs(halfExtents.Z));
+ extent += new Vector3(Margin, Margin, Margin);
+
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+ }
+ public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+ {
+ //moving concave objects not supported
+ inertia = new Vector3();
+ }
+ public float getHeightFieldValue(int x,int y)
+ {
+ float val = 0f;
+ if (_useFloatData)
+ {
+ val = _heightfieldData[(y * _width) + x];
+ }
+ else
+ {
+ //assume unsigned short int
+ int heightFieldValue = (int)_heightfieldData[(y * _width) + x];
+ val = heightFieldValue * _maxHeight/65535f;
+ }
+ return val;
+ }
+ public void getVertex(int x,int y,ref Vector3 vertex)
+ {
+ if (x < 0) x = 0;
+ if (y < 0) y = 0;
+ if (x >= _width) x = _width - 1;
+ if (y >= _length) y = _length - 1;
+ float height = getHeightFieldValue(x,y);
+ switch(_upAxis)
+ {
+ case 0:
+ vertex.X = height;
+ vertex.Y = (- _width/2 ) + x;
+ vertex.Z = (- _length/2 ) + y;
+ break;
+ case 1:
+ vertex.X = (- _width/2 ) + x;
+ vertex.Y = height;
+ vertex.Z = (- _length/2 ) + y;
+ break;
+ case 2:
+ vertex.X = (- _width/2 ) + x;
+ vertex.Y = (- _length/2 ) + y;
+ vertex.Z = height;
+ break;
+ default:
+ //need to get valid m_upAxis
+ throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
+ //break;
+ }
+ vertex *= _localScaling;
+ }
+ public void quantizeWithClamp(ref int[] _out,Vector3 point)
+ {
+ Vector3 clampedPoint = point;
+ MathHelper.SetMax(ref clampedPoint,_localAabbMin);
+ MathHelper.SetMin(ref clampedPoint, _localAabbMax);
+ Vector3 v = clampedPoint;
+
+ _out[0] = (int)(v.X);
+ _out[1] = (int)(v.Y);
+ _out[2] = (int)(v.Z);
+ //correct for
+ }
+ }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
index 06b98bce58..3aead7015f 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
@@ -78,6 +78,7 @@
+
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
index 22842ca4ee..9b88ab3a04 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
@@ -573,7 +573,11 @@ namespace MonoXnaCompactMaths
public static Matrix operator /(Matrix matrix1, float divider)
{
- throw new NotImplementedException();
+ return new Matrix(
+ matrix1.M11 / divider, matrix1.M12 / divider, matrix1.M13 / divider, matrix1.M14 / divider,
+ matrix1.M21 / divider, matrix1.M22 / divider, matrix1.M23 / divider, matrix1.M24 / divider,
+ matrix1.M31 / divider, matrix1.M32 / divider, matrix1.M33 / divider, matrix1.M34 / divider,
+ matrix1.M41 / divider, matrix1.M42 / divider, matrix1.M43 / divider, matrix1.M44 / divider);
}
@@ -658,7 +662,10 @@ namespace MonoXnaCompactMaths
public override string ToString()
{
- throw new NotImplementedException();
+ return "[(" + this.M11 + ", " + this.M12 + ", " + this.M13 + ", " + this.M14 + ")\n ("
+ + this.M21 + ", " + this.M22 + ", " + this.M23 + ", " + this.M24 + ")\n ("
+ + this.M31 + ", " + this.M32 + ", " + this.M33 + ", " + this.M34 + ")\n ("
+ + this.M41 + ", " + this.M42 + ", " + this.M43 + ", " + this.M44 + ")]";
}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
index b4f187321d..b6d9d343cd 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
@@ -90,7 +90,91 @@ namespace MonoXnaCompactMaths
public static Quaternion CreateFromRotationMatrix(Matrix matrix)
{
- throw new NotImplementedException();
+ float Omega2 = matrix.M44;
+ if (!isAprox(Omega2, 1f))
+ {
+ //"Normalize" the Rotation matrix. Norma = M44 = Omega2
+ matrix = matrix / Omega2;
+ }
+ //Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion)
+ float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg;
+ lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f;
+ lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f;
+ lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f;
+ lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f;
+ lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f;
+ lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f;
+
+ //lambadIS = (qJ + s*qK)^2
+ //q0 = w | q1 = x | q2 = y, q3 = z
+ //Every value of qI (I=1,2,3) has 4 possible values cause the sqrt
+ float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4];
+ float[] sig1 = {1f, 1f, -1f, -1f};
+ float[] sig2 = {1f, -1f, 1f, -1f};
+ for (int i = 0; i < 4; i++)
+ {
+ x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f;
+ y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f;
+ z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f;
+ }
+
+ //Only a set of x, y, z are the corrects values. So it requires testing
+ int li_i=0, li_j=0, li_k=0;
+ bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N;
+ bool lb_superLambda = false;
+ while((li_i<4)&&(!lb_superLambda))
+ {
+ while ((li_j < 4) && (!lb_superLambda))
+ {
+ while ((li_k < 4) && (!lb_superLambda))
+ {
+ lb_testL1P = isAprox((float)(
+ Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos);
+ lb_testL2P = isAprox((float)(
+ Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos);
+ lb_testL3P = isAprox((float)(
+ Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos);
+ lb_testL1N = isAprox((float)(
+ Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg);
+ lb_testL2N = isAprox((float)(
+ Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg);
+ lb_testL3N = isAprox((float)(
+ Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg);
+
+ lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P
+ && lb_testL1N && lb_testL2N && lb_testL3N);
+
+ if (!lb_superLambda) li_k++;
+ }
+ if (!lb_superLambda) li_j++;
+ }
+ if (!lb_superLambda) li_i++;
+ }
+
+ Quaternion q = new Quaternion();
+
+ if (lb_superLambda)
+ {
+ q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k];
+ q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z);
+
+ if (!isAprox(Omega2, 1f))
+ {
+ if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!");
+ q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1.
+ }
+ }
+ else
+ {
+ q = Quaternion.identity;
+ }
+
+ return q;
+ }
+ private static float floatError = 0.000001f;
+ private static bool isAprox(float test, float realValue)
+ {
+ return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError))));
}
@@ -317,7 +401,8 @@ namespace MonoXnaCompactMaths
public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
{
- throw new NotImplementedException();
+ return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor,
+ quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor);
}
@@ -335,7 +420,7 @@ namespace MonoXnaCompactMaths
public override string ToString()
{
- throw new NotImplementedException();
+ return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")";
}
private static void Conjugate(ref Quaternion quaternion, out Quaternion result)