From e4316394c300ef470fad04774844779888c7a5ea Mon Sep 17 00:00:00 2001
From: MW
Date: Fri, 30 Mar 2007 15:19:15 +0000
Subject: Done some fixes to the ODE plugin, but its still not working
correctly, believe that the terrain is rotated /placed wrongly.
---
OpenSim.Physics/OdePlugin/OdePlugin.cs | 788 +++++++++++++++++----------------
1 file changed, 409 insertions(+), 379 deletions(-)
(limited to 'OpenSim.Physics/OdePlugin/OdePlugin.cs')
diff --git a/OpenSim.Physics/OdePlugin/OdePlugin.cs b/OpenSim.Physics/OdePlugin/OdePlugin.cs
index 7b0d64a..325e2fd 100644
--- a/OpenSim.Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim.Physics/OdePlugin/OdePlugin.cs
@@ -31,387 +31,417 @@ using Ode.NET;
namespace OpenSim.Physics.OdePlugin
{
- ///
- /// ODE plugin
- ///
- public class OdePlugin : IPhysicsPlugin
- {
- private OdeScene _mScene;
-
- public OdePlugin()
- {
-
- }
-
- public bool Init()
- {
- return true;
- }
-
- public PhysicsScene GetScene()
- {
- if(_mScene == null)
- {
- _mScene = new OdeScene();
- }
- return(_mScene);
- }
-
- public string GetName()
- {
- return("OpenDynamicsEngine");
- }
-
- public void Dispose()
- {
-
- }
- }
-
- public class OdeScene :PhysicsScene
- {
- static public IntPtr world;
- static public IntPtr space;
- static private IntPtr contactgroup;
- static private IntPtr LandGeom;
- static private IntPtr Land;
- private double[] _heightmap;
- static private d.NearCallback nearCallback = near;
- private List _characters = new List();
- private static d.ContactGeom[] contacts = new d.ContactGeom[500];
- private static d.Contact contact;
-
- public OdeScene() {
- world = d.WorldCreate();
- space = d.HashSpaceCreate(IntPtr.Zero);
- contactgroup = d.JointGroupCreate(0);
- d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);
- d.WorldSetCFM(world, 1e-5f);
- d.WorldSetAutoDisableFlag(world, false);
- this._heightmap=new double[65536];
- }
-
- // This function blatantly ripped off from BoxStack.cs
- static private void near(IntPtr space, IntPtr g1, IntPtr g2)
- {
- IntPtr b1 = d.GeomGetBody(g1);
- IntPtr b2 = d.GeomGetBody(g2);
- if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
- return;
-
- int count = d.Collide(g1, g2, 500, contacts, d.ContactGeom.SizeOf);
- for (int i = 0; i < count; ++i)
- {
- contact.geom = contacts[i];
- IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
- d.JointAttach(joint, b1, b2);
- }
-
- }
-
- public override PhysicsActor AddAvatar(PhysicsVector position)
- {
- PhysicsVector pos = new PhysicsVector();
- pos.X = position.X;
- pos.Y = position.Y;
- pos.Z = position.Z;
- OdeCharacter newAv= new OdeCharacter(this,pos);
- this._characters.Add(newAv);
- return newAv;
- }
-
- public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size)
- {
- PhysicsVector pos = new PhysicsVector();
- pos.X = position.X;
- pos.Y = position.Y;
- pos.Z = position.Z;
- PhysicsVector siz = new PhysicsVector();
- siz.X = size.X;
- siz.Y = size.Y;
- siz.Z = size.Z;
- return new OdePrim();
- }
-
- public override void Simulate(float timeStep)
- {
- foreach (OdeCharacter actor in _characters) {
- actor.Move(timeStep*5f);
- }
- d.SpaceCollide(space, IntPtr.Zero, nearCallback);
- d.WorldQuickStep(world, timeStep*5f);
- d.JointGroupEmpty(contactgroup);
- foreach (OdeCharacter actor in _characters)
- {
- actor.UpdatePosition();
- }
-
- }
-
- public override void GetResults()
- {
-
- }
-
- public override bool IsThreaded
- {
- get
- {
- return(false); // for now we won't be multithreaded
- }
- }
-
- public override void SetTerrain(float[] heightMap)
- {
- for(int i=0; i<65536; i++) {
- this._heightmap[i]=(double)heightMap[i];
- }
- IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
- d.GeomHeightfieldDataBuildDouble(HeightmapData,_heightmap,1,256,256,256,256,1.0f,0.0f,2.0f,0);
- LandGeom=d.CreateHeightfield(space, HeightmapData, 0);
- }
+ ///
+ /// ODE plugin
+ ///
+ public class OdePlugin : IPhysicsPlugin
+ {
+ private OdeScene _mScene;
+
+ public OdePlugin()
+ {
+
+ }
+
+ public bool Init()
+ {
+ return true;
+ }
+
+ public PhysicsScene GetScene()
+ {
+ if (_mScene == null)
+ {
+ _mScene = new OdeScene();
+ }
+ return (_mScene);
+ }
+
+ public string GetName()
+ {
+ return ("OpenDynamicsEngine");
+ }
+
+ public void Dispose()
+ {
+
+ }
+ }
+
+ public class OdeScene : PhysicsScene
+ {
+ static public IntPtr world;
+ static public IntPtr space;
+ static private IntPtr contactgroup;
+ static private IntPtr LandGeom;
+ static private IntPtr Land;
+ private double[] _heightmap;
+ static private d.NearCallback nearCallback = near;
+ private List _characters = new List();
+ private static d.ContactGeom[] contacts = new d.ContactGeom[30];
+ private static d.Contact contact;
+
+ public OdeScene()
+ {
+ contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM;
+ contact.surface.mu = d.Infinity;
+ contact.surface.mu2 = 0.0f;
+ contact.surface.bounce = 0.1f;
+ contact.surface.bounce_vel = 0.1f;
+ contact.surface.soft_cfm = 0.01f;
+
+ world = d.WorldCreate();
+ space = d.HashSpaceCreate(IntPtr.Zero);
+ contactgroup = d.JointGroupCreate(0);
+ d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);
+ //d.WorldSetCFM(world, 1e-5f);
+ d.WorldSetAutoDisableFlag(world, false);
+ d.WorldSetContactSurfaceLayer(world, 0.001f);
+ // d.CreatePlane(space, 0, 0, 1, 0);
+ this._heightmap = new double[65536];
+ }
+
+ // This function blatantly ripped off from BoxStack.cs
+ static private void near(IntPtr space, IntPtr g1, IntPtr g2)
+ {
+ //Console.WriteLine("collision callback");
+ IntPtr b1 = d.GeomGetBody(g1);
+ IntPtr b2 = d.GeomGetBody(g2);
+ if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
+ return;
+
+ int count = d.Collide(g1, g2, 500, contacts, d.ContactGeom.SizeOf);
+ for (int i = 0; i < count; ++i)
+ {
+ contact.geom = contacts[i];
+ IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
+ d.JointAttach(joint, b1, b2);
+ }
+
+ }
+
+ public override PhysicsActor AddAvatar(PhysicsVector position)
+ {
+ PhysicsVector pos = new PhysicsVector();
+ pos.X = position.X;
+ pos.Y = position.Y;
+ pos.Z = position.Z + 20;
+ OdeCharacter newAv = new OdeCharacter(this, pos);
+ this._characters.Add(newAv);
+ return newAv;
+ }
+
+ public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size)
+ {
+ PhysicsVector pos = new PhysicsVector();
+ pos.X = position.X;
+ pos.Y = position.Y;
+ pos.Z = position.Z;
+ PhysicsVector siz = new PhysicsVector();
+ siz.X = size.X;
+ siz.Y = size.Y;
+ siz.Z = size.Z;
+ return new OdePrim();
+ }
+
+ public override void Simulate(float timeStep)
+ {
+ foreach (OdeCharacter actor in _characters)
+ {
+ actor.Move(timeStep * 5f);
+ }
+ d.SpaceCollide(space, IntPtr.Zero, nearCallback);
+ d.WorldQuickStep(world, timeStep * 5f);
+ d.JointGroupEmpty(contactgroup);
+ foreach (OdeCharacter actor in _characters)
+ {
+ actor.UpdatePosition();
+ }
+
+ }
+
+ public override void GetResults()
+ {
+
+ }
+
+ public override bool IsThreaded
+ {
+ get
+ {
+ return (false); // for now we won't be multithreaded
+ }
+ }
+
+ public override void SetTerrain(float[] heightMap)
+ {
+ for (int i = 0; i < 65536; i++)
+ {
+ this._heightmap[i] = (double)heightMap[i];
+ }
+ IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
+ d.GeomHeightfieldDataBuildDouble(HeightmapData, _heightmap, 0, 256, 256, 256, 256, 1.0f, 0.0f, 2.0f, 0);
+ d.GeomHeightfieldDataSetBounds(HeightmapData, 256, 256);
+ LandGeom = d.CreateHeightfield(space, HeightmapData, 1);
+ d.Matrix3 R = new d.Matrix3();
+
+ Axiom.MathLib.Quaternion q1 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(1,0,0));
+ Axiom.MathLib.Quaternion q2 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(0,1,0));
+ //Axiom.MathLib.Quaternion q3 = Axiom.MathLib.Quaternion.FromAngleAxis(3.14f, new Axiom.MathLib.Vector3(0, 0, 1));
+
+ q1 = q1 * q2;
+ //q1 = q1 * q3;
+ Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3();
+ float angle = 0;
+ q1.ToAngleAxis(ref angle, ref v3);
+
+ d.RFromAxisAndAngle(out R, v3.x, v3.y, v3.z, angle);
+ d.GeomSetRotation(LandGeom, ref R);
+ d.GeomSetPosition(LandGeom, 128, 128, 0);
+ }
public override void DeleteTerrain()
{
-
- }
- }
-
- public class OdeCharacter : PhysicsActor
- {
- private PhysicsVector _position;
- private PhysicsVector _velocity;
- private PhysicsVector _acceleration;
- private bool flying;
- private float gravityAccel;
- private IntPtr BoundingCapsule;
- IntPtr capsule_geom;
- d.Mass capsule_mass;
-
- public OdeCharacter(OdeScene parent_scene, PhysicsVector pos)
- {
- _velocity = new PhysicsVector();
- _position = pos;
- _acceleration = new PhysicsVector();
- d.MassSetCapsule(out capsule_mass, 5.0f, 3, 0.5f, 2f);
- capsule_geom = d.CreateCapsule(OdeScene.space, 0.5f, 2f);
- this.BoundingCapsule=d.BodyCreate(OdeScene.world);
- d.BodySetMass(BoundingCapsule, ref capsule_mass);
- d.BodySetPosition(BoundingCapsule,pos.X,pos.Y,pos.Z);
- }
-
- public override bool Flying
- {
- get
- {
- return flying;
- }
- set
- {
- flying = value;
- }
- }
-
- public override PhysicsVector Position
- {
- get
- {
- return _position;
- }
- set
- {
- _position = value;
- }
- }
-
- public override PhysicsVector Velocity
- {
- get
- {
- return _velocity;
- }
- set
- {
- _velocity = value;
- }
- }
-
- public override bool Kinematic
- {
- get
- {
- return false;
- }
- set
- {
-
- }
- }
-
- public override Axiom.MathLib.Quaternion Orientation
- {
- get
- {
- return Axiom.MathLib.Quaternion.Identity;
- }
- set
- {
-
- }
- }
-
- public override PhysicsVector Acceleration
- {
- get
- {
- return _acceleration;
- }
-
- }
- public void SetAcceleration (PhysicsVector accel)
- {
- this._acceleration = accel;
- }
-
- public override void AddForce(PhysicsVector force)
- {
-
- }
-
- public override void SetMomentum(PhysicsVector momentum)
- {
-
- }
-
- public void Move(float timeStep)
- {
- PhysicsVector vec = new PhysicsVector();
- vec.X = this._velocity.X * timeStep;
- vec.Y = this._velocity.Y * timeStep;
- if(flying)
- {
- vec.Z = ( this._velocity.Z+0.5f) * timeStep;
- }
- d.BodySetLinearVel(this.BoundingCapsule, vec.X, vec.Y, vec.Z);
- }
-
- public void UpdatePosition()
- {
- d.Vector3 vec = d.BodyGetPosition(BoundingCapsule);
- this._position.X = vec.X;
- this._position.Y = vec.Y;
- this._position.Z = vec.Z;
- }
- }
-
- public class OdePrim : PhysicsActor
- {
- private PhysicsVector _position;
- private PhysicsVector _velocity;
- private PhysicsVector _acceleration;
-
- public OdePrim()
- {
- _velocity = new PhysicsVector();
- _position = new PhysicsVector();
- _acceleration = new PhysicsVector();
- }
- public override bool Flying
- {
- get
- {
- return false; //no flying prims for you
- }
- set
- {
-
- }
- }
- public override PhysicsVector Position
- {
- get
- {
- PhysicsVector pos = new PhysicsVector();
- // PhysicsVector vec = this._prim.Position;
- //pos.X = vec.X;
- //pos.Y = vec.Y;
- //pos.Z = vec.Z;
- return pos;
-
- }
- set
- {
- /*PhysicsVector vec = value;
- PhysicsVector pos = new PhysicsVector();
- pos.X = vec.X;
- pos.Y = vec.Y;
- pos.Z = vec.Z;
- this._prim.Position = pos;*/
- }
- }
-
- public override PhysicsVector Velocity
- {
- get
- {
- return _velocity;
- }
- set
- {
- _velocity = value;
- }
- }
-
- public override bool Kinematic
- {
- get
- {
- return false;
- //return this._prim.Kinematic;
- }
- set
- {
- //this._prim.Kinematic = value;
- }
- }
-
- public override Axiom.MathLib.Quaternion Orientation
- {
- get
- {
- Axiom.MathLib.Quaternion res = new Axiom.MathLib.Quaternion();
- return res;
- }
- set
- {
-
- }
- }
-
- public override PhysicsVector Acceleration
- {
- get
- {
- return _acceleration;
- }
-
- }
- public void SetAcceleration (PhysicsVector accel)
- {
- this._acceleration = accel;
- }
-
- public override void AddForce(PhysicsVector force)
- {
-
- }
-
- public override void SetMomentum(PhysicsVector momentum)
- {
-
- }
-
-
- }
+
+ }
+ }
+
+ public class OdeCharacter : PhysicsActor
+ {
+ private PhysicsVector _position;
+ private PhysicsVector _velocity;
+ private PhysicsVector _acceleration;
+ private bool flying;
+ private float gravityAccel;
+ private IntPtr BoundingCapsule;
+ IntPtr capsule_geom;
+ d.Mass capsule_mass;
+
+ public OdeCharacter(OdeScene parent_scene, PhysicsVector pos)
+ {
+ _velocity = new PhysicsVector();
+ _position = pos;
+ _acceleration = new PhysicsVector();
+ d.MassSetCapsule(out capsule_mass, 5.0f, 3, 0.5f, 2f);
+ capsule_geom = d.CreateCapsule(OdeScene.space, 0.5f, 2f);
+ this.BoundingCapsule = d.BodyCreate(OdeScene.world);
+ d.BodySetMass(BoundingCapsule, ref capsule_mass);
+ d.BodySetPosition(BoundingCapsule, pos.X, pos.Y, pos.Z);
+ d.GeomSetBody(capsule_geom, BoundingCapsule);
+ }
+
+ public override bool Flying
+ {
+ get
+ {
+ return flying;
+ }
+ set
+ {
+ flying = value;
+ }
+ }
+
+ public override PhysicsVector Position
+ {
+ get
+ {
+ return _position;
+ }
+ set
+ {
+ _position = value;
+ }
+ }
+
+ public override PhysicsVector Velocity
+ {
+ get
+ {
+ return _velocity;
+ }
+ set
+ {
+ _velocity = value;
+ }
+ }
+
+ public override bool Kinematic
+ {
+ get
+ {
+ return false;
+ }
+ set
+ {
+
+ }
+ }
+
+ public override Axiom.MathLib.Quaternion Orientation
+ {
+ get
+ {
+ return Axiom.MathLib.Quaternion.Identity;
+ }
+ set
+ {
+
+ }
+ }
+
+ public override PhysicsVector Acceleration
+ {
+ get
+ {
+ return _acceleration;
+ }
+
+ }
+ public void SetAcceleration(PhysicsVector accel)
+ {
+ this._acceleration = accel;
+ }
+
+ public override void AddForce(PhysicsVector force)
+ {
+
+ }
+
+ public override void SetMomentum(PhysicsVector momentum)
+ {
+
+ }
+
+ public void Move(float timeStep)
+ {
+ PhysicsVector vec = new PhysicsVector();
+ vec.X = this._velocity.X * timeStep;
+ vec.Y = this._velocity.Y * timeStep;
+ if (flying)
+ {
+ vec.Z = (this._velocity.Z + 0.5f) * timeStep;
+ }
+ d.BodySetLinearVel(this.BoundingCapsule, vec.X, vec.Y, vec.Z);
+ }
+
+ public void UpdatePosition()
+ {
+ d.Vector3 vec = d.BodyGetPosition(BoundingCapsule);
+ this._position.X = vec.X;
+ this._position.Y = vec.Y;
+ this._position.Z = vec.Z;
+ }
+ }
+
+ public class OdePrim : PhysicsActor
+ {
+ private PhysicsVector _position;
+ private PhysicsVector _velocity;
+ private PhysicsVector _acceleration;
+
+ public OdePrim()
+ {
+ _velocity = new PhysicsVector();
+ _position = new PhysicsVector();
+ _acceleration = new PhysicsVector();
+ }
+ public override bool Flying
+ {
+ get
+ {
+ return false; //no flying prims for you
+ }
+ set
+ {
+
+ }
+ }
+ public override PhysicsVector Position
+ {
+ get
+ {
+ PhysicsVector pos = new PhysicsVector();
+ // PhysicsVector vec = this._prim.Position;
+ //pos.X = vec.X;
+ //pos.Y = vec.Y;
+ //pos.Z = vec.Z;
+ return pos;
+
+ }
+ set
+ {
+ /*PhysicsVector vec = value;
+ PhysicsVector pos = new PhysicsVector();
+ pos.X = vec.X;
+ pos.Y = vec.Y;
+ pos.Z = vec.Z;
+ this._prim.Position = pos;*/
+ }
+ }
+
+ public override PhysicsVector Velocity
+ {
+ get
+ {
+ return _velocity;
+ }
+ set
+ {
+ _velocity = value;
+ }
+ }
+
+ public override bool Kinematic
+ {
+ get
+ {
+ return false;
+ //return this._prim.Kinematic;
+ }
+ set
+ {
+ //this._prim.Kinematic = value;
+ }
+ }
+
+ public override Axiom.MathLib.Quaternion Orientation
+ {
+ get
+ {
+ Axiom.MathLib.Quaternion res = new Axiom.MathLib.Quaternion();
+ return res;
+ }
+ set
+ {
+
+ }
+ }
+
+ public override PhysicsVector Acceleration
+ {
+ get
+ {
+ return _acceleration;
+ }
+
+ }
+ public void SetAcceleration(PhysicsVector accel)
+ {
+ this._acceleration = accel;
+ }
+
+ public override void AddForce(PhysicsVector force)
+ {
+
+ }
+
+ public override void SetMomentum(PhysicsVector momentum)
+ {
+
+ }
+
+
+ }
}
--
cgit v1.1