aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
authorJohn Hurliman2009-10-25 23:16:12 -0700
committerJohn Hurliman2009-10-26 18:23:43 -0700
commitd199767e6991d6f368661fce9c5a072e564b8a4b (patch)
treed9347b8a424c0164e208f908613aa8fe1511444b /OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
parent* Double the priority on avatar bake texture requests to get avatars rezzing ... (diff)
downloadopensim-SC_OLD-d199767e6991d6f368661fce9c5a072e564b8a4b.zip
opensim-SC_OLD-d199767e6991d6f368661fce9c5a072e564b8a4b.tar.gz
opensim-SC_OLD-d199767e6991d6f368661fce9c5a072e564b8a4b.tar.bz2
opensim-SC_OLD-d199767e6991d6f368661fce9c5a072e564b8a4b.tar.xz
Experimental change of PhysicsVector to Vector3. Untested
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs269
1 files changed, 132 insertions, 137 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 09c8582..5ff9d32 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -57,44 +57,43 @@ namespace OpenSim.Region.Physics.OdePlugin
57 { 57 {
58 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 58 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
59 59
60 private PhysicsVector _position; 60 private Vector3 _position;
61 private PhysicsVector _velocity; 61 private Vector3 _velocity;
62 private PhysicsVector _torque = new PhysicsVector(0,0,0); 62 private Vector3 _torque;
63 private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f); 63 private Vector3 m_lastVelocity;
64 private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f); 64 private Vector3 m_lastposition;
65 private Quaternion m_lastorientation = new Quaternion(); 65 private Quaternion m_lastorientation = new Quaternion();
66 private PhysicsVector m_rotationalVelocity; 66 private Vector3 m_rotationalVelocity;
67 private PhysicsVector _size; 67 private Vector3 _size;
68 private PhysicsVector _acceleration; 68 private Vector3 _acceleration;
69 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); 69 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
70 private Quaternion _orientation; 70 private Quaternion _orientation;
71 private PhysicsVector m_taintposition; 71 private Vector3 m_taintposition;
72 private PhysicsVector m_taintsize; 72 private Vector3 m_taintsize;
73 private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0); 73 private Vector3 m_taintVelocity;
74 private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0); 74 private Vector3 m_taintTorque;
75 private Quaternion m_taintrot; 75 private Quaternion m_taintrot;
76 private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f); 76 private Vector3 m_angularlock = Vector3.One;
77 private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f); 77 private Vector3 m_taintAngularLock = Vector3.One;
78 private IntPtr Amotor = IntPtr.Zero; 78 private IntPtr Amotor = IntPtr.Zero;
79 79
80 private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0); 80 private Vector3 m_PIDTarget;
81 // private PhysicsVector m_taintPIDTarget = new PhysicsVector(0, 0, 0); 81 private float m_PIDTau;
82 private float m_PIDTau = 0f;
83 private float PID_D = 35f; 82 private float PID_D = 35f;
84 private float PID_G = 25f; 83 private float PID_G = 25f;
85 private bool m_usePID = false; 84 private bool m_usePID;
86 85
87 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 86 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
88 // and are for non-VEHICLES only. 87 // and are for non-VEHICLES only.
89 88
90 private float m_PIDHoverHeight = 0f; 89 private float m_PIDHoverHeight;
91 private float m_PIDHoverTau = 0f; 90 private float m_PIDHoverTau;
92 private bool m_useHoverPID = false; 91 private bool m_useHoverPID;
93 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; 92 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
94 private float m_targetHoverHeight = 0f; 93 private float m_targetHoverHeight;
95 private float m_groundHeight = 0f; 94 private float m_groundHeight;
96 private float m_waterHeight = 0f; 95 private float m_waterHeight;
97 private float m_buoyancy = 0f; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 96 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
98 97
99 // private float m_tensor = 5f; 98 // private float m_tensor = 5f;
100 private int body_autodisable_frames = 20; 99 private int body_autodisable_frames = 20;
@@ -105,11 +104,11 @@ namespace OpenSim.Region.Physics.OdePlugin
105 | CollisionCategories.Body 104 | CollisionCategories.Body
106 | CollisionCategories.Character 105 | CollisionCategories.Character
107 ); 106 );
108 private bool m_taintshape = false; 107 private bool m_taintshape;
109 private bool m_taintPhysics = false; 108 private bool m_taintPhysics;
110 private bool m_collidesLand = true; 109 private bool m_collidesLand = true;
111 private bool m_collidesWater = false; 110 private bool m_collidesWater;
112 public bool m_returnCollisions = false; 111 public bool m_returnCollisions;
113 112
114 // Default we're a Geometry 113 // Default we're a Geometry
115 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 114 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
@@ -117,85 +116,83 @@ namespace OpenSim.Region.Physics.OdePlugin
117 // Default, Collide with Other Geometries, spaces and Bodies 116 // Default, Collide with Other Geometries, spaces and Bodies
118 private CollisionCategories m_collisionFlags = m_default_collisionFlags; 117 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
119 118
120 public bool m_taintremove = false; 119 public bool m_taintremove;
121 public bool m_taintdisable = false; 120 public bool m_taintdisable;
122 public bool m_disabled = false; 121 public bool m_disabled;
123 public bool m_taintadd = false; 122 public bool m_taintadd;
124 public bool m_taintselected = false; 123 public bool m_taintselected;
125 public bool m_taintCollidesWater = false; 124 public bool m_taintCollidesWater;
126 125
127 public uint m_localID = 0; 126 public uint m_localID;
128 127
129 //public GCHandle gc; 128 //public GCHandle gc;
130 private CollisionLocker ode; 129 private CollisionLocker ode;
131 130
132 private bool m_taintforce = false; 131 private bool m_taintforce = false;
133 private bool m_taintaddangularforce = false; 132 private bool m_taintaddangularforce = false;
134 private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f); 133 private Vector3 m_force;
135 private List<PhysicsVector> m_forcelist = new List<PhysicsVector>(); 134 private List<Vector3> m_forcelist = new List<Vector3>();
136 private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>(); 135 private List<Vector3> m_angularforcelist = new List<Vector3>();
137 136
138 private IMesh _mesh; 137 private IMesh _mesh;
139 private PrimitiveBaseShape _pbs; 138 private PrimitiveBaseShape _pbs;
140 private OdeScene _parent_scene; 139 private OdeScene _parent_scene;
141 public IntPtr m_targetSpace = (IntPtr) 0; 140 public IntPtr m_targetSpace = IntPtr.Zero;
142 public IntPtr prim_geom; 141 public IntPtr prim_geom;
143 public IntPtr prev_geom; 142 public IntPtr prev_geom;
144 public IntPtr _triMeshData; 143 public IntPtr _triMeshData;
145 144
146 private IntPtr _linkJointGroup = (IntPtr)0; 145 private IntPtr _linkJointGroup = IntPtr.Zero;
147 private PhysicsActor _parent = null; 146 private PhysicsActor _parent;
148 private PhysicsActor m_taintparent = null; 147 private PhysicsActor m_taintparent;
149 148
150 private List<OdePrim> childrenPrim = new List<OdePrim>(); 149 private List<OdePrim> childrenPrim = new List<OdePrim>();
151 150
152 private bool iscolliding = false; 151 private bool iscolliding;
153 private bool m_isphysical = false; 152 private bool m_isphysical;
154 private bool m_isSelected = false; 153 private bool m_isSelected;
155 154
156 internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively 155 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
157 156
158 private bool m_throttleUpdates = false; 157 private bool m_throttleUpdates;
159 private int throttleCounter = 0; 158 private int throttleCounter;
160 public int m_interpenetrationcount = 0; 159 public int m_interpenetrationcount;
161 public float m_collisionscore = 0; 160 public float m_collisionscore;
162 public int m_roundsUnderMotionThreshold = 0; 161 public int m_roundsUnderMotionThreshold;
163 private int m_crossingfailures = 0; 162 private int m_crossingfailures;
164 163
165 public bool outofBounds = false; 164 public bool outofBounds;
166 private float m_density = 10.000006836f; // Aluminum g/cm3; 165 private float m_density = 10.000006836f; // Aluminum g/cm3;
167 166
168 public bool _zeroFlag = false; 167 public bool _zeroFlag;
169 private bool m_lastUpdateSent = false; 168 private bool m_lastUpdateSent;
170 169
171 public IntPtr Body = (IntPtr) 0; 170 public IntPtr Body = IntPtr.Zero;
172 public String m_primName; 171 public String m_primName;
173// private String m_primName; 172 private Vector3 _target_velocity;
174 private PhysicsVector _target_velocity;
175 public d.Mass pMass; 173 public d.Mass pMass;
176 174
177 public int m_eventsubscription = 0; 175 public int m_eventsubscription;
178 private CollisionEventUpdate CollisionEventsThisFrame = null; 176 private CollisionEventUpdate CollisionEventsThisFrame;
179 177
180 private IntPtr m_linkJoint = (IntPtr)0; 178 private IntPtr m_linkJoint = IntPtr.Zero;
181 179
182 public volatile bool childPrim = false; 180 public volatile bool childPrim;
183 181
184 private ODEDynamics m_vehicle; 182 private ODEDynamics m_vehicle;
185 183
186 internal int m_material = (int)Material.Wood; 184 internal int m_material = (int)Material.Wood;
187 185
188 public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size, 186 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
189 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 187 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
190 { 188 {
191 _target_velocity = new PhysicsVector(0, 0, 0);
192 m_vehicle = new ODEDynamics(); 189 m_vehicle = new ODEDynamics();
193 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); 190 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
194 ode = dode; 191 ode = dode;
195 _velocity = new PhysicsVector(); 192 if (!pos.IsFinite())
196 if (!PhysicsVector.isFinite(pos))
197 { 193 {
198 pos = new PhysicsVector(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f), parent_scene.GetTerrainHeightAtXY(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f)) + 0.5f); 194 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
195 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
199 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); 196 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
200 } 197 }
201 _position = pos; 198 _position = pos;
@@ -210,9 +207,9 @@ namespace OpenSim.Region.Physics.OdePlugin
210 prim_geom = IntPtr.Zero; 207 prim_geom = IntPtr.Zero;
211 prev_geom = IntPtr.Zero; 208 prev_geom = IntPtr.Zero;
212 209
213 if (!PhysicsVector.isFinite(pos)) 210 if (!pos.IsFinite())
214 { 211 {
215 size = new PhysicsVector(0.5f, 0.5f, 0.5f); 212 size = new Vector3(0.5f, 0.5f, 0.5f);
216 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size"); 213 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
217 } 214 }
218 215
@@ -222,8 +219,6 @@ namespace OpenSim.Region.Physics.OdePlugin
222 219
223 _size = size; 220 _size = size;
224 m_taintsize = _size; 221 m_taintsize = _size;
225 _acceleration = new PhysicsVector();
226 m_rotationalVelocity = PhysicsVector.Zero;
227 222
228 if (!QuaternionIsFinite(rotation)) 223 if (!QuaternionIsFinite(rotation))
229 { 224 {
@@ -388,7 +383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
388 m_disabled = false; 383 m_disabled = false;
389 384
390 // The body doesn't already have a finite rotation mode set here 385 // The body doesn't already have a finite rotation mode set here
391 if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null) 386 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
392 { 387 {
393 createAMotor(m_angularlock); 388 createAMotor(m_angularlock);
394 } 389 }
@@ -882,7 +877,7 @@ namespace OpenSim.Region.Physics.OdePlugin
882 877
883 if (prim_geom != IntPtr.Zero) 878 if (prim_geom != IntPtr.Zero)
884 { 879 {
885 if (!_position.IsIdentical(m_taintposition,0f)) 880 if (!_position.ApproxEquals(m_taintposition, 0f))
886 changemove(timestep); 881 changemove(timestep);
887 882
888 if (m_taintrot != _orientation) 883 if (m_taintrot != _orientation)
@@ -907,7 +902,7 @@ namespace OpenSim.Region.Physics.OdePlugin
907 changePhysicsStatus(timestep); 902 changePhysicsStatus(timestep);
908 // 903 //
909 904
910 if (!_size.IsIdentical(m_taintsize,0)) 905 if (!_size.ApproxEquals(m_taintsize,0f))
911 changesize(timestep); 906 changesize(timestep);
912 // 907 //
913 908
@@ -921,7 +916,7 @@ namespace OpenSim.Region.Physics.OdePlugin
921 if (m_taintaddangularforce) 916 if (m_taintaddangularforce)
922 changeAddAngularForce(timestep); 917 changeAddAngularForce(timestep);
923 918
924 if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f)) 919 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
925 changeSetTorque(timestep); 920 changeSetTorque(timestep);
926 921
927 if (m_taintdisable) 922 if (m_taintdisable)
@@ -930,7 +925,7 @@ namespace OpenSim.Region.Physics.OdePlugin
930 if (m_taintselected != m_isSelected) 925 if (m_taintselected != m_isSelected)
931 changeSelectedStatus(timestep); 926 changeSelectedStatus(timestep);
932 927
933 if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f)) 928 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
934 changevelocity(timestep); 929 changevelocity(timestep);
935 930
936 if (m_taintparent != _parent) 931 if (m_taintparent != _parent)
@@ -939,7 +934,7 @@ namespace OpenSim.Region.Physics.OdePlugin
939 if (m_taintCollidesWater != m_collidesWater) 934 if (m_taintCollidesWater != m_collidesWater)
940 changefloatonwater(timestep); 935 changefloatonwater(timestep);
941 936
942 if (!m_angularlock.IsIdentical(m_taintAngularLock,0)) 937 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
943 changeAngularLock(timestep); 938 changeAngularLock(timestep);
944 939
945 } 940 }
@@ -959,7 +954,7 @@ namespace OpenSim.Region.Physics.OdePlugin
959 //If we have a parent then we're not authorative here 954 //If we have a parent then we're not authorative here
960 if (_parent == null) 955 if (_parent == null)
961 { 956 {
962 if (!m_taintAngularLock.IsIdentical(new PhysicsVector(1f,1f,1f), 0)) 957 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
963 { 958 {
964 //d.BodySetFiniteRotationMode(Body, 0); 959 //d.BodySetFiniteRotationMode(Body, 0);
965 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); 960 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
@@ -976,7 +971,7 @@ namespace OpenSim.Region.Physics.OdePlugin
976 } 971 }
977 } 972 }
978 // Store this for later in case we get turned into a separate body 973 // Store this for later in case we get turned into a separate body
979 m_angularlock = new PhysicsVector(m_taintAngularLock.X, m_taintAngularLock.Y, m_taintAngularLock.Z); 974 m_angularlock = m_taintAngularLock;
980 975
981 } 976 }
982 977
@@ -1120,7 +1115,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1120 prm.m_disabled = false; 1115 prm.m_disabled = false;
1121 1116
1122 // The body doesn't already have a finite rotation mode set here 1117 // The body doesn't already have a finite rotation mode set here
1123 if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null) 1118 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1124 { 1119 {
1125 prm.createAMotor(m_angularlock); 1120 prm.createAMotor(m_angularlock);
1126 } 1121 }
@@ -1163,7 +1158,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1163 m_disabled = false; 1158 m_disabled = false;
1164 1159
1165 // The body doesn't already have a finite rotation mode set here 1160 // The body doesn't already have a finite rotation mode set here
1166 if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null) 1161 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1167 { 1162 {
1168 createAMotor(m_angularlock); 1163 createAMotor(m_angularlock);
1169 } 1164 }
@@ -1347,7 +1342,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1347 m_taintshape = false; 1342 m_taintshape = false;
1348 m_taintforce = false; 1343 m_taintforce = false;
1349 m_taintdisable = false; 1344 m_taintdisable = false;
1350 m_taintVelocity = PhysicsVector.Zero; 1345 m_taintVelocity = Vector3.Zero;
1351 } 1346 }
1352 1347
1353 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1348 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
@@ -1576,7 +1571,7 @@ Console.WriteLine(" JointCreateFixed");
1576//Console.WriteLine("Move " + m_primName); 1571//Console.WriteLine("Move " + m_primName);
1577 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1572 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1578 // NON-'VEHICLES' are dealt with here 1573 // NON-'VEHICLES' are dealt with here
1579 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) 1574 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1580 { 1575 {
1581 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1576 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1582 if (m_angularlock.X == 1) 1577 if (m_angularlock.X == 1)
@@ -1633,7 +1628,7 @@ Console.WriteLine(" JointCreateFixed");
1633 1628
1634 d.Vector3 pos = d.BodyGetPosition(Body); 1629 d.Vector3 pos = d.BodyGetPosition(Body);
1635 _target_velocity = 1630 _target_velocity =
1636 new PhysicsVector( 1631 new Vector3(
1637 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 1632 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1638 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 1633 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1639 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 1634 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
@@ -1641,7 +1636,7 @@ Console.WriteLine(" JointCreateFixed");
1641 1636
1642 // if velocity is zero, use position control; otherwise, velocity control 1637 // if velocity is zero, use position control; otherwise, velocity control
1643 1638
1644 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) 1639 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1645 { 1640 {
1646 // keep track of where we stopped. No more slippin' & slidin' 1641 // keep track of where we stopped. No more slippin' & slidin'
1647 1642
@@ -1726,13 +1721,13 @@ Console.WriteLine(" JointCreateFixed");
1726 1721
1727 1722
1728 _target_velocity = 1723 _target_velocity =
1729 new PhysicsVector(0.0f, 0.0f, 1724 new Vector3(0.0f, 0.0f,
1730 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 1725 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1731 ); 1726 );
1732 1727
1733 // if velocity is zero, use position control; otherwise, velocity control 1728 // if velocity is zero, use position control; otherwise, velocity control
1734 1729
1735 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) 1730 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1736 { 1731 {
1737 // keep track of where we stopped. No more slippin' & slidin' 1732 // keep track of where we stopped. No more slippin' & slidin'
1738 1733
@@ -1821,7 +1816,7 @@ Console.WriteLine(" JointCreateFixed");
1821 d.BodySetQuaternion(Body, ref myrot); 1816 d.BodySetQuaternion(Body, ref myrot);
1822 if (m_isphysical) 1817 if (m_isphysical)
1823 { 1818 {
1824 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0)) 1819 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1825 createAMotor(m_angularlock); 1820 createAMotor(m_angularlock);
1826 } 1821 }
1827 } 1822 }
@@ -2130,7 +2125,7 @@ Console.WriteLine(" JointCreateFixed");
2130 //m_log.Info("[PHYSICS]: dequeing forcelist"); 2125 //m_log.Info("[PHYSICS]: dequeing forcelist");
2131 if (IsPhysical) 2126 if (IsPhysical)
2132 { 2127 {
2133 PhysicsVector iforce = new PhysicsVector(); 2128 Vector3 iforce = Vector3.Zero;
2134 for (int i = 0; i < m_forcelist.Count; i++) 2129 for (int i = 0; i < m_forcelist.Count; i++)
2135 { 2130 {
2136 iforce = iforce + (m_forcelist[i] * 100); 2131 iforce = iforce + (m_forcelist[i] * 100);
@@ -2160,8 +2155,8 @@ Console.WriteLine(" JointCreateFixed");
2160 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); 2155 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2161 } 2156 }
2162 } 2157 }
2163 2158
2164 m_taintTorque = new PhysicsVector(0, 0, 0); 2159 m_taintTorque = Vector3.Zero;
2165 } 2160 }
2166 2161
2167 public void changeAddAngularForce(float timestamp) 2162 public void changeAddAngularForce(float timestamp)
@@ -2173,7 +2168,7 @@ Console.WriteLine(" JointCreateFixed");
2173 //m_log.Info("[PHYSICS]: dequeing forcelist"); 2168 //m_log.Info("[PHYSICS]: dequeing forcelist");
2174 if (IsPhysical) 2169 if (IsPhysical)
2175 { 2170 {
2176 PhysicsVector iforce = new PhysicsVector(); 2171 Vector3 iforce = Vector3.Zero;
2177 for (int i = 0; i < m_angularforcelist.Count; i++) 2172 for (int i = 0; i < m_angularforcelist.Count; i++)
2178 { 2173 {
2179 iforce = iforce + (m_angularforcelist[i] * 100); 2174 iforce = iforce + (m_angularforcelist[i] * 100);
@@ -2207,7 +2202,7 @@ Console.WriteLine(" JointCreateFixed");
2207 2202
2208 //resetCollisionAccounting(); 2203 //resetCollisionAccounting();
2209 } 2204 }
2210 m_taintVelocity = PhysicsVector.Zero; 2205 m_taintVelocity = Vector3.Zero;
2211 } 2206 }
2212 2207
2213 public override bool IsPhysical 2208 public override bool IsPhysical
@@ -2216,7 +2211,7 @@ Console.WriteLine(" JointCreateFixed");
2216 set { 2211 set {
2217 m_isphysical = value; 2212 m_isphysical = value;
2218 if (!m_isphysical) // Zero the remembered last velocity 2213 if (!m_isphysical) // Zero the remembered last velocity
2219 m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f); 2214 m_lastVelocity = Vector3.Zero;
2220 } 2215 }
2221 } 2216 }
2222 2217
@@ -2261,7 +2256,7 @@ Console.WriteLine(" JointCreateFixed");
2261 get { return _zeroFlag; } 2256 get { return _zeroFlag; }
2262 } 2257 }
2263 2258
2264 public override PhysicsVector Position 2259 public override Vector3 Position
2265 { 2260 {
2266 get { return _position; } 2261 get { return _position; }
2267 2262
@@ -2270,12 +2265,12 @@ Console.WriteLine(" JointCreateFixed");
2270 } 2265 }
2271 } 2266 }
2272 2267
2273 public override PhysicsVector Size 2268 public override Vector3 Size
2274 { 2269 {
2275 get { return _size; } 2270 get { return _size; }
2276 set 2271 set
2277 { 2272 {
2278 if (PhysicsVector.isFinite(value)) 2273 if (value.IsFinite())
2279 { 2274 {
2280 _size = value; 2275 _size = value;
2281 } 2276 }
@@ -2291,13 +2286,13 @@ Console.WriteLine(" JointCreateFixed");
2291 get { return CalculateMass(); } 2286 get { return CalculateMass(); }
2292 } 2287 }
2293 2288
2294 public override PhysicsVector Force 2289 public override Vector3 Force
2295 { 2290 {
2296 //get { return PhysicsVector.Zero; } 2291 //get { return Vector3.Zero; }
2297 get { return m_force; } 2292 get { return m_force; }
2298 set 2293 set
2299 { 2294 {
2300 if (PhysicsVector.isFinite(value)) 2295 if (value.IsFinite())
2301 { 2296 {
2302 m_force = value; 2297 m_force = value;
2303 } 2298 }
@@ -2319,7 +2314,7 @@ Console.WriteLine(" JointCreateFixed");
2319 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); 2314 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2320 } 2315 }
2321 2316
2322 public override void VehicleVectorParam(int param, PhysicsVector value) 2317 public override void VehicleVectorParam(int param, Vector3 value)
2323 { 2318 {
2324 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); 2319 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2325 } 2320 }
@@ -2337,14 +2332,14 @@ Console.WriteLine(" JointCreateFixed");
2337 } 2332 }
2338 } 2333 }
2339 2334
2340 public override PhysicsVector CenterOfMass 2335 public override Vector3 CenterOfMass
2341 { 2336 {
2342 get { return PhysicsVector.Zero; } 2337 get { return Vector3.Zero; }
2343 } 2338 }
2344 2339
2345 public override PhysicsVector GeometricCenter 2340 public override Vector3 GeometricCenter
2346 { 2341 {
2347 get { return PhysicsVector.Zero; } 2342 get { return Vector3.Zero; }
2348 } 2343 }
2349 2344
2350 public override PrimitiveBaseShape Shape 2345 public override PrimitiveBaseShape Shape
@@ -2356,13 +2351,13 @@ Console.WriteLine(" JointCreateFixed");
2356 } 2351 }
2357 } 2352 }
2358 2353
2359 public override PhysicsVector Velocity 2354 public override Vector3 Velocity
2360 { 2355 {
2361 get 2356 get
2362 { 2357 {
2363 // Averate previous velocity with the new one so 2358 // Averate previous velocity with the new one so
2364 // client object interpolation works a 'little' better 2359 // client object interpolation works a 'little' better
2365 PhysicsVector returnVelocity = new PhysicsVector(); 2360 Vector3 returnVelocity = Vector3.Zero;
2366 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; 2361 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
2367 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; 2362 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
2368 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; 2363 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
@@ -2370,7 +2365,7 @@ Console.WriteLine(" JointCreateFixed");
2370 } 2365 }
2371 set 2366 set
2372 { 2367 {
2373 if (PhysicsVector.isFinite(value)) 2368 if (value.IsFinite())
2374 { 2369 {
2375 _velocity = value; 2370 _velocity = value;
2376 2371
@@ -2385,19 +2380,19 @@ Console.WriteLine(" JointCreateFixed");
2385 } 2380 }
2386 } 2381 }
2387 2382
2388 public override PhysicsVector Torque 2383 public override Vector3 Torque
2389 { 2384 {
2390 get 2385 get
2391 { 2386 {
2392 if (!m_isphysical || Body == IntPtr.Zero) 2387 if (!m_isphysical || Body == IntPtr.Zero)
2393 return new PhysicsVector(0,0,0); 2388 return Vector3.Zero;
2394 2389
2395 return _torque; 2390 return _torque;
2396 } 2391 }
2397 2392
2398 set 2393 set
2399 { 2394 {
2400 if (PhysicsVector.isFinite(value)) 2395 if (value.IsFinite())
2401 { 2396 {
2402 m_taintTorque = value; 2397 m_taintTorque = value;
2403 _parent_scene.AddPhysicsActorTaint(this); 2398 _parent_scene.AddPhysicsActorTaint(this);
@@ -2449,20 +2444,20 @@ Console.WriteLine(" JointCreateFixed");
2449 return true; 2444 return true;
2450 } 2445 }
2451 2446
2452 public override PhysicsVector Acceleration 2447 public override Vector3 Acceleration
2453 { 2448 {
2454 get { return _acceleration; } 2449 get { return _acceleration; }
2455 } 2450 }
2456 2451
2457 2452
2458 public void SetAcceleration(PhysicsVector accel) 2453 public void SetAcceleration(Vector3 accel)
2459 { 2454 {
2460 _acceleration = accel; 2455 _acceleration = accel;
2461 } 2456 }
2462 2457
2463 public override void AddForce(PhysicsVector force, bool pushforce) 2458 public override void AddForce(Vector3 force, bool pushforce)
2464 { 2459 {
2465 if (PhysicsVector.isFinite(force)) 2460 if (force.IsFinite())
2466 { 2461 {
2467 m_forcelist.Add(force); 2462 m_forcelist.Add(force);
2468 m_taintforce = true; 2463 m_taintforce = true;
@@ -2474,9 +2469,9 @@ Console.WriteLine(" JointCreateFixed");
2474 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); 2469 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2475 } 2470 }
2476 2471
2477 public override void AddAngularForce(PhysicsVector force, bool pushforce) 2472 public override void AddAngularForce(Vector3 force, bool pushforce)
2478 { 2473 {
2479 if (PhysicsVector.isFinite(force)) 2474 if (force.IsFinite())
2480 { 2475 {
2481 m_angularforcelist.Add(force); 2476 m_angularforcelist.Add(force);
2482 m_taintaddangularforce = true; 2477 m_taintaddangularforce = true;
@@ -2487,23 +2482,23 @@ Console.WriteLine(" JointCreateFixed");
2487 } 2482 }
2488 } 2483 }
2489 2484
2490 public override PhysicsVector RotationalVelocity 2485 public override Vector3 RotationalVelocity
2491 { 2486 {
2492 get 2487 get
2493 { 2488 {
2494 PhysicsVector pv = new PhysicsVector(0, 0, 0); 2489 Vector3 pv = Vector3.Zero;
2495 if (_zeroFlag) 2490 if (_zeroFlag)
2496 return pv; 2491 return pv;
2497 m_lastUpdateSent = false; 2492 m_lastUpdateSent = false;
2498 2493
2499 if (m_rotationalVelocity.IsIdentical(pv, 0.2f)) 2494 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2500 return pv; 2495 return pv;
2501 2496
2502 return m_rotationalVelocity; 2497 return m_rotationalVelocity;
2503 } 2498 }
2504 set 2499 set
2505 { 2500 {
2506 if (PhysicsVector.isFinite(value)) 2501 if (value.IsFinite())
2507 { 2502 {
2508 m_rotationalVelocity = value; 2503 m_rotationalVelocity = value;
2509 } 2504 }
@@ -2544,16 +2539,16 @@ Console.WriteLine(" JointCreateFixed");
2544 m_taintparent = null; 2539 m_taintparent = null;
2545 } 2540 }
2546 2541
2547 public override void LockAngularMotion(PhysicsVector axis) 2542 public override void LockAngularMotion(Vector3 axis)
2548 { 2543 {
2549 // reverse the zero/non zero values for ODE. 2544 // reverse the zero/non zero values for ODE.
2550 if (PhysicsVector.isFinite(axis)) 2545 if (axis.IsFinite())
2551 { 2546 {
2552 axis.X = (axis.X > 0) ? 1f : 0f; 2547 axis.X = (axis.X > 0) ? 1f : 0f;
2553 axis.Y = (axis.Y > 0) ? 1f : 0f; 2548 axis.Y = (axis.Y > 0) ? 1f : 0f;
2554 axis.Z = (axis.Z > 0) ? 1f : 0f; 2549 axis.Z = (axis.Z > 0) ? 1f : 0f;
2555 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); 2550 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2556 m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z); 2551 m_taintAngularLock = axis;
2557 } 2552 }
2558 else 2553 else
2559 { 2554 {
@@ -2566,7 +2561,7 @@ Console.WriteLine(" JointCreateFixed");
2566 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 2561 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2567 if (_parent == null) 2562 if (_parent == null)
2568 { 2563 {
2569 PhysicsVector pv = new PhysicsVector(0, 0, 0); 2564 Vector3 pv = Vector3.Zero;
2570 bool lastZeroFlag = _zeroFlag; 2565 bool lastZeroFlag = _zeroFlag;
2571 if (Body != (IntPtr)0) // FIXME -> or if it is a joint 2566 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2572 { 2567 {
@@ -2575,9 +2570,9 @@ Console.WriteLine(" JointCreateFixed");
2575 d.Vector3 vel = d.BodyGetLinearVel(Body); 2570 d.Vector3 vel = d.BodyGetLinearVel(Body);
2576 d.Vector3 rotvel = d.BodyGetAngularVel(Body); 2571 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2577 d.Vector3 torque = d.BodyGetTorque(Body); 2572 d.Vector3 torque = d.BodyGetTorque(Body);
2578 _torque.setValues(torque.X, torque.Y, torque.Z); 2573 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2579 PhysicsVector l_position = new PhysicsVector(); 2574 Vector3 l_position = Vector3.Zero;
2580 Quaternion l_orientation = new Quaternion(); 2575 Quaternion l_orientation = Quaternion.Identity;
2581 2576
2582 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) 2577 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2583 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 2578 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
@@ -2712,16 +2707,16 @@ Console.WriteLine(" JointCreateFixed");
2712 _velocity.Z = vel.Z; 2707 _velocity.Z = vel.Z;
2713 2708
2714 _acceleration = ((_velocity - m_lastVelocity) / 0.1f); 2709 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2715 _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); 2710 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2716 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); 2711 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2717 2712
2718 if (_velocity.IsIdentical(pv, 0.5f)) 2713 if (_velocity.ApproxEquals(pv, 0.5f))
2719 { 2714 {
2720 m_rotationalVelocity = pv; 2715 m_rotationalVelocity = pv;
2721 } 2716 }
2722 else 2717 else
2723 { 2718 {
2724 m_rotationalVelocity.setValues(rotvel.X, rotvel.Y, rotvel.Z); 2719 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2725 } 2720 }
2726 2721
2727 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); 2722 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
@@ -2769,15 +2764,15 @@ Console.WriteLine(" JointCreateFixed");
2769 } 2764 }
2770 } 2765 }
2771 2766
2772 public override void SetMomentum(PhysicsVector momentum) 2767 public override void SetMomentum(Vector3 momentum)
2773 { 2768 {
2774 } 2769 }
2775 2770
2776 public override PhysicsVector PIDTarget 2771 public override Vector3 PIDTarget
2777 { 2772 {
2778 set 2773 set
2779 { 2774 {
2780 if (PhysicsVector.isFinite(value)) 2775 if (value.IsFinite())
2781 { 2776 {
2782 m_PIDTarget = value; 2777 m_PIDTarget = value;
2783 } 2778 }
@@ -2793,7 +2788,7 @@ Console.WriteLine(" JointCreateFixed");
2793 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 2788 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2794 public override float PIDHoverTau { set { m_PIDHoverTau = value; } } 2789 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2795 2790
2796 private void createAMotor(PhysicsVector axis) 2791 private void createAMotor(Vector3 axis)
2797 { 2792 {
2798 if (Body == IntPtr.Zero) 2793 if (Body == IntPtr.Zero)
2799 return; 2794 return;