OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

applied Darok"s BulletXPlugin changes 003.patch

afrisby
dan miller 2007-10-07 14:40:02 +00:00
parent 6a43889f65
commit 2d5f5e2b32
10 changed files with 856 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

460
OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
View File

@ -30,18 +30,28 @@
#region References #region References
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using OpenSim.Framework.Types;
using OpenSim.Region.Physics.Manager; using OpenSim.Region.Physics.Manager;
using OpenSim.Framework.Types;
using Axiom.Math; using Axiom.Math;
using AxiomQuaternion = Axiom.Math.Quaternion; using AxiomQuaternion = Axiom.Math.Quaternion;
//Specific References for BulletXPlugin //Specific References for BulletXPlugin
using MonoXnaCompactMaths; //Called as MXCM using MonoXnaCompactMaths;
using XnaDevRu.BulletX; using XnaDevRu.BulletX;
using XnaDevRu.BulletX.Dynamics; using XnaDevRu.BulletX.Dynamics;
#endregion #endregion
namespace OpenSim.Region.Physics.BulletXPlugin namespace OpenSim.Region.Physics.BulletXPlugin
{ {
/// <summary>
/// This class is only here for compilations reasons
/// </summary>
public class Mesh
{
public Mesh()
{
}
}
/// <summary> /// <summary>
/// BulletXConversions are called now BulletXMaths /// BulletXConversions are called now BulletXMaths
/// This Class converts objects and types for BulletX and give some operations /// 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 maxXY = 256;
private const int maxZ = 4096; private const int maxZ = 4096;
private const int maxHandles = 32766; //Why? I don't know private const int maxHandles = 32766; //Why? I don't know
private static float gravity = 9.8f; private const float gravity = 9.8f;
private static float heightLevel0 = 77.0f; private const float heightLevel0 = 77.0f;
private static float heightLevel1 = 200.0f; private const float heightLevel1 = 200.0f;
private static float lowGravityFactor = 0.2f; private const float lowGravityFactor = 0.2f;
//OpenSim calls Simulate 10 times per seconds. So FPS = "Simulate Calls" * simulationSubSteps = 100 FPS
private float[] _heightmap; private const int simulationSubSteps = 10;
//private float[] _heightmap;
private BulletXPlanet _simFlatPlanet;
private List<BulletXCharacter> _characters = new List<BulletXCharacter>(); private List<BulletXCharacter> _characters = new List<BulletXCharacter>();
private List<BulletXPrim> _prims = new List<BulletXPrim>(); private List<BulletXPrim> _prims = new List<BulletXPrim>();
@ -235,6 +247,11 @@ namespace OpenSim.Region.Physics.BulletXPlugin
public static float HeightLevel0 { get { return heightLevel0; } } public static float HeightLevel0 { get { return heightLevel0; } }
public static float HeightLevel1 { get { return heightLevel1; } } public static float HeightLevel1 { get { return heightLevel1; } }
public static float LowGravityFactor { get { return lowGravityFactor; } } 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 #endregion
public BulletXScene() public BulletXScene()
@ -250,8 +267,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver); ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
ddWorld.Gravity = new MonoXnaCompactMaths.Vector3(0, 0, -gravity); 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)
{ {
@ -262,7 +278,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
BulletXCharacter newAv = null; BulletXCharacter newAv = null;
lock (BulletXLock) lock (BulletXLock)
{ {
newAv = new BulletXCharacter(this, pos); newAv = new BulletXCharacter(avName, this, pos);
_characters.Add(newAv); _characters.Add(newAv);
} }
return newAv; return newAv;
@ -273,48 +289,98 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{ {
lock (BulletXLock) 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); _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; BulletXPrim newPrim = null;
lock (BulletXLock) lock (BulletXLock)
{ {
newPrim = new BulletXPrim(this, position, size, rotation); newPrim = new BulletXPrim(name, this, position, size, rotation, mesh, pbs);
_prims.Add(newPrim); _prims.Add(newPrim);
} }
return 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) public override void RemovePrim(PhysicsActor prim)
{ {
if (prim is BulletXPrim) if (prim is BulletXPrim)
{ {
lock (BulletXLock) 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); _prims.Remove((BulletXPrim)prim);
} }
GC.Collect();
} }
} }
public override void Simulate(float timeStep) public override void Simulate(float timeStep)
{ {
lock (BulletXLock) lock (BulletXLock)
{ {
BXSMove(timeStep); //Try to remove garbage
ddWorld.StepSimulation(timeStep, 0, timeStep); RemoveForgottenRigidBodies();
//Heightmap Validation: //End of remove
BXSValidateHeight(); MoveAllObjects(timeStep);
ddWorld.StepSimulation(timeStep, simulationSubSteps, timeStep);
//Extra Heightmap Validation: BulletX's HeightFieldTerrain somestimes doesn't work so fine.
ValidateHeightForAll();
//End heightmap validation. //End heightmap validation.
BXSUpdateKinetics(); UpdateKineticsForAll();
} }
} }
private void BXSMove(float timeStep) private void MoveAllObjects(float timeStep)
{ {
foreach (BulletXCharacter actor in _characters) foreach (BulletXCharacter actor in _characters)
{ {
@ -324,39 +390,33 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{ {
} }
} }
private void BXSValidateHeight() private void ValidateHeightForAll()
{ {
float _height; float _height;
foreach (BulletXCharacter actor in _characters) foreach (BulletXCharacter actor in _characters)
{ {
if ((actor.RigidBodyHorizontalPosition.x < 0) || (actor.RigidBodyHorizontalPosition.y < 0)) //_height = HeightValue(actor.RigidBodyPosition);
{ _height = _simFlatPlanet.HeightValue(actor.RigidBodyPosition);
_height = 0;
}
else
{
_height = this._heightmap[
(int)Math.Round(actor.RigidBodyHorizontalPosition.x) * 256
+ (int)Math.Round(actor.RigidBodyHorizontalPosition.y)];
}
actor.ValidateHeight(_height); actor.ValidateHeight(_height);
//if (_simFlatPlanet.heightIsNotValid(actor.RigidBodyPosition, out _height)) actor.ValidateHeight(_height);
} }
foreach (BulletXPrim prim in _prims) foreach (BulletXPrim prim in _prims)
{ {
if ((prim.RigidBodyHorizontalPosition.x < 0) || (prim.RigidBodyHorizontalPosition.y < 0)) //_height = HeightValue(prim.RigidBodyPosition);
{ _height = _simFlatPlanet.HeightValue(prim.RigidBodyPosition);
_height = 0;
}
else
{
_height = this._heightmap[
(int)Math.Round(prim.RigidBodyHorizontalPosition.x) * 256
+ (int)Math.Round(prim.RigidBodyHorizontalPosition.y)];
}
prim.ValidateHeight(_height); 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. //UpdatePosition > UpdateKinetics.
//Not only position will be updated, also velocity cause acceleration. //Not only position will be updated, also velocity cause acceleration.
@ -368,6 +428,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{ {
prim.UpdateKinetics(); prim.UpdateKinetics();
} }
//if(this._simFlatPlanet!=null) this._simFlatPlanet.Restore();
} }
public override void GetResults() public override void GetResults()
{ {
@ -382,24 +443,97 @@ namespace OpenSim.Region.Physics.BulletXPlugin
} }
public override void SetTerrain(float[] heightMap) public override void SetTerrain(float[] heightMap)
{ {
//As the same as ODE, heightmap (x,y) must be swapped for BulletX ////As the same as ODE, heightmap (x,y) must be swapped for BulletX
for (int i = 0; i < 65536; i++) //for (int i = 0; i < 65536; i++)
{ //{
// this._heightmap[i] = (double)heightMap[i]; // // this._heightmap[i] = (double)heightMap[i];
// dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...) // // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
int x = i & 0xff; // int x = i & 0xff;
int y = i >> 8; // int y = i >> 8;
this._heightmap[i] = heightMap[x * 256 + y]; // this._heightmap[i] = heightMap[x * 256 + y];
} //}
lock (BulletXLock)
{ //float[] swappedHeightMap = new float[65536];
//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;
// 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() 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;
//}
} }
/// <summary> /// <summary>
/// PhysicsActor Character Class for BulletX /// PhysicsActor Character Class for BulletX
@ -414,19 +548,20 @@ namespace OpenSim.Region.Physics.BulletXPlugin
private bool flying; private bool flying;
private RigidBody rigidBody; private RigidBody rigidBody;
public Axiom.Math.Vector2 RigidBodyHorizontalPosition public MonoXnaCompactMaths.Vector3 RigidBodyPosition
{ {
get get { return this.rigidBody.CenterOfMassPosition; }
{
return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
}
} }
public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos) 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) 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) PhysicsVector size, PhysicsVector acceleration, AxiomQuaternion orientation)
{ {
//This fields will be removed. They're temporal //This fields will be removed. They're temporal
@ -462,7 +597,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3(); MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3();
_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0 _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
rigidBody = new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping, _friction, _restitution); 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 //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
MonoXnaCompactMaths.Vector3 _vDebugTranslation; MonoXnaCompactMaths.Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
@ -535,6 +670,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin
} }
} }
} }
public RigidBody RigidBody
{
get
{
return rigidBody;
}
}
public override bool Flying public override bool Flying
{ {
get get
@ -580,7 +722,7 @@ namespace OpenSim.Region.Physics.BulletXPlugin
vec.X = this._velocity.X; vec.X = this._velocity.X;
vec.Y = this._velocity.Y; 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) if (flying)
{ {
//Antigravity with movement //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 //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 const float _density = 1000.0f;
private RigidBody rigidBody; private RigidBody rigidBody;
private BulletXScene _parent_scene;
//_physical value will be linked with the prim object value //_physical value will be linked with the prim object value
private Boolean _physical = false; private Boolean _physical = false;
public Axiom.Math.Vector2 RigidBodyHorizontalPosition public MonoXnaCompactMaths.Vector3 RigidBodyPosition
{ {
get get { return this.rigidBody.CenterOfMassPosition; }
{
return new Axiom.Math.Vector2(this.rigidBody.CenterOfMassPosition.X, this.rigidBody.CenterOfMassPosition.Y);
}
} }
public BulletXPrim(BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size, AxiomQuaternion rotation) 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, public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size,
PhysicsVector aceleration, AxiomQuaternion rotation) 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 ((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; _acceleration = aceleration;
//Because a bug, orientation will be fixed to AxiomQuaternion.Identity _orientation = rotation;
_orientation = AxiomQuaternion.Identity;
//_orientation = rotation; _parent_scene = parent_scene;
//---
//For RigidBody Constructor. The next values might change //For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f; float _linearDamping = 0.0f;
float _angularDamping = 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); CollisionShape _collisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_size) / 2.0f);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset); DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
MonoXnaCompactMaths.Vector3 _localInertia = new MonoXnaCompactMaths.Vector3(); 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 = 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 //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
MonoXnaCompactMaths.Vector3 _vDebugTranslation; MonoXnaCompactMaths.Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition; _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
@ -753,10 +899,18 @@ namespace OpenSim.Region.Physics.BulletXPlugin
set set
{ {
lock (BulletXScene.BulletXLock) lock (BulletXScene.BulletXLock)
{
//Static objects don' have linear velocity
if (_physical)
{ {
_velocity = value; _velocity = value;
Speed(); Speed();
} }
else
{
_velocity = new PhysicsVector();
}
}
} }
} }
public override PhysicsVector Size public override PhysicsVector Size
@ -801,7 +955,14 @@ namespace OpenSim.Region.Physics.BulletXPlugin
get get
{ {
//For now all prims are boxes //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 public override bool Flying
@ -863,6 +1024,8 @@ namespace OpenSim.Region.Physics.BulletXPlugin
m.Translation = v3; m.Translation = v3;
rigidBody.WorldTransform = m; rigidBody.WorldTransform = m;
//When a Prim touch the ground it's vertical velocity it's reduced to ZERO //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 PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f)); Speed(new PhysicsVector(this.rigidBody.LinearVelocity.X, this.rigidBody.LinearVelocity.Y, 0.0f));
} }
} }
@ -872,18 +1035,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
{ {
this._position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition); this._position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
this._velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity); this._velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
//Orientation is not implemented yet in MonoXnaCompactMaths this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation);
//this._orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation); < Good
//ReOrient();
//---
ReOrient();
} }
else //Doesn't updates properties. That's a cancel else //Doesn't updates properties. That's a cancel
{ {
Translate(); Translate();
Speed(); //Speed(); //<- Static objects don't have linear velocity
//Orientation is not implemented yet in MonoXnaCompactMaths
//ReOrient();
ReOrient(); ReOrient();
} }
} }
@ -915,10 +1072,49 @@ namespace OpenSim.Region.Physics.BulletXPlugin
} }
private void ReSize(PhysicsVector _newSize) 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; MonoXnaCompactMaths.Vector3 _newsize;
_newsize = BulletXMaths.PhysicsVectorToXnaVector3(_newSize); _newsize = BulletXMaths.PhysicsVectorToXnaVector3(_newSize);
//For now all prims are Boxes if ((_newsize.X == 0) || (_newsize.Y == 0) || (_newsize.Z == 0)) throw new Exception("Size 0");
rigidBody.CollisionShape = new XnaDevRu.BulletX.BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(_newSize) / 2.0f);
//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() private void ReOrient()
{ {
@ -935,4 +1131,74 @@ namespace OpenSim.Region.Physics.BulletXPlugin
#endregion #endregion
} }
/// <summary>
/// This Class manage a HeighField as a RigidBody. This is for to be added in the BulletXScene
/// </summary>
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;
}
}
} }

13
OpenSim/Region/Physics/Manager/PhysicsPluginManager.cs
View File

@ -104,6 +104,19 @@ namespace OpenSim.Region.Physics.Manager
pluginAssembly = null; 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 public interface IPhysicsPlugin

17
ThirdPartyLicenses/BulletLicense.txt Normal file
View File

@ -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

BIN
bin/Modified.XnaDevRu.BulletX.dll
View File

Binary file not shown.

BIN
bin/MonoXnaCompactMaths.dll
View File

Binary file not shown.

BIN
libraries/ModifiedBulletX/ModifiedBulletX.suo
View File

Binary file not shown.

360
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs Normal file
View File

@ -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]<endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 1:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[2]+=_length/2-1;
quantizedAabbMax[2]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 2:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[1]+=_length/2-1;
quantizedAabbMax[1]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1];
break;
default:
//need to get valid m_upAxis
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
for(int j=startJ; j<endJ; j++)
{
for(int x=startX; x<endX; x++)
{
Vector3[] vertices = new Vector3[3];
//if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1)))
if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 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
}
}
}

1
libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
View File

@ -78,6 +78,7 @@
<Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />
<Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />
<Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />
<Compile Include="Collision\CollisionShapes\HeightfieldTerrainShape.cs" />
<Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />
<Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />
<Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />

11
libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
View File

@ -573,7 +573,11 @@ namespace MonoXnaCompactMaths
public static Matrix operator /(Matrix matrix1, float divider) 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() 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 + ")]";
} }

91
libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
View File

@ -90,7 +90,91 @@ namespace MonoXnaCompactMaths
public static Quaternion CreateFromRotationMatrix(Matrix matrix) 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) 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() 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) private static void Conjugate(ref Quaternion quaternion, out Quaternion result)