aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
authorubit2012-10-12 01:36:55 +0200
committerubit2012-10-12 01:36:55 +0200
commitb0738e1c062dd7d4fc8206422de8d964298a2c0d (patch)
treeeecf4c8e4068db9cc3481528b28e9b67213aab7e /OpenSim/Region/Physics
parentMerge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork (diff)
parent remove some more debug spam on ode (diff)
downloadopensim-SC_OLD-b0738e1c062dd7d4fc8206422de8d964298a2c0d.zip
opensim-SC_OLD-b0738e1c062dd7d4fc8206422de8d964298a2c0d.tar.gz
opensim-SC_OLD-b0738e1c062dd7d4fc8206422de8d964298a2c0d.tar.bz2
opensim-SC_OLD-b0738e1c062dd7d4fc8206422de8d964298a2c0d.tar.xz
Merge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork
Diffstat (limited to 'OpenSim/Region/Physics')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs5346
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs5155
2 files changed, 5735 insertions, 4766 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index eaee950..eaf0d0a 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -25,11 +25,6 @@
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
33/* 28/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 29 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
@@ -53,251 +48,250 @@ using System.Runtime.InteropServices;
53using System.Threading; 48using System.Threading;
54using log4net; 49using log4net;
55using OpenMetaverse; 50using OpenMetaverse;
56using OdeAPI; 51using Ode.NET;
57using OpenSim.Framework; 52using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager; 53using OpenSim.Region.Physics.Manager;
59 54
60namespace OpenSim.Region.Physics.OdePlugin 55namespace OpenSim.Region.Physics.OdePlugin
61{ 56{
57 /// <summary>
58 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
59 /// </summary>
62 public class OdePrim : PhysicsActor 60 public class OdePrim : PhysicsActor
63 { 61 {
64 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 62 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
65 63
66 private bool m_isphysical; 64 private bool m_isphysical;
67 private bool m_fakeisphysical;
68 private bool m_isphantom;
69 private bool m_fakeisphantom;
70 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
71 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72 65
73 protected bool m_building; 66 public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
74 protected bool m_forcePosOrRotation; 67 private int m_expectedCollisionContacts = 0;
75 private bool m_iscolliding;
76 68
77 internal bool m_isSelected; 69 /// <summary>
78 private bool m_delaySelect; 70 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
79 private bool m_lastdoneSelected; 71 /// </summary>
80 internal bool m_outbounds; 72 public override bool IsPhysical
81 73 {
82 private Quaternion m_lastorientation; 74 get { return m_isphysical; }
83 private Quaternion _orientation; 75 set
76 {
77 m_isphysical = value;
78 if (!m_isphysical) // Zero the remembered last velocity
79 m_lastVelocity = Vector3.Zero;
80 }
81 }
84 82
85 private Vector3 _position; 83 private Vector3 _position;
86 private Vector3 _velocity; 84 private Vector3 _velocity;
87 private Vector3 _torque; 85 private Vector3 _torque;
88 private Vector3 m_lastVelocity; 86 private Vector3 m_lastVelocity;
89 private Vector3 m_lastposition; 87 private Vector3 m_lastposition;
88 private Quaternion m_lastorientation = new Quaternion();
90 private Vector3 m_rotationalVelocity; 89 private Vector3 m_rotationalVelocity;
91 private Vector3 _size; 90 private Vector3 _size;
92 private Vector3 _acceleration; 91 private Vector3 _acceleration;
92 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
93 private Quaternion _orientation;
94 private Vector3 m_taintposition;
95 private Vector3 m_taintsize;
96 private Vector3 m_taintVelocity;
97 private Vector3 m_taintTorque;
98 private Quaternion m_taintrot;
93 private Vector3 m_angularlock = Vector3.One; 99 private Vector3 m_angularlock = Vector3.One;
94 private IntPtr Amotor; 100 private Vector3 m_taintAngularLock = Vector3.One;
101 private IntPtr Amotor = IntPtr.Zero;
95 102
96 private Vector3 m_force; 103 private object m_assetsLock = new object();
97 private Vector3 m_forceacc; 104 private bool m_assetFailed = false;
98 private Vector3 m_angularForceacc;
99
100 private float m_invTimeStep;
101 private float m_timeStep;
102 105
103 private Vector3 m_PIDTarget; 106 private Vector3 m_PIDTarget;
104 private float m_PIDTau; 107 private float m_PIDTau;
108 private float PID_D = 35f;
109 private float PID_G = 25f;
105 private bool m_usePID; 110 private bool m_usePID;
106 111
112 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
113 // and are for non-VEHICLES only.
114
107 private float m_PIDHoverHeight; 115 private float m_PIDHoverHeight;
108 private float m_PIDHoverTau; 116 private float m_PIDHoverTau;
109 private bool m_useHoverPID; 117 private bool m_useHoverPID;
110 private PIDHoverType m_PIDHoverType; 118 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
111 private float m_targetHoverHeight; 119 private float m_targetHoverHeight;
112 private float m_groundHeight; 120 private float m_groundHeight;
113 private float m_waterHeight; 121 private float m_waterHeight;
114 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 122 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
115 123
116 private int body_autodisable_frames; 124 // private float m_tensor = 5f;
117 public int bodydisablecontrol; 125 private int body_autodisable_frames = 20;
118
119 126
120 // Default we're a Geometry
121 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
122 // Default colide nonphysical don't try to colide with anything
123 private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
124 127
125 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | 128 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
126 CollisionCategories.Character | 129 | CollisionCategories.Space
127 CollisionCategories.Land | 130 | CollisionCategories.Body
128 CollisionCategories.VolumeDtc); 131 | CollisionCategories.Character
129 132 );
130// private bool m_collidesLand = true; 133 private bool m_taintshape;
134 private bool m_taintPhysics;
135 private bool m_collidesLand = true;
131 private bool m_collidesWater; 136 private bool m_collidesWater;
132// public bool m_returnCollisions;
133
134 private bool m_NoColide; // for now only for internal use for bad meshs
135 137
138 // Default we're a Geometry
139 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
136 140
137 // Default, Collide with Other Geometries, spaces and Bodies 141 // Default, Collide with Other Geometries, spaces and Bodies
138 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; 142 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
139 143
140 public bool m_disabled; 144 public bool m_taintremove { get; private set; }
145 public bool m_taintdisable { get; private set; }
146 internal bool m_disabled;
147 public bool m_taintadd { get; private set; }
148 public bool m_taintselected { get; private set; }
149 public bool m_taintCollidesWater { get; private set; }
141 150
142 private uint m_localID; 151 private bool m_taintforce = false;
152 private bool m_taintaddangularforce = false;
153 private Vector3 m_force;
154 private List<Vector3> m_forcelist = new List<Vector3>();
155 private List<Vector3> m_angularforcelist = new List<Vector3>();
143 156
144 private IMesh m_mesh;
145 private object m_meshlock = new object();
146 private PrimitiveBaseShape _pbs; 157 private PrimitiveBaseShape _pbs;
158 private OdeScene _parent_scene;
147 159
148 private UUID? m_assetID; 160 /// <summary>
149 private MeshState m_meshState; 161 /// The physics space which contains prim geometries
150 162 /// </summary>
151 public OdeScene _parent_scene; 163 public IntPtr m_targetSpace = IntPtr.Zero;
152 164
153 /// <summary> 165 /// <summary>
154 /// The physics space which contains prim geometry 166 /// The prim geometry, used for collision detection.
155 /// </summary> 167 /// </summary>
156 public IntPtr m_targetSpace; 168 /// <remarks>
169 /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
170 /// mesh change) or when the physical prim is being removed from the scene.
171 /// </remarks>
172 public IntPtr prim_geom { get; private set; }
157 173
158 public IntPtr prim_geom; 174 public IntPtr _triMeshData { get; private set; }
159 public IntPtr _triMeshData;
160 175
176 private IntPtr _linkJointGroup = IntPtr.Zero;
161 private PhysicsActor _parent; 177 private PhysicsActor _parent;
178 private PhysicsActor m_taintparent;
162 179
163 private List<OdePrim> childrenPrim = new List<OdePrim>(); 180 private List<OdePrim> childrenPrim = new List<OdePrim>();
164 181
165 public float m_collisionscore; 182 private bool iscolliding;
166 private int m_colliderfilter = 0; 183 private bool m_isSelected;
167
168 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
169
170 private float m_density;
171 private byte m_shapetype;
172 public bool _zeroFlag;
173 private bool m_lastUpdateSent;
174 184
175 public IntPtr Body; 185 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
176 186
177 private Vector3 _target_velocity; 187 private bool m_throttleUpdates;
188 private int throttleCounter;
189 public int m_interpenetrationcount { get; private set; }
190 internal float m_collisionscore;
191 public int m_roundsUnderMotionThreshold { get; private set; }
192 private int m_crossingfailures;
178 193
179 public Vector3 m_OBBOffset; 194 public bool outofBounds { get; private set; }
180 public Vector3 m_OBB; 195 private float m_density = 10.000006836f; // Aluminum g/cm3;
181 public float primOOBradiusSQ;
182 196
183 private bool m_hasOBB = true; 197 public bool _zeroFlag { get; private set; }
198 private bool m_lastUpdateSent;
184 199
185 private float m_physCost; 200 public IntPtr Body = IntPtr.Zero;
186 private float m_streamCost; 201 private Vector3 _target_velocity;
202 private d.Mass pMass;
187 203
188 public d.Mass primdMass; // prim inertia information on it's own referencial 204 private int m_eventsubscription;
189 float primMass; // prim own mass 205 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
190 float primVolume; // prim own volume;
191 float _mass; // object mass acording to case
192 206
193 public int givefakepos; 207 /// <summary>
194 private Vector3 fakepos; 208 /// Signal whether there were collisions on the previous frame, so we know if we need to send the
195 public int givefakeori; 209 /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
196 private Quaternion fakeori; 210 /// </summary>
211 /// <remarks>
212 /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
213 /// </remarks>
214 private bool m_collisionsOnPreviousFrame;
197 215
198 private int m_eventsubscription; 216 private IntPtr m_linkJoint = IntPtr.Zero;
199 private int m_cureventsubscription;
200 private CollisionEventUpdate CollisionEventsThisFrame = null;
201 private bool SentEmptyCollisionsEvent;
202 217
203 public volatile bool childPrim; 218 internal volatile bool childPrim;
204 219
205 public ODEDynamics m_vehicle; 220 private ODEDynamics m_vehicle;
206 221
207 internal int m_material = (int)Material.Wood; 222 internal int m_material = (int)Material.Wood;
208 private float mu;
209 private float bounce;
210 223
211 /// <summary> 224 public OdePrim(
212 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. 225 String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
213 /// </summary> 226 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
214 public override bool IsPhysical // this is not reliable for internal use
215 { 227 {
216 get { return m_fakeisphysical; } 228 Name = primName;
217 set 229 m_vehicle = new ODEDynamics();
218 { 230 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
219 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
220 // and also to stop imediatly some updates
221 // but real change will only happen in taintprocessing
222
223 if (!value) // Zero the remembered last velocity
224 m_lastVelocity = Vector3.Zero;
225 AddChange(changes.Physical, value);
226 }
227 }
228 231
229 public override bool IsVolumeDtc 232 if (!pos.IsFinite())
230 {
231 get { return m_fakeisVolumeDetect; }
232 set
233 { 233 {
234 m_fakeisVolumeDetect = value; 234 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
235 AddChange(changes.VolumeDtc, value); 235 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
236 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
236 } 237 }
237 } 238 _position = pos;
239 m_taintposition = pos;
240 PID_D = parent_scene.bodyPIDD;
241 PID_G = parent_scene.bodyPIDG;
242 m_density = parent_scene.geomDefaultDensity;
243 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
244 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
238 245
239 public override bool Phantom // this is not reliable for internal use 246 prim_geom = IntPtr.Zero;
240 {
241 get { return m_fakeisphantom; }
242 set
243 {
244 m_fakeisphantom = value;
245 AddChange(changes.Phantom, value);
246 }
247 }
248 247
249 public override bool Building // this is not reliable for internal use 248 if (!pos.IsFinite())
250 {
251 get { return m_building; }
252 set
253 { 249 {
254 if (value) 250 size = new Vector3(0.5f, 0.5f, 0.5f);
255 m_building = true; 251 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
256 AddChange(changes.building, value);
257 } 252 }
258 }
259 253
260 public override void getContactData(ref ContactData cdata) 254 if (size.X <= 0) size.X = 0.01f;
261 { 255 if (size.Y <= 0) size.Y = 0.01f;
262 cdata.mu = mu; 256 if (size.Z <= 0) size.Z = 0.01f;
263 cdata.bounce = bounce;
264 257
265 // cdata.softcolide = m_softcolide; 258 _size = size;
266 cdata.softcolide = false; 259 m_taintsize = _size;
267 260
268 if (m_isphysical) 261 if (!QuaternionIsFinite(rotation))
269 { 262 {
270 ODEDynamics veh; 263 rotation = Quaternion.Identity;
271 if (_parent != null) 264 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
272 veh = ((OdePrim)_parent).m_vehicle;
273 else
274 veh = m_vehicle;
275
276 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
277 cdata.mu *= veh.FrictionFactor;
278// cdata.mu *= 0;
279 } 265 }
280 }
281 266
282 public override float PhysicsCost 267 _orientation = rotation;
283 { 268 m_taintrot = _orientation;
284 get 269 _pbs = pbs;
270
271 _parent_scene = parent_scene;
272 m_targetSpace = (IntPtr)0;
273
274 if (pos.Z < 0)
285 { 275 {
286 return m_physCost; 276 IsPhysical = false;
287 } 277 }
288 } 278 else
289
290 public override float StreamCost
291 {
292 get
293 { 279 {
294 return m_streamCost; 280 IsPhysical = pisPhysical;
281 // If we're physical, we need to be in the master space for now.
282 // linksets *should* be in a space together.. but are not currently
283 if (IsPhysical)
284 m_targetSpace = _parent_scene.space;
295 } 285 }
286
287 m_taintadd = true;
288 m_assetFailed = false;
289 _parent_scene.AddPhysicsActorTaint(this);
296 } 290 }
297 291
298 public override int PhysicsActorType 292 public override int PhysicsActorType
299 { 293 {
300 get { return (int)ActorTypes.Prim; } 294 get { return (int) ActorTypes.Prim; }
301 set { return; } 295 set { return; }
302 } 296 }
303 297
@@ -307,23 +301,6 @@ namespace OpenSim.Region.Physics.OdePlugin
307 set { return; } 301 set { return; }
308 } 302 }
309 303
310 public override uint LocalID
311 {
312 get { return m_localID; }
313 set { m_localID = value; }
314 }
315
316 public override PhysicsActor ParentActor
317 {
318 get
319 {
320 if (childPrim)
321 return _parent;
322 else
323 return (PhysicsActor)this;
324 }
325 }
326
327 public override bool Grabbed 304 public override bool Grabbed
328 { 305 {
329 set { return; } 306 set { return; }
@@ -333,3068 +310,2383 @@ namespace OpenSim.Region.Physics.OdePlugin
333 { 310 {
334 set 311 set
335 { 312 {
336 if (value) 313 // This only makes the object not collidable if the object
337 m_isSelected = value; // if true set imediatly to stop moves etc 314 // is physical or the object is modified somehow *IN THE FUTURE*
338 AddChange(changes.Selected, value); 315 // without this, if an avatar selects prim, they can walk right
339 } 316 // through it while it's selected
340 } 317 m_collisionscore = 0;
341
342 public override bool Flying
343 {
344 // no flying prims for you
345 get { return false; }
346 set { }
347 }
348 318
349 public override bool IsColliding 319 if ((IsPhysical && !_zeroFlag) || !value)
350 {
351 get { return m_iscolliding; }
352 set
353 {
354 if (value)
355 { 320 {
356 m_colliderfilter += 2; 321 m_taintselected = value;
357 if (m_colliderfilter > 2) 322 _parent_scene.AddPhysicsActorTaint(this);
358 m_colliderfilter = 2;
359 } 323 }
360 else 324 else
361 { 325 {
362 m_colliderfilter--; 326 m_taintselected = value;
363 if (m_colliderfilter < 0) 327 m_isSelected = value;
364 m_colliderfilter = 0;
365 } 328 }
366 329
367 if (m_colliderfilter == 0) 330 if (m_isSelected)
368 m_iscolliding = false; 331 disableBodySoft();
369 else
370 m_iscolliding = true;
371 } 332 }
372 } 333 }
373 334
374 public override bool CollidingGround 335 /// <summary>
375 { 336 /// Set a new geometry for this prim.
376 get { return false; } 337 /// </summary>
377 set { return; } 338 /// <param name="geom"></param>
378 } 339 private void SetGeom(IntPtr geom)
379
380 public override bool CollidingObj
381 {
382 get { return false; }
383 set { return; }
384 }
385
386
387 public override bool ThrottleUpdates {get;set;}
388
389 public override bool Stopped
390 { 340 {
391 get { return _zeroFlag; } 341 prim_geom = geom;
392 } 342//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
393 343
394 public override Vector3 Position 344 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
395 { 345 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
396 get
397 {
398 if (givefakepos > 0)
399 return fakepos;
400 else
401 return _position;
402 }
403 346
404 set 347 _parent_scene.geom_name_map[prim_geom] = Name;
405 { 348 _parent_scene.actor_name_map[prim_geom] = this;
406 fakepos = value;
407 givefakepos++;
408 AddChange(changes.Position, value);
409 }
410 }
411 349
412 public override Vector3 Size 350 if (childPrim)
413 {
414 get { return _size; }
415 set
416 { 351 {
417 if (value.IsFinite()) 352 if (_parent != null && _parent is OdePrim)
418 {
419 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype);
420 }
421 else
422 { 353 {
423 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); 354 OdePrim parent = (OdePrim)_parent;
355//Console.WriteLine("SetGeom calls ChildSetGeom");
356 parent.ChildSetGeom(this);
424 } 357 }
425 } 358 }
359 //m_log.Warn("Setting Geom to: " + prim_geom);
426 } 360 }
427 361
428 public override float Mass 362 private void enableBodySoft()
429 {
430 get { return primMass; }
431 }
432
433 public override Vector3 Force
434 { 363 {
435 get { return m_force; } 364 if (!childPrim)
436 set
437 { 365 {
438 if (value.IsFinite()) 366 if (IsPhysical && Body != IntPtr.Zero)
439 {
440 AddChange(changes.Force, value);
441 }
442 else
443 { 367 {
444 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); 368 d.BodyEnable(Body);
369 if (m_vehicle.Type != Vehicle.TYPE_NONE)
370 m_vehicle.Enable(Body, _parent_scene);
445 } 371 }
372
373 m_disabled = false;
446 } 374 }
447 } 375 }
448 376
449 public override void SetVolumeDetect(int param) 377 private void disableBodySoft()
450 { 378 {
451 m_fakeisVolumeDetect = (param != 0); 379 m_disabled = true;
452 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
453 }
454 380
455 public override Vector3 GeometricCenter 381 if (IsPhysical && Body != IntPtr.Zero)
456 {
457 // this is not real geometric center but a average of positions relative to root prim acording to
458 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
459 // ignoring tortured prims details since sl also seems to ignore
460 // so no real use in doing it on physics
461 get
462 { 382 {
463 return Vector3.Zero; 383 d.BodyDisable(Body);
464 } 384 }
465 } 385 }
466 386
467 public override Vector3 CenterOfMass 387 /// <summary>
388 /// Make a prim subject to physics.
389 /// </summary>
390 private void enableBody()
468 { 391 {
469 get 392 // Don't enable this body if we're a child prim
393 // this should be taken care of in the parent function not here
394 if (!childPrim)
470 { 395 {
471 lock (_parent_scene.OdeLock) 396 // Sets the geom to a body
472 { 397 Body = d.BodyCreate(_parent_scene.world);
473 d.Vector3 dtmp;
474 if (!childPrim && Body != IntPtr.Zero)
475 {
476 dtmp = d.BodyGetPosition(Body);
477 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
478 }
479 else if (prim_geom != IntPtr.Zero)
480 {
481 d.Quaternion dq;
482 d.GeomCopyQuaternion(prim_geom, out dq);
483 Quaternion q;
484 q.X = dq.X;
485 q.Y = dq.Y;
486 q.Z = dq.Z;
487 q.W = dq.W;
488
489 Vector3 Ptot = m_OBBOffset * q;
490 dtmp = d.GeomGetPosition(prim_geom);
491 Ptot.X += dtmp.X;
492 Ptot.Y += dtmp.Y;
493 Ptot.Z += dtmp.Z;
494
495 // if(childPrim) we only know about physical linksets
496 return Ptot;
497/*
498 float tmass = _mass;
499 Ptot *= tmass;
500 398
501 float m; 399 setMass();
502 400 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
503 foreach (OdePrim prm in childrenPrim) 401 d.Quaternion myrot = new d.Quaternion();
504 { 402 myrot.X = _orientation.X;
505 m = prm._mass; 403 myrot.Y = _orientation.Y;
506 Ptot += prm.CenterOfMass * m; 404 myrot.Z = _orientation.Z;
507 tmass += m; 405 myrot.W = _orientation.W;
508 } 406 d.BodySetQuaternion(Body, ref myrot);
407 d.GeomSetBody(prim_geom, Body);
408 m_collisionCategories |= CollisionCategories.Body;
409 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
509 410
510 if (tmass == 0) 411 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
511 tmass = 0; 412 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
512 else
513 tmass = 1.0f / tmass;
514 413
515 Ptot *= tmass; 414 d.BodySetAutoDisableFlag(Body, true);
516 return Ptot; 415 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
517*/ 416
518 } 417 // disconnect from world gravity so we can apply buoyancy
519 else 418 d.BodySetGravityMode (Body, false);
520 return _position;
521 }
522 }
523 }
524 419
525 public override Vector3 OOBsize 420 m_interpenetrationcount = 0;
526 { 421 m_collisionscore = 0;
527 get 422 m_disabled = false;
528 {
529 return m_OBB;
530 }
531 }
532 423
533 public override Vector3 OOBoffset 424 // The body doesn't already have a finite rotation mode set here
534 { 425 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
535 get
536 { 426 {
537 return m_OBBOffset; 427 createAMotor(m_angularlock);
538 } 428 }
539 } 429 if (m_vehicle.Type != Vehicle.TYPE_NONE)
540
541 public override float OOBRadiusSQ
542 {
543 get
544 { 430 {
545 return primOOBradiusSQ; 431 m_vehicle.Enable(Body, _parent_scene);
546 } 432 }
547 }
548 433
549 public override PrimitiveBaseShape Shape 434 _parent_scene.ActivatePrim(this);
550 {
551 set
552 {
553// AddChange(changes.Shape, value);
554 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype);
555 } 435 }
556 } 436 }
557 437
558 public override byte PhysicsShapeType 438 #region Mass Calculation
559 {
560 get
561 {
562 return m_shapetype;
563 }
564 set
565 {
566 m_shapetype = value;
567 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
568 }
569 }
570 439
571 public override Vector3 Velocity 440 private float CalculateMass()
572 { 441 {
573 get 442 float volume = _size.X * _size.Y * _size.Z; // default
574 { 443 float tmp;
575 if (_zeroFlag) 444
576 return Vector3.Zero; 445 float returnMass = 0;
577 return _velocity; 446 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
578 } 447 float hollowVolume = hollowAmount * hollowAmount;
579 set 448
449 switch (_pbs.ProfileShape)
580 { 450 {
581 if (value.IsFinite()) 451 case ProfileShape.Square:
582 { 452 // default box
583 AddChange(changes.Velocity, value);
584 }
585 else
586 {
587 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
588 }
589 453
590 } 454 if (_pbs.PathCurve == (byte)Extrusion.Straight)
591 } 455 {
456 if (hollowAmount > 0.0)
457 {
458 switch (_pbs.HollowShape)
459 {
460 case HollowShape.Square:
461 case HollowShape.Same:
462 break;
592 463
593 public override Vector3 Torque 464 case HollowShape.Circle:
594 {
595 get
596 {
597 if (!IsPhysical || Body == IntPtr.Zero)
598 return Vector3.Zero;
599 465
600 return _torque; 466 hollowVolume *= 0.78539816339f;
601 } 467 break;
602 468
603 set 469 case HollowShape.Triangle:
604 {
605 if (value.IsFinite())
606 {
607 AddChange(changes.Torque, value);
608 }
609 else
610 {
611 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
612 }
613 }
614 }
615 470
616 public override float CollisionScore 471 hollowVolume *= (0.5f * .5f);
617 { 472 break;
618 get { return m_collisionscore; }
619 set { m_collisionscore = value; }
620 }
621 473
622 public override bool Kinematic 474 default:
623 { 475 hollowVolume = 0;
624 get { return false; } 476 break;
625 set { } 477 }
626 } 478 volume *= (1.0f - hollowVolume);
479 }
480 }
627 481
628 public override Quaternion Orientation 482 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
629 { 483 {
630 get 484 //a tube
631 {
632 if (givefakeori > 0)
633 return fakeori;
634 else
635 485
636 return _orientation; 486 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
637 } 487 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
638 set 488 volume -= volume*tmp*tmp;
639 { 489
640 if (QuaternionIsFinite(value)) 490 if (hollowAmount > 0.0)
641 { 491 {
642 fakeori = value; 492 hollowVolume *= hollowAmount;
643 givefakeori++; 493
644// Debug 494 switch (_pbs.HollowShape)
645 float qlen = value.Length(); 495 {
646 if (value.Length() > 1.01f || qlen <0.99) 496 case HollowShape.Square:
647 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {1}", Name, qlen); 497 case HollowShape.Same:
648// 498 break;
649 value.Normalize(); 499
500 case HollowShape.Circle:
501 hollowVolume *= 0.78539816339f;;
502 break;
503
504 case HollowShape.Triangle:
505 hollowVolume *= 0.5f * 0.5f;
506 break;
507 default:
508 hollowVolume = 0;
509 break;
510 }
511 volume *= (1.0f - hollowVolume);
512 }
513 }
650 514
651 AddChange(changes.Orientation, value); 515 break;
652 }
653 else
654 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
655 516
656 } 517 case ProfileShape.Circle:
657 }
658 518
659 public override Vector3 Acceleration 519 if (_pbs.PathCurve == (byte)Extrusion.Straight)
660 { 520 {
661 get { return _acceleration; } 521 volume *= 0.78539816339f; // elipse base
662 set { }
663 }
664 522
665 public override Vector3 RotationalVelocity 523 if (hollowAmount > 0.0)
666 { 524 {
667 get 525 switch (_pbs.HollowShape)
668 { 526 {
669 Vector3 pv = Vector3.Zero; 527 case HollowShape.Same:
670 if (_zeroFlag) 528 case HollowShape.Circle:
671 return pv; 529 break;
672 530
673 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 531 case HollowShape.Square:
674 return pv; 532 hollowVolume *= 0.5f * 2.5984480504799f;
533 break;
675 534
676 return m_rotationalVelocity; 535 case HollowShape.Triangle:
677 } 536 hollowVolume *= .5f * 1.27323954473516f;
678 set 537 break;
679 {
680 if (value.IsFinite())
681 {
682 AddChange(changes.AngVelocity, value);
683 }
684 else
685 {
686 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
687 }
688 }
689 }
690 538
691 public override float Buoyancy 539 default:
692 { 540 hollowVolume = 0;
693 get { return m_buoyancy; } 541 break;
694 set 542 }
695 { 543 volume *= (1.0f - hollowVolume);
696 AddChange(changes.Buoyancy,value); 544 }
697 } 545 }
698 }
699 546
700 public override bool FloatOnWater 547 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
701 { 548 {
702 set 549 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
703 { 550 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
704 AddChange(changes.CollidesWater, value); 551 volume *= (1.0f - tmp * tmp);
705 } 552
706 } 553 if (hollowAmount > 0.0)
554 {
707 555
708 public override Vector3 PIDTarget 556 // calculate the hollow volume by it's shape compared to the prim shape
709 { 557 hollowVolume *= hollowAmount;
710 set
711 {
712 if (value.IsFinite())
713 {
714 AddChange(changes.PIDTarget,value);
715 }
716 else
717 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
718 }
719 }
720 558
721 public override bool PIDActive 559 switch (_pbs.HollowShape)
722 { 560 {
723 set 561 case HollowShape.Same:
724 { 562 case HollowShape.Circle:
725 AddChange(changes.PIDActive,value); 563 break;
726 }
727 }
728 564
729 public override float PIDTau 565 case HollowShape.Square:
730 { 566 hollowVolume *= 0.5f * 2.5984480504799f;
731 set 567 break;
732 {
733 float tmp = 0;
734 if (value > 0)
735 {
736 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
737 if (value < mint)
738 tmp = mint;
739 else
740 tmp = value;
741 }
742 AddChange(changes.PIDTau,tmp);
743 }
744 }
745 568
746 public override float PIDHoverHeight 569 case HollowShape.Triangle:
747 { 570 hollowVolume *= .5f * 1.27323954473516f;
748 set 571 break;
749 {
750 AddChange(changes.PIDHoverHeight,value);
751 }
752 }
753 public override bool PIDHoverActive
754 {
755 set
756 {
757 AddChange(changes.PIDHoverActive, value);
758 }
759 }
760 572
761 public override PIDHoverType PIDHoverType 573 default:
762 { 574 hollowVolume = 0;
763 set 575 break;
764 { 576 }
765 AddChange(changes.PIDHoverType,value); 577 volume *= (1.0f - hollowVolume);
766 } 578 }
767 } 579 }
580 break;
768 581
769 public override float PIDHoverTau 582 case ProfileShape.HalfCircle:
770 { 583 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
771 set 584 {
772 { 585 volume *= 0.52359877559829887307710723054658f;
773 float tmp =0; 586 }
774 if (value > 0) 587 break;
775 {
776 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
777 if (value < mint)
778 tmp = mint;
779 else
780 tmp = value;
781 }
782 AddChange(changes.PIDHoverTau, tmp);
783 }
784 }
785 588
786 public override Quaternion APIDTarget { set { return; } } 589 case ProfileShape.EquilateralTriangle:
787 590
788 public override bool APIDActive { set { return; } } 591 if (_pbs.PathCurve == (byte)Extrusion.Straight)
592 {
593 volume *= 0.32475953f;
789 594
790 public override float APIDStrength { set { return; } } 595 if (hollowAmount > 0.0)
596 {
791 597
792 public override float APIDDamping { set { return; } } 598 // calculate the hollow volume by it's shape compared to the prim shape
599 switch (_pbs.HollowShape)
600 {
601 case HollowShape.Same:
602 case HollowShape.Triangle:
603 hollowVolume *= .25f;
604 break;
793 605
794 public override int VehicleType 606 case HollowShape.Square:
795 { 607 hollowVolume *= 0.499849f * 3.07920140172638f;
796 // we may need to put a fake on this 608 break;
797 get
798 {
799 if (m_vehicle == null)
800 return (int)Vehicle.TYPE_NONE;
801 else
802 return (int)m_vehicle.Type;
803 }
804 set
805 {
806 AddChange(changes.VehicleType, value);
807 }
808 }
809 609
810 public override void VehicleFloatParam(int param, float value) 610 case HollowShape.Circle:
811 { 611 // Hollow shape is a perfect cyllinder in respect to the cube's scale
812 strVehicleFloatParam fp = new strVehicleFloatParam(); 612 // Cyllinder hollow volume calculation
813 fp.param = param;
814 fp.value = value;
815 AddChange(changes.VehicleFloatParam, fp);
816 }
817 613
818 public override void VehicleVectorParam(int param, Vector3 value) 614 hollowVolume *= 0.1963495f * 3.07920140172638f;
819 { 615 break;
820 strVehicleVectorParam fp = new strVehicleVectorParam();
821 fp.param = param;
822 fp.value = value;
823 AddChange(changes.VehicleVectorParam, fp);
824 }
825 616
826 public override void VehicleRotationParam(int param, Quaternion value) 617 default:
827 { 618 hollowVolume = 0;
828 strVehicleQuatParam fp = new strVehicleQuatParam(); 619 break;
829 fp.param = param; 620 }
830 fp.value = value; 621 volume *= (1.0f - hollowVolume);
831 AddChange(changes.VehicleRotationParam, fp); 622 }
832 } 623 }
624 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
625 {
626 volume *= 0.32475953f;
627 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
628 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
629 volume *= (1.0f - tmp * tmp);
833 630
834 public override void VehicleFlags(int param, bool value) 631 if (hollowAmount > 0.0)
835 { 632 {
836 strVehicleBoolParam bp = new strVehicleBoolParam();
837 bp.param = param;
838 bp.value = value;
839 AddChange(changes.VehicleFlags, bp);
840 }
841 633
842 public override void SetVehicle(object vdata) 634 hollowVolume *= hollowAmount;
843 {
844 AddChange(changes.SetVehicle, vdata);
845 }
846 public void SetAcceleration(Vector3 accel)
847 {
848 _acceleration = accel;
849 }
850 635
851 public override void AddForce(Vector3 force, bool pushforce) 636 switch (_pbs.HollowShape)
852 { 637 {
853 if (force.IsFinite()) 638 case HollowShape.Same:
854 { 639 case HollowShape.Triangle:
855 if(pushforce) 640 hollowVolume *= .25f;
856 AddChange(changes.AddForce, force); 641 break;
857 else // a impulse
858 AddChange(changes.AddForce, force * m_invTimeStep);
859 }
860 else
861 {
862 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
863 }
864 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
865 }
866 642
867 public override void AddAngularForce(Vector3 force, bool pushforce) 643 case HollowShape.Square:
868 { 644 hollowVolume *= 0.499849f * 3.07920140172638f;
869 if (force.IsFinite()) 645 break;
646
647 case HollowShape.Circle:
648
649 hollowVolume *= 0.1963495f * 3.07920140172638f;
650 break;
651
652 default:
653 hollowVolume = 0;
654 break;
655 }
656 volume *= (1.0f - hollowVolume);
657 }
658 }
659 break;
660
661 default:
662 break;
663 }
664
665 float taperX1;
666 float taperY1;
667 float taperX;
668 float taperY;
669 float pathBegin;
670 float pathEnd;
671 float profileBegin;
672 float profileEnd;
673
674 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
870 { 675 {
871// if(pushforce) for now applyrotationimpulse seems more happy applied as a force 676 taperX1 = _pbs.PathScaleX * 0.01f;
872 AddChange(changes.AddAngForce, force); 677 if (taperX1 > 1.0f)
873// else // a impulse 678 taperX1 = 2.0f - taperX1;
874// AddChange(changes.AddAngForce, force * m_invTimeStep); 679 taperX = 1.0f - taperX1;
680
681 taperY1 = _pbs.PathScaleY * 0.01f;
682 if (taperY1 > 1.0f)
683 taperY1 = 2.0f - taperY1;
684 taperY = 1.0f - taperY1;
875 } 685 }
876 else 686 else
877 { 687 {
878 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); 688 taperX = _pbs.PathTaperX * 0.01f;
689 if (taperX < 0.0f)
690 taperX = -taperX;
691 taperX1 = 1.0f - taperX;
692
693 taperY = _pbs.PathTaperY * 0.01f;
694 if (taperY < 0.0f)
695 taperY = -taperY;
696 taperY1 = 1.0f - taperY;
879 } 697 }
880 }
881 698
882 public override void CrossingFailure() 699 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
883 {
884 if (m_outbounds)
885 {
886 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
887 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
888 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
889 700
890 m_lastposition = _position; 701 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
891 _velocity.X = 0; 702 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
892 _velocity.Y = 0; 703 volume *= (pathEnd - pathBegin);
893 _velocity.Z = 0;
894 704
895 m_lastVelocity = _velocity; 705// this is crude aproximation
896 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 706 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
897 m_vehicle.Stop(); 707 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
708 volume *= (profileEnd - profileBegin);
898 709
899 if(Body != IntPtr.Zero) 710 returnMass = m_density * volume;
900 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
901 if (prim_geom != IntPtr.Zero)
902 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
903 711
904 m_outbounds = false; 712 if (returnMass <= 0)
905 changeDisable(false); 713 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
906 base.RequestPhysicsterseUpdate(); 714// else if (returnMass > _parent_scene.maximumMassObject)
715// returnMass = _parent_scene.maximumMassObject;
716
717 // Recursively calculate mass
718 bool HasChildPrim = false;
719 lock (childrenPrim)
720 {
721 if (childrenPrim.Count > 0)
722 {
723 HasChildPrim = true;
724 }
907 } 725 }
908 }
909 726
910 public override void SetMomentum(Vector3 momentum) 727 if (HasChildPrim)
911 { 728 {
912 } 729 OdePrim[] childPrimArr = new OdePrim[0];
913 730
914 public override void SetMaterial(int pMaterial) 731 lock (childrenPrim)
915 { 732 childPrimArr = childrenPrim.ToArray();
916 m_material = pMaterial;
917 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
918 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
919 }
920 733
921 public void setPrimForRemoval() 734 for (int i = 0; i < childPrimArr.Length; i++)
922 { 735 {
923 AddChange(changes.Remove, null); 736 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
924 } 737 returnMass += childPrimArr[i].CalculateMass();
738 // failsafe, this shouldn't happen but with OpenSim, you never know :)
739 if (i > 256)
740 break;
741 }
742 }
925 743
926 public override void link(PhysicsActor obj) 744 if (returnMass > _parent_scene.maximumMassObject)
927 { 745 returnMass = _parent_scene.maximumMassObject;
928 AddChange(changes.Link, obj);
929 }
930 746
931 public override void delink() 747 return returnMass;
932 {
933 AddChange(changes.DeLink, null);
934 } 748 }
935 749
936 public override void LockAngularMotion(Vector3 axis) 750 #endregion
751
752 private void setMass()
937 { 753 {
938 // reverse the zero/non zero values for ODE. 754 if (Body != (IntPtr) 0)
939 if (axis.IsFinite())
940 {
941 axis.X = (axis.X > 0) ? 1f : 0f;
942 axis.Y = (axis.Y > 0) ? 1f : 0f;
943 axis.Z = (axis.Z > 0) ? 1f : 0f;
944// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
945 AddChange(changes.AngLock, axis);
946 }
947 else
948 { 755 {
949 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); 756 float newmass = CalculateMass();
950 }
951 }
952 757
953 public override void SubscribeEvents(int ms) 758 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
954 {
955 m_eventsubscription = ms;
956 m_cureventsubscription = 0;
957 if (CollisionEventsThisFrame == null)
958 CollisionEventsThisFrame = new CollisionEventUpdate();
959 SentEmptyCollisionsEvent = false;
960 }
961 759
962 public override void UnSubscribeEvents() 760 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
963 { 761 d.BodySetMass(Body, ref pMass);
964 if (CollisionEventsThisFrame != null)
965 {
966 CollisionEventsThisFrame.Clear();
967 CollisionEventsThisFrame = null;
968 } 762 }
969 m_eventsubscription = 0;
970 _parent_scene.RemoveCollisionEventReporting(this);
971 } 763 }
972 764
973 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 765 /// <summary>
974 { 766 /// Stop a prim from being subject to physics.
975 if (CollisionEventsThisFrame == null) 767 /// </summary>
976 CollisionEventsThisFrame = new CollisionEventUpdate(); 768 internal void disableBody()
977// if(CollisionEventsThisFrame.Count < 32)
978 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
979 }
980
981 public void SendCollisions()
982 { 769 {
983 if (CollisionEventsThisFrame == null) 770 //this kills the body so things like 'mesh' can re-create it.
984 return; 771 lock (this)
985
986 if (m_cureventsubscription < m_eventsubscription)
987 return;
988
989 m_cureventsubscription = 0;
990
991 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
992
993 if (!SentEmptyCollisionsEvent || ncolisions > 0)
994 { 772 {
995 base.SendCollisionUpdate(CollisionEventsThisFrame); 773 if (!childPrim)
996
997 if (ncolisions == 0)
998 { 774 {
999 SentEmptyCollisionsEvent = true; 775 if (Body != IntPtr.Zero)
1000 _parent_scene.RemoveCollisionEventReporting(this); 776 {
777 _parent_scene.DeactivatePrim(this);
778 m_collisionCategories &= ~CollisionCategories.Body;
779 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
780
781 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
782 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
783
784 d.BodyDestroy(Body);
785 lock (childrenPrim)
786 {
787 if (childrenPrim.Count > 0)
788 {
789 foreach (OdePrim prm in childrenPrim)
790 {
791 _parent_scene.DeactivatePrim(prm);
792 prm.Body = IntPtr.Zero;
793 }
794 }
795 }
796 Body = IntPtr.Zero;
797 }
1001 } 798 }
1002 else 799 else
1003 { 800 {
1004 SentEmptyCollisionsEvent = false; 801 _parent_scene.DeactivatePrim(this);
1005 CollisionEventsThisFrame.Clear(); 802
803 m_collisionCategories &= ~CollisionCategories.Body;
804 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
805
806 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
807 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
808
809 Body = IntPtr.Zero;
1006 } 810 }
1007 } 811 }
1008 }
1009 812
1010 internal void AddCollisionFrameTime(int t) 813 m_disabled = true;
1011 { 814 m_collisionscore = 0;
1012 if (m_cureventsubscription < 50000)
1013 m_cureventsubscription += t;
1014 } 815 }
1015 816
1016 public override bool SubscribedEvents() 817 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1017 {
1018 if (m_eventsubscription > 0)
1019 return true;
1020 return false;
1021 }
1022 818
1023 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 819 private void setMesh(OdeScene parent_scene, IMesh mesh)
1024 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1025 { 820 {
1026 Name = primName; 821// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
1027 LocalID = plocalID;
1028
1029 m_vehicle = null;
1030
1031 if (!pos.IsFinite())
1032 {
1033 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1034 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1035 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1036 }
1037 _position = pos;
1038 givefakepos = 0;
1039
1040 m_timeStep = parent_scene.ODE_STEPSIZE;
1041 m_invTimeStep = 1f / m_timeStep;
1042 822
1043 m_density = parent_scene.geomDefaultDensity; 823 // This sleeper is there to moderate how long it takes between
1044 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 824 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
1045 825
1046 prim_geom = IntPtr.Zero; 826 //Thread.Sleep(10);
1047 collide_geom = IntPtr.Zero;
1048 Body = IntPtr.Zero;
1049 827
1050 if (!size.IsFinite()) 828 //Kill Body so that mesh can re-make the geom
829 if (IsPhysical && Body != IntPtr.Zero)
1051 { 830 {
1052 size = new Vector3(0.5f, 0.5f, 0.5f); 831 if (childPrim)
1053 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); 832 {
833 if (_parent != null)
834 {
835 OdePrim parent = (OdePrim)_parent;
836 parent.ChildDelink(this);
837 }
838 }
839 else
840 {
841 disableBody();
842 }
1054 } 843 }
1055 844
1056 if (size.X <= 0) size.X = 0.01f; 845 IntPtr vertices, indices;
1057 if (size.Y <= 0) size.Y = 0.01f; 846 int vertexCount, indexCount;
1058 if (size.Z <= 0) size.Z = 0.01f; 847 int vertexStride, triStride;
1059 848 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1060 _size = size; 849 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
850 m_expectedCollisionContacts = indexCount;
851 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1061 852
1062 if (!QuaternionIsFinite(rotation)) 853 // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
854 // the same time.
855 lock (m_MeshToTriMeshMap)
1063 { 856 {
1064 rotation = Quaternion.Identity; 857 if (m_MeshToTriMeshMap.ContainsKey(mesh))
1065 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); 858 {
859 _triMeshData = m_MeshToTriMeshMap[mesh];
860 }
861 else
862 {
863 _triMeshData = d.GeomTriMeshDataCreate();
864
865 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
866 d.GeomTriMeshDataPreprocess(_triMeshData);
867 m_MeshToTriMeshMap[mesh] = _triMeshData;
868 }
1066 } 869 }
1067 870
1068 _orientation = rotation; 871// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1069 givefakeori = 0; 872 try
1070
1071 _pbs = pbs;
1072
1073 _parent_scene = parent_scene;
1074 m_targetSpace = IntPtr.Zero;
1075
1076 if (pos.Z < 0)
1077 { 873 {
1078 m_isphysical = false; 874 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1079 } 875 }
1080 else 876 catch (AccessViolationException)
1081 { 877 {
1082 m_isphysical = pisPhysical; 878 m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
879 return;
1083 } 880 }
1084 m_fakeisphysical = m_isphysical;
1085 881
1086 m_isVolumeDetect = false; 882 // if (IsPhysical && Body == (IntPtr) 0)
1087 m_fakeisVolumeDetect = false; 883 // {
884 // Recreate the body
885 // m_interpenetrationcount = 0;
886 // m_collisionscore = 0;
1088 887
1089 m_force = Vector3.Zero; 888 // enableBody();
889 // }
890 }
1090 891
1091 m_iscolliding = false; 892 internal void ProcessTaints()
1092 m_colliderfilter = 0; 893 {
1093 m_NoColide = false; 894#if SPAM
895Console.WriteLine("ZProcessTaints for " + Name);
896#endif
1094 897
1095 _triMeshData = IntPtr.Zero; 898 // This must be processed as the very first taint so that later operations have a prim_geom to work with
899 // if this is a new prim.
900 if (m_taintadd)
901 changeadd();
1096 902
1097 m_shapetype = _shapeType; 903 if (!_position.ApproxEquals(m_taintposition, 0f))
904 changemove();
1098 905
1099 m_lastdoneSelected = false; 906 if (m_taintrot != _orientation)
1100 m_isSelected = false; 907 {
1101 m_delaySelect = false; 908 if (childPrim && IsPhysical) // For physical child prim...
909 {
910 rotate();
911 // KF: ODE will also rotate the parent prim!
912 // so rotate the root back to where it was
913 OdePrim parent = (OdePrim)_parent;
914 parent.rotate();
915 }
916 else
917 {
918 //Just rotate the prim
919 rotate();
920 }
921 }
922
923 if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
924 changePhysicsStatus();
1102 925
1103 m_isphantom = pisPhantom; 926 if (!_size.ApproxEquals(m_taintsize, 0f))
1104 m_fakeisphantom = pisPhantom; 927 changesize();
1105 928
1106 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; 929 if (m_taintshape)
1107 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; 930 changeshape();
1108 931
1109 m_building = true; // control must set this to false when done 932 if (m_taintforce)
933 changeAddForce();
1110 934
1111 _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); 935 if (m_taintaddangularforce)
1112 } 936 changeAddAngularForce();
1113 937
1114 private void resetCollisionAccounting() 938 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1115 { 939 changeSetTorque();
1116 m_collisionscore = 0;
1117 }
1118 940
1119 private void UpdateCollisionCatFlags() 941 if (m_taintdisable)
1120 { 942 changedisable();
1121 if(m_isphysical && m_disabled)
1122 {
1123 m_collisionCategories = 0;
1124 m_collisionFlags = 0;
1125 }
1126 943
1127 else if (m_isSelected) 944 if (m_taintselected != m_isSelected)
1128 { 945 changeSelectedStatus();
1129 m_collisionCategories = CollisionCategories.Selected;
1130 m_collisionFlags = 0;
1131 }
1132 946
1133 else if (m_isVolumeDetect) 947 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1134 { 948 changevelocity();
1135 m_collisionCategories = CollisionCategories.VolumeDtc; 949
1136 if (m_isphysical) 950 if (m_taintparent != _parent)
1137 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; 951 changelink();
1138 else 952
1139 m_collisionFlags = 0; 953 if (m_taintCollidesWater != m_collidesWater)
1140 } 954 changefloatonwater();
1141 else if (m_isphantom) 955
1142 { 956 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
1143 m_collisionCategories = CollisionCategories.Phantom; 957 changeAngularLock();
1144 if (m_isphysical)
1145 m_collisionFlags = CollisionCategories.Land;
1146 else
1147 m_collisionFlags = 0;
1148 }
1149 else
1150 {
1151 m_collisionCategories = CollisionCategories.Geom;
1152 if (m_isphysical)
1153 m_collisionFlags = m_default_collisionFlagsPhysical;
1154 else
1155 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1156 }
1157 } 958 }
1158 959
1159 private void ApplyCollisionCatFlags() 960 /// <summary>
961 /// Change prim in response to an angular lock taint.
962 /// </summary>
963 private void changeAngularLock()
1160 { 964 {
1161 if (prim_geom != IntPtr.Zero) 965 // do we have a Physical object?
966 if (Body != IntPtr.Zero)
1162 { 967 {
1163 if (!childPrim && childrenPrim.Count > 0) 968 //Check that we have a Parent
969 //If we have a parent then we're not authorative here
970 if (_parent == null)
1164 { 971 {
1165 foreach (OdePrim prm in childrenPrim) 972 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
1166 { 973 {
1167 if (m_isphysical && m_disabled) 974 //d.BodySetFiniteRotationMode(Body, 0);
1168 { 975 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
1169 prm.m_collisionCategories = 0; 976 createAMotor(m_taintAngularLock);
1170 prm.m_collisionFlags = 0; 977 }
1171 } 978 else
1172 else 979 {
1173 { 980 if (Amotor != IntPtr.Zero)
1174 // preserve some
1175 if (prm.m_isSelected)
1176 {
1177 prm.m_collisionCategories = CollisionCategories.Selected;
1178 prm.m_collisionFlags = 0;
1179 }
1180 else if (prm.m_isVolumeDetect)
1181 {
1182 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1183 if (m_isphysical)
1184 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1185 else
1186 prm.m_collisionFlags = 0;
1187 }
1188 else if (prm.m_isphantom)
1189 {
1190 prm.m_collisionCategories = CollisionCategories.Phantom;
1191 if (m_isphysical)
1192 prm.m_collisionFlags = CollisionCategories.Land;
1193 else
1194 prm.m_collisionFlags = 0;
1195 }
1196 else
1197 {
1198 prm.m_collisionCategories = m_collisionCategories;
1199 prm.m_collisionFlags = m_collisionFlags;
1200 }
1201 }
1202
1203 if (prm.prim_geom != IntPtr.Zero)
1204 { 981 {
1205 if (prm.m_NoColide) 982 d.JointDestroy(Amotor);
1206 { 983 Amotor = IntPtr.Zero;
1207 d.GeomSetCategoryBits(prm.prim_geom, 0);
1208 if (m_isphysical)
1209 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1210 else
1211 d.GeomSetCollideBits(prm.prim_geom, 0);
1212 }
1213 else
1214 {
1215 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1216 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1217 }
1218 } 984 }
1219 } 985 }
1220 } 986 }
987 }
1221 988
1222 if (m_NoColide) 989 // Store this for later in case we get turned into a separate body
990 m_angularlock = m_taintAngularLock;
991 }
992
993 /// <summary>
994 /// Change prim in response to a link taint.
995 /// </summary>
996 private void changelink()
997 {
998 // If the newly set parent is not null
999 // create link
1000 if (_parent == null && m_taintparent != null)
1001 {
1002 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1223 { 1003 {
1224 d.GeomSetCategoryBits(prim_geom, 0); 1004 OdePrim obj = (OdePrim)m_taintparent;
1225 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 1005 //obj.disableBody();
1226 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1006//Console.WriteLine("changelink calls ParentPrim");
1007 obj.AddChildPrim(this);
1008
1009 /*
1010 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1227 { 1011 {
1228 d.GeomSetCategoryBits(collide_geom, 0); 1012 _linkJointGroup = d.JointGroupCreate(0);
1229 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); 1013 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1014 d.JointAttach(m_linkJoint, obj.Body, Body);
1015 d.JointSetFixed(m_linkJoint);
1230 } 1016 }
1017 */
1231 } 1018 }
1232 else 1019 }
1020 // If the newly set parent is null
1021 // destroy link
1022 else if (_parent != null && m_taintparent == null)
1023 {
1024//Console.WriteLine(" changelink B");
1025
1026 if (_parent is OdePrim)
1233 { 1027 {
1234 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1028 OdePrim obj = (OdePrim)_parent;
1235 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 1029 obj.ChildDelink(this);
1236 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1030 childPrim = false;
1237 { 1031 //_parent = null;
1238 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1239 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1240 }
1241 } 1032 }
1033
1034 /*
1035 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1036 d.JointGroupDestroy(_linkJointGroup);
1037
1038 _linkJointGroup = (IntPtr)0;
1039 m_linkJoint = (IntPtr)0;
1040 */
1242 } 1041 }
1042
1043 _parent = m_taintparent;
1044 m_taintPhysics = IsPhysical;
1243 } 1045 }
1244 1046
1245 private void createAMotor(Vector3 axis) 1047 /// <summary>
1048 /// Add a child prim to this parent prim.
1049 /// </summary>
1050 /// <param name="prim">Child prim</param>
1051 private void AddChildPrim(OdePrim prim)
1246 { 1052 {
1247 if (Body == IntPtr.Zero) 1053 if (LocalID == prim.LocalID)
1248 return; 1054 return;
1249 1055
1250 if (Amotor != IntPtr.Zero) 1056 if (Body == IntPtr.Zero)
1251 { 1057 {
1252 d.JointDestroy(Amotor); 1058 Body = d.BodyCreate(_parent_scene.world);
1253 Amotor = IntPtr.Zero; 1059 setMass();
1254 } 1060 }
1255 1061
1256 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); 1062 lock (childrenPrim)
1257 1063 {
1258 if (axisnum <= 0) 1064 if (childrenPrim.Contains(prim))
1259 return; 1065 return;
1260
1261 // stop it
1262 d.BodySetTorque(Body, 0, 0, 0);
1263 d.BodySetAngularVel(Body, 0, 0, 0);
1264
1265 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1266 d.JointAttach(Amotor, Body, IntPtr.Zero);
1267
1268 d.JointSetAMotorMode(Amotor, 0);
1269 1066
1270 d.JointSetAMotorNumAxes(Amotor, axisnum); 1067// m_log.DebugFormat(
1068// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
1271 1069
1272 // get current orientation to lock 1070 childrenPrim.Add(prim);
1273 1071
1274 d.Quaternion dcur = d.BodyGetQuaternion(Body); 1072 foreach (OdePrim prm in childrenPrim)
1275 Quaternion curr; // crap convertion between identical things 1073 {
1276 curr.X = dcur.X; 1074 d.Mass m2;
1277 curr.Y = dcur.Y; 1075 d.MassSetZero(out m2);
1278 curr.Z = dcur.Z; 1076 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1279 curr.W = dcur.W;
1280 Vector3 ax;
1281 1077
1282 int i = 0; 1078 d.Quaternion quat = new d.Quaternion();
1283 int j = 0; 1079 quat.W = prm._orientation.W;
1284 if (axis.X == 0) 1080 quat.X = prm._orientation.X;
1285 { 1081 quat.Y = prm._orientation.Y;
1286 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X 1082 quat.Z = prm._orientation.Z;
1287 // ODE should do this with axis relative to body 1 but seems to fail
1288 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1289 d.JointSetAMotorAngle(Amotor, 0, 0);
1290 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1291 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1292 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1294 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1295 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1296 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1297 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1298 i++;
1299 j = 256; // move to next axis set
1300 }
1301 1083
1302 if (axis.Y == 0) 1084 d.Matrix3 mat = new d.Matrix3();
1303 { 1085 d.RfromQ(out mat, ref quat);
1304 ax = (new Vector3(0, 1, 0)) * curr; 1086 d.MassRotate(ref m2, ref mat);
1305 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1087 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1306 d.JointSetAMotorAngle(Amotor, i, 0); 1088 d.MassAdd(ref pMass, ref m2);
1307 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); 1089 }
1308 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1309 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1311 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1312 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1313 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1314 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1315 i++;
1316 j += 256;
1317 }
1318 1090
1319 if (axis.Z == 0) 1091 foreach (OdePrim prm in childrenPrim)
1320 { 1092 {
1321 ax = (new Vector3(0, 0, 1)) * curr; 1093 prm.m_collisionCategories |= CollisionCategories.Body;
1322 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1094 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1323 d.JointSetAMotorAngle(Amotor, i, 0);
1324 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1325 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1326 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1328 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1329 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1330 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1331 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1332 }
1333 }
1334 1095
1096//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
1097 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1098 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1335 1099
1336 private void SetGeom(IntPtr geom) 1100 d.Quaternion quat = new d.Quaternion();
1337 { 1101 quat.W = prm._orientation.W;
1338 prim_geom = geom; 1102 quat.X = prm._orientation.X;
1339 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); 1103 quat.Y = prm._orientation.Y;
1340 if (prim_geom != IntPtr.Zero) 1104 quat.Z = prm._orientation.Z;
1341 {
1342 1105
1343 if (m_NoColide) 1106 d.Matrix3 mat = new d.Matrix3();
1344 { 1107 d.RfromQ(out mat, ref quat);
1345 d.GeomSetCategoryBits(prim_geom, 0); 1108 if (Body != IntPtr.Zero)
1346 if (m_isphysical)
1347 { 1109 {
1348 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 1110 d.GeomSetBody(prm.prim_geom, Body);
1111 prm.childPrim = true;
1112 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1113 //d.GeomSetOffsetPosition(prim.prim_geom,
1114 // (Position.X - prm.Position.X) - pMass.c.X,
1115 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1116 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1117 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1118 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1119 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1120 d.BodySetMass(Body, ref pMass);
1349 } 1121 }
1350 else 1122 else
1351 { 1123 {
1352 d.GeomSetCollideBits(prim_geom, 0); 1124 m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
1353 d.GeomDisable(prim_geom);
1354 } 1125 }
1355 } 1126
1356 else 1127 prm.m_interpenetrationcount = 0;
1128 prm.m_collisionscore = 0;
1129 prm.m_disabled = false;
1130
1131 // The body doesn't already have a finite rotation mode set here
1132 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1133 {
1134 prm.createAMotor(m_angularlock);
1135 }
1136 prm.Body = Body;
1137 _parent_scene.ActivatePrim(prm);
1138 }
1139
1140 m_collisionCategories |= CollisionCategories.Body;
1141 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1142
1143//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
1144 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1145//Console.WriteLine(" Post GeomSetCategoryBits 2");
1146 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1147
1148 d.Quaternion quat2 = new d.Quaternion();
1149 quat2.W = _orientation.W;
1150 quat2.X = _orientation.X;
1151 quat2.Y = _orientation.Y;
1152 quat2.Z = _orientation.Z;
1153
1154 d.Matrix3 mat2 = new d.Matrix3();
1155 d.RfromQ(out mat2, ref quat2);
1156 d.GeomSetBody(prim_geom, Body);
1157 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1158 //d.GeomSetOffsetPosition(prim.prim_geom,
1159 // (Position.X - prm.Position.X) - pMass.c.X,
1160 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1161 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1162 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1163 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1164 d.BodySetMass(Body, ref pMass);
1165
1166 d.BodySetAutoDisableFlag(Body, true);
1167 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1168
1169 m_interpenetrationcount = 0;
1170 m_collisionscore = 0;
1171 m_disabled = false;
1172
1173 // The body doesn't already have a finite rotation mode set here
1174 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1357 { 1175 {
1358 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1176 createAMotor(m_angularlock);
1359 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1360 } 1177 }
1361 1178
1362 UpdatePrimBodyData(); 1179 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1363 _parent_scene.actor_name_map[prim_geom] = this;
1364 1180
1181 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1182 m_vehicle.Enable(Body, _parent_scene);
1365 1183
1366// debug 1184 _parent_scene.ActivatePrim(this);
1367 d.AABB aabb; 1185 }
1368 d.GeomGetAABB(prim_geom, out aabb); 1186 }
1369 float x = aabb.MaxX - aabb.MinX;
1370 float y = aabb.MaxY - aabb.MinY;
1371 float z = aabb.MaxZ - aabb.MinZ;
1372 if( x > 60.0f || y > 60.0f || z > 60.0f)
1373 m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1374 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1375 else if (x < 0.001f || y < 0.001f || z < 0.001f)
1376 m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1377 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1378 1187
1379// 1188 private void ChildSetGeom(OdePrim odePrim)
1189 {
1190// m_log.DebugFormat(
1191// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1380 1192
1193 //if (IsPhysical && Body != IntPtr.Zero)
1194 lock (childrenPrim)
1195 {
1196 foreach (OdePrim prm in childrenPrim)
1197 {
1198 //prm.childPrim = true;
1199 prm.disableBody();
1200 //prm.m_taintparent = null;
1201 //prm._parent = null;
1202 //prm.m_taintPhysics = false;
1203 //prm.m_disabled = true;
1204 //prm.childPrim = false;
1205 }
1206 }
1381 1207
1208 disableBody();
1382 1209
1210 // Spurious - Body == IntPtr.Zero after disableBody()
1211// if (Body != IntPtr.Zero)
1212// {
1213// _parent_scene.DeactivatePrim(this);
1214// }
1383 1215
1216 lock (childrenPrim)
1217 {
1218 foreach (OdePrim prm in childrenPrim)
1219 {
1220//Console.WriteLine("ChildSetGeom calls ParentPrim");
1221 AddChildPrim(prm);
1222 }
1384 } 1223 }
1385 else
1386 m_log.Warn("Setting bad Geom");
1387 } 1224 }
1388 1225
1389 private bool GetMeshGeom() 1226 private void ChildDelink(OdePrim odePrim)
1390 { 1227 {
1391 IntPtr vertices, indices; 1228// m_log.DebugFormat(
1392 int vertexCount, indexCount; 1229// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1393 int vertexStride, triStride;
1394
1395 IMesh mesh = m_mesh;
1396 1230
1397 if (mesh == null) 1231 // Okay, we have a delinked child.. need to rebuild the body.
1398 return false; 1232 lock (childrenPrim)
1233 {
1234 foreach (OdePrim prm in childrenPrim)
1235 {
1236 prm.childPrim = true;
1237 prm.disableBody();
1238 //prm.m_taintparent = null;
1239 //prm._parent = null;
1240 //prm.m_taintPhysics = false;
1241 //prm.m_disabled = true;
1242 //prm.childPrim = false;
1243 }
1244 }
1399 1245
1400 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); 1246 disableBody();
1401 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
1402 1247
1403 if (vertexCount == 0 || indexCount == 0) 1248 lock (childrenPrim)
1404 { 1249 {
1405 m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", 1250 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1406 Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); 1251 childrenPrim.Remove(odePrim);
1407 1252 }
1408 m_hasOBB = false;
1409 m_OBBOffset = Vector3.Zero;
1410 m_OBB = _size * 0.5f;
1411 1253
1412 m_physCost = 0.1f; 1254 // Spurious - Body == IntPtr.Zero after disableBody()
1413 m_streamCost = 1.0f; 1255// if (Body != IntPtr.Zero)
1256// {
1257// _parent_scene.DeactivatePrim(this);
1258// }
1414 1259
1415 _parent_scene.mesher.ReleaseMesh(mesh); 1260 lock (childrenPrim)
1416 m_meshState = MeshState.MeshFailed; 1261 {
1417 m_mesh = null; 1262 foreach (OdePrim prm in childrenPrim)
1418 return false; 1263 {
1264//Console.WriteLine("ChildDelink calls ParentPrim");
1265 AddChildPrim(prm);
1266 }
1419 } 1267 }
1268 }
1420 1269
1421 IntPtr geo = IntPtr.Zero; 1270 /// <summary>
1422 1271 /// Change prim in response to a selection taint.
1423 try 1272 /// </summary>
1273 private void changeSelectedStatus()
1274 {
1275 if (m_taintselected)
1424 { 1276 {
1425 _triMeshData = d.GeomTriMeshDataCreate(); 1277 m_collisionCategories = CollisionCategories.Selected;
1278 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1279
1280 // We do the body disable soft twice because 'in theory' a collision could have happened
1281 // in between the disabling and the collision properties setting
1282 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1283 // through the ground.
1284
1285 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1286 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1287 // so that causes the selected part to wake up and continue moving.
1288
1289 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1290 // assembly will stop simulating during the selection, because of the lack of atomicity
1291 // of select operations (their processing could be interrupted by a thread switch, causing
1292 // simulation to continue before all of the selected object notifications trickle down to
1293 // the physics engine).
1294
1295 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1296 // selected and disabled. then, due to a thread switch, the selection processing is
1297 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1298 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1299 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1300 // up, start simulating again, which in turn wakes up the last 50.
1301
1302 if (IsPhysical)
1303 {
1304 disableBodySoft();
1305 }
1426 1306
1427 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); 1307 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1428 d.GeomTriMeshDataPreprocess(_triMeshData); 1308 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1429 1309
1430 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); 1310 if (IsPhysical)
1311 {
1312 disableBodySoft();
1313 }
1431 } 1314 }
1432 1315 else
1433 catch (Exception e)
1434 { 1316 {
1435 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); 1317 m_collisionCategories = CollisionCategories.Geom;
1436 if (_triMeshData != IntPtr.Zero) 1318
1319 if (IsPhysical)
1320 m_collisionCategories |= CollisionCategories.Body;
1321
1322 m_collisionFlags = m_default_collisionFlags;
1323
1324 if (m_collidesLand)
1325 m_collisionFlags |= CollisionCategories.Land;
1326 if (m_collidesWater)
1327 m_collisionFlags |= CollisionCategories.Water;
1328
1329 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1330 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1331
1332 if (IsPhysical)
1437 { 1333 {
1438 try 1334 if (Body != IntPtr.Zero)
1439 {
1440 d.GeomTriMeshDataDestroy(_triMeshData);
1441 }
1442 catch
1443 { 1335 {
1336 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1337 d.BodySetForce(Body, 0, 0, 0);
1338 enableBodySoft();
1444 } 1339 }
1445 } 1340 }
1446 _triMeshData = IntPtr.Zero;
1447
1448 m_hasOBB = false;
1449 m_OBBOffset = Vector3.Zero;
1450 m_OBB = _size * 0.5f;
1451 m_physCost = 0.1f;
1452 m_streamCost = 1.0f;
1453
1454 _parent_scene.mesher.ReleaseMesh(mesh);
1455 m_meshState = MeshState.MeshFailed;
1456 m_mesh = null;
1457 return false;
1458 } 1341 }
1459 1342
1460 m_physCost = 0.0013f * (float)indexCount; 1343 resetCollisionAccounting();
1461 // todo 1344 m_isSelected = m_taintselected;
1462 m_streamCost = 1.0f; 1345 }//end changeSelectedStatus
1463
1464 SetGeom(geo);
1465 1346
1466 return true; 1347 internal void ResetTaints()
1348 {
1349 m_taintposition = _position;
1350 m_taintrot = _orientation;
1351 m_taintPhysics = IsPhysical;
1352 m_taintselected = m_isSelected;
1353 m_taintsize = _size;
1354 m_taintshape = false;
1355 m_taintforce = false;
1356 m_taintdisable = false;
1357 m_taintVelocity = Vector3.Zero;
1467 } 1358 }
1468 1359
1469 private void CreateGeom() 1360 /// <summary>
1361 /// Create a geometry for the given mesh in the given target space.
1362 /// </summary>
1363 /// <param name="m_targetSpace"></param>
1364 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1365 private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
1470 { 1366 {
1471 bool hasMesh = false; 1367#if SPAM
1472 1368Console.WriteLine("CreateGeom:");
1473 m_NoColide = false; 1369#endif
1474 1370 if (mesh != null)
1475 if ((m_meshState & MeshState.FailMask) != 0)
1476 m_NoColide = true;
1477
1478 else if(m_mesh != null)
1479 { 1371 {
1480 if (GetMeshGeom()) 1372 setMesh(_parent_scene, mesh);
1481 hasMesh = true;
1482 else
1483 m_NoColide = true;
1484 } 1373 }
1485 1374 else
1486
1487 if (!hasMesh)
1488 { 1375 {
1489 IntPtr geo = IntPtr.Zero; 1376 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1490 1377 {
1491 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 1378 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1492 && _size.X == _size.Y && _size.Y == _size.Z)
1493 { // it's a sphere
1494 try
1495 { 1379 {
1496 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); 1380 if (((_size.X / 2f) > 0f))
1381 {
1382// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1383 try
1384 {
1385//Console.WriteLine(" CreateGeom 1");
1386 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1387 m_expectedCollisionContacts = 3;
1388 }
1389 catch (AccessViolationException)
1390 {
1391 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1392 return;
1393 }
1394 }
1395 else
1396 {
1397// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1398 try
1399 {
1400//Console.WriteLine(" CreateGeom 2");
1401 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1402 m_expectedCollisionContacts = 4;
1403 }
1404 catch (AccessViolationException)
1405 {
1406 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1407 return;
1408 }
1409 }
1497 } 1410 }
1498 catch (Exception e) 1411 else
1499 { 1412 {
1500 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); 1413// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1501 return; 1414 try
1415 {
1416//Console.WriteLine(" CreateGeom 3");
1417 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1418 m_expectedCollisionContacts = 4;
1419 }
1420 catch (AccessViolationException)
1421 {
1422 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1423 return;
1424 }
1502 } 1425 }
1503 } 1426 }
1504 else 1427 else
1505 {// do it as a box 1428 {
1429// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1506 try 1430 try
1507 { 1431 {
1508 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); 1432//Console.WriteLine(" CreateGeom 4");
1433 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1434 m_expectedCollisionContacts = 4;
1509 } 1435 }
1510 catch (Exception e) 1436 catch (AccessViolationException)
1511 { 1437 {
1512 m_log.Warn("[PHYSICS]: Create box failed: {0}", e); 1438 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1513 return; 1439 return;
1514 } 1440 }
1515 } 1441 }
1516 m_physCost = 0.1f;
1517 m_streamCost = 1.0f;
1518 SetGeom(geo);
1519 } 1442 }
1520 } 1443 }
1521 1444
1522 private void RemoveGeom() 1445 /// <summary>
1446 /// Remove the existing geom from this prim.
1447 /// </summary>
1448 /// <param name="m_targetSpace"></param>
1449 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1450 /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
1451 internal bool RemoveGeom()
1523 { 1452 {
1524 if (prim_geom != IntPtr.Zero) 1453 if (prim_geom != IntPtr.Zero)
1525 { 1454 {
1526 _parent_scene.actor_name_map.Remove(prim_geom);
1527
1528 try 1455 try
1529 { 1456 {
1457 _parent_scene.geom_name_map.Remove(prim_geom);
1458 _parent_scene.actor_name_map.Remove(prim_geom);
1530 d.GeomDestroy(prim_geom); 1459 d.GeomDestroy(prim_geom);
1531 if (_triMeshData != IntPtr.Zero) 1460 m_expectedCollisionContacts = 0;
1532 { 1461 prim_geom = IntPtr.Zero;
1533 d.GeomTriMeshDataDestroy(_triMeshData);
1534 _triMeshData = IntPtr.Zero;
1535 }
1536 } 1462 }
1537 catch (Exception e) 1463 catch (System.AccessViolationException)
1538 { 1464 {
1539 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); 1465 prim_geom = IntPtr.Zero;
1466 m_expectedCollisionContacts = 0;
1467 m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
1468
1469 return false;
1540 } 1470 }
1541 1471
1542 prim_geom = IntPtr.Zero; 1472 return true;
1543 collide_geom = IntPtr.Zero;
1544 m_targetSpace = IntPtr.Zero;
1545 } 1473 }
1546 else 1474 else
1547 { 1475 {
1548 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); 1476 m_log.WarnFormat(
1549 } 1477 "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
1550
1551 lock (m_meshlock)
1552 {
1553 if (m_mesh != null)
1554 {
1555 _parent_scene.mesher.ReleaseMesh(m_mesh);
1556 m_mesh = null;
1557 }
1558 }
1559
1560 Body = IntPtr.Zero;
1561 m_hasOBB = false;
1562 }
1563
1564 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1565 // should only be called for non physical prims unless they are becoming non physical
1566 private void SetInStaticSpace(OdePrim prim)
1567 {
1568 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1569 prim.m_targetSpace = targetSpace;
1570 collide_geom = IntPtr.Zero;
1571 }
1572
1573 public void enableBodySoft()
1574 {
1575 m_disabled = false;
1576 if (!childPrim && !m_isSelected)
1577 {
1578 if (m_isphysical && Body != IntPtr.Zero)
1579 {
1580 UpdateCollisionCatFlags();
1581 ApplyCollisionCatFlags();
1582
1583 d.BodyEnable(Body);
1584 }
1585 }
1586 resetCollisionAccounting();
1587 }
1588 1478
1589 private void disableBodySoft() 1479 return false;
1590 {
1591 m_disabled = true;
1592 if (!childPrim)
1593 {
1594 if (m_isphysical && Body != IntPtr.Zero)
1595 {
1596 if (m_isSelected)
1597 m_collisionFlags = CollisionCategories.Selected;
1598 else
1599 m_collisionCategories = 0;
1600 m_collisionFlags = 0;
1601 ApplyCollisionCatFlags();
1602 d.BodyDisable(Body);
1603 }
1604 } 1480 }
1605 } 1481 }
1606 1482 /// <summary>
1607 private void MakeBody() 1483 /// Add prim in response to an add taint.
1484 /// </summary>
1485 private void changeadd()
1608 { 1486 {
1609 if (!m_isphysical) // only physical get bodies 1487// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
1610 return; 1488
1489 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1490 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1611 1491
1612 if (childPrim) // child prims don't get bodies; 1492 if (targetspace == IntPtr.Zero)
1613 return; 1493 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1614 1494
1615 if (m_building) 1495 m_targetSpace = targetspace;
1616 return;
1617 1496
1618 if (prim_geom == IntPtr.Zero) 1497 IMesh mesh = null;
1619 {
1620 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1621 return;
1622 }
1623 1498
1624 if (Body != IntPtr.Zero) 1499 if (_parent_scene.needsMeshing(_pbs))
1625 { 1500 {
1626 DestroyBody(); 1501 // Don't need to re-enable body.. it's done in SetMesh
1627 m_log.Warn("[PHYSICS]: MakeBody called having a body"); 1502 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1503 // createmesh returns null when it's a shape that isn't a cube.
1504 // m_log.Debug(m_localID);
1505 if (mesh == null)
1506 CheckMeshAsset();
1628 } 1507 }
1629 1508
1630 if (d.GeomGetBody(prim_geom) != IntPtr.Zero) 1509#if SPAM
1631 { 1510Console.WriteLine("changeadd 1");
1632 d.GeomSetBody(prim_geom, IntPtr.Zero); 1511#endif
1633 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); 1512 CreateGeom(m_targetSpace, mesh);
1634 }
1635 1513
1636 d.Matrix3 mymat = new d.Matrix3(); 1514 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1637 d.Quaternion myrot = new d.Quaternion(); 1515 d.Quaternion myrot = new d.Quaternion();
1638 d.Mass objdmass = new d.Mass { };
1639
1640 Body = d.BodyCreate(_parent_scene.world);
1641
1642 objdmass = primdMass;
1643
1644 // rotate inertia
1645 myrot.X = _orientation.X; 1516 myrot.X = _orientation.X;
1646 myrot.Y = _orientation.Y; 1517 myrot.Y = _orientation.Y;
1647 myrot.Z = _orientation.Z; 1518 myrot.Z = _orientation.Z;
1648 myrot.W = _orientation.W; 1519 myrot.W = _orientation.W;
1520 d.GeomSetQuaternion(prim_geom, ref myrot);
1649 1521
1650 d.RfromQ(out mymat, ref myrot); 1522 if (IsPhysical && Body == IntPtr.Zero)
1651 d.MassRotate(ref objdmass, ref mymat); 1523 enableBody();
1652
1653 // set the body rotation
1654 d.BodySetRotation(Body, ref mymat);
1655 1524
1656 // recompute full object inertia if needed 1525 changeSelectedStatus();
1657 if (childrenPrim.Count > 0)
1658 {
1659 d.Matrix3 mat = new d.Matrix3();
1660 d.Quaternion quat = new d.Quaternion();
1661 d.Mass tmpdmass = new d.Mass { };
1662 Vector3 rcm;
1663 1526
1664 rcm.X = _position.X; 1527 m_taintadd = false;
1665 rcm.Y = _position.Y; 1528 }
1666 rcm.Z = _position.Z;
1667 1529
1668 lock (childrenPrim) 1530 /// <summary>
1531 /// Move prim in response to a move taint.
1532 /// </summary>
1533 private void changemove()
1534 {
1535 if (IsPhysical)
1536 {
1537 if (!m_disabled && !m_taintremove && !childPrim)
1669 { 1538 {
1670 foreach (OdePrim prm in childrenPrim) 1539 if (Body == IntPtr.Zero)
1540 enableBody();
1541
1542 //Prim auto disable after 20 frames,
1543 //if you move it, re-enable the prim manually.
1544 if (_parent != null)
1671 { 1545 {
1672 if (prm.prim_geom == IntPtr.Zero) 1546 if (m_linkJoint != IntPtr.Zero)
1673 { 1547 {
1674 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); 1548 d.JointDestroy(m_linkJoint);
1675 continue; 1549 m_linkJoint = IntPtr.Zero;
1676 } 1550 }
1551 }
1552
1553 if (Body != IntPtr.Zero)
1554 {
1555 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1677 1556
1678 tmpdmass = prm.primdMass; 1557 if (_parent != null)
1679
1680 // apply prim current rotation to inertia
1681 quat.X = prm._orientation.X;
1682 quat.Y = prm._orientation.Y;
1683 quat.Z = prm._orientation.Z;
1684 quat.W = prm._orientation.W;
1685 d.RfromQ(out mat, ref quat);
1686 d.MassRotate(ref tmpdmass, ref mat);
1687
1688 Vector3 ppos = prm._position;
1689 ppos.X -= rcm.X;
1690 ppos.Y -= rcm.Y;
1691 ppos.Z -= rcm.Z;
1692 // refer inertia to root prim center of mass position
1693 d.MassTranslate(ref tmpdmass,
1694 ppos.X,
1695 ppos.Y,
1696 ppos.Z);
1697
1698 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1699 // fix prim colision cats
1700
1701 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1702 { 1558 {
1703 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 1559 OdePrim odParent = (OdePrim)_parent;
1704 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); 1560 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1561 {
1562// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1563Console.WriteLine(" JointCreateFixed");
1564 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1565 d.JointAttach(m_linkJoint, Body, odParent.Body);
1566 d.JointSetFixed(m_linkJoint);
1567 }
1705 } 1568 }
1706 1569 d.BodyEnable(Body);
1707 d.GeomClearOffset(prm.prim_geom); 1570 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1708 d.GeomSetBody(prm.prim_geom, Body); 1571 {
1709 prm.Body = Body; 1572 m_vehicle.Enable(Body, _parent_scene);
1710 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation 1573 }
1574 }
1575 else
1576 {
1577 m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
1711 } 1578 }
1712 } 1579 }
1580 //else
1581 // {
1582 //m_log.Debug("[BUG]: race!");
1583 //}
1713 } 1584 }
1714 1585
1715 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset 1586 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1716 // associate root geom with body 1587 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1717 d.GeomSetBody(prim_geom, Body); 1588// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1718
1719 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1720 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1721
1722 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1723 myrot.X = -myrot.X;
1724 myrot.Y = -myrot.Y;
1725 myrot.Z = -myrot.Z;
1726 1589
1727 d.RfromQ(out mymat, ref myrot); 1590 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1728 d.MassRotate(ref objdmass, ref mymat); 1591 m_targetSpace = tempspace;
1729 1592
1730 d.BodySetMass(Body, ref objdmass); 1593// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1731 _mass = objdmass.mass;
1732 1594
1733 // disconnect from world gravity so we can apply buoyancy 1595 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1734 d.BodySetGravityMode(Body, false);
1735
1736 d.BodySetAutoDisableFlag(Body, true);
1737 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1738 d.BodySetDamping(Body, .005f, .005f);
1739
1740 if (m_targetSpace != IntPtr.Zero)
1741 {
1742 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1743 if (d.SpaceQuery(m_targetSpace, prim_geom))
1744 d.SpaceRemove(m_targetSpace, prim_geom);
1745 }
1746
1747 if (childrenPrim.Count == 0)
1748 {
1749 collide_geom = prim_geom;
1750 m_targetSpace = _parent_scene.ActiveSpace;
1751 }
1752 else
1753 {
1754 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1755 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1756 d.SpaceSetSublevel(m_targetSpace, 3);
1757 d.SpaceSetCleanup(m_targetSpace, false);
1758
1759 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1760 CollisionCategories.Geom |
1761 CollisionCategories.Phantom |
1762 CollisionCategories.VolumeDtc
1763 ));
1764 d.GeomSetCollideBits(m_targetSpace, 0);
1765 collide_geom = m_targetSpace;
1766 }
1767 1596
1597// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1768 d.SpaceAdd(m_targetSpace, prim_geom); 1598 d.SpaceAdd(m_targetSpace, prim_geom);
1769 1599
1770 if (m_delaySelect) 1600 changeSelectedStatus();
1771 {
1772 m_isSelected = true;
1773 m_delaySelect = false;
1774 }
1775
1776 m_collisionscore = 0;
1777 1601
1778 UpdateCollisionCatFlags(); 1602 resetCollisionAccounting();
1779 ApplyCollisionCatFlags(); 1603 m_taintposition = _position;
1604 }
1780 1605
1781 _parent_scene.addActivePrim(this); 1606 internal void Move(float timestep)
1607 {
1608 float fx = 0;
1609 float fy = 0;
1610 float fz = 0;
1782 1611
1783 lock (childrenPrim) 1612 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1784 { 1613 {
1785 foreach (OdePrim prm in childrenPrim) 1614 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1786 { 1615 {
1787 if (prm.prim_geom == IntPtr.Zero) 1616 // 'VEHICLES' are dealt with in ODEDynamics.cs
1788 continue; 1617 m_vehicle.Step(timestep, _parent_scene);
1618 }
1619 else
1620 {
1621//Console.WriteLine("Move " + Name);
1622 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1623 // NON-'VEHICLES' are dealt with here
1624// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1625// {
1626// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1627// /*
1628// if (m_angularlock.X == 1)
1629// avel2.X = 0;
1630// if (m_angularlock.Y == 1)
1631// avel2.Y = 0;
1632// if (m_angularlock.Z == 1)
1633// avel2.Z = 0;
1634// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1635// */
1636// }
1637 //float PID_P = 900.0f;
1638
1639 float m_mass = CalculateMass();
1640
1641// fz = 0f;
1642 //m_log.Info(m_collisionFlags.ToString());
1789 1643
1790 Vector3 ppos = prm._position; 1644
1791 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position 1645 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1646 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1647 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1648 // gravityz multiplier = 1 - m_buoyancy
1649 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1792 1650
1793 if (prm.m_targetSpace != m_targetSpace) 1651 if (m_usePID)
1794 { 1652 {
1795 if (prm.m_targetSpace != IntPtr.Zero) 1653//Console.WriteLine("PID " + Name);
1654 // KF - this is for object move? eg. llSetPos() ?
1655 //if (!d.BodyIsEnabled(Body))
1656 //d.BodySetForce(Body, 0f, 0f, 0f);
1657 // If we're using the PID controller, then we have no gravity
1658 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1659 fz = 0f;
1660
1661 // no lock; for now it's only called from within Simulate()
1662
1663 // If the PID Controller isn't active then we set our force
1664 // calculating base velocity to the current position
1665
1666 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1796 { 1667 {
1797 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); 1668 //PID_G = PID_G / m_PIDTau;
1798 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 1669 m_PIDTau = 1;
1799 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1800 } 1670 }
1801 prm.m_targetSpace = m_targetSpace; 1671
1802 d.SpaceAdd(m_targetSpace, prm.prim_geom); 1672 if ((PID_G - m_PIDTau) <= 0)
1803 } 1673 {
1804 1674 PID_G = m_PIDTau + 1;
1805 prm.m_collisionscore = 0; 1675 }
1676 //PidStatus = true;
1806 1677
1807 if(!m_disabled) 1678 // PhysicsVector vec = new PhysicsVector();
1808 prm.m_disabled = false; 1679 d.Vector3 vel = d.BodyGetLinearVel(Body);
1809 1680
1810 _parent_scene.addActivePrim(prm); 1681 d.Vector3 pos = d.BodyGetPosition(Body);
1811 } 1682 _target_velocity =
1812 } 1683 new Vector3(
1684 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1685 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1686 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1687 );
1813 1688
1814 // The body doesn't already have a finite rotation mode set here 1689 // if velocity is zero, use position control; otherwise, velocity control
1815 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1816 {
1817 createAMotor(m_angularlock);
1818 }
1819 1690
1691 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1692 {
1693 // keep track of where we stopped. No more slippin' & slidin'
1694
1695 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1696 // react to the physics scene by moving it's position.
1697 // Avatar to Avatar collisions
1698 // Prim to avatar collisions
1699
1700 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1701 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1702 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1703 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1704 d.BodySetLinearVel(Body, 0, 0, 0);
1705 d.BodyAddForce(Body, 0, 0, fz);
1706 return;
1707 }
1708 else
1709 {
1710 _zeroFlag = false;
1820 1711
1821 if (m_isSelected || m_disabled) 1712 // We're flying and colliding with something
1822 { 1713 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1823 d.BodyDisable(Body); 1714 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1824 } 1715
1825 else 1716 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1826 {
1827 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1828 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1829 }
1830 _parent_scene.addActiveGroups(this);
1831 }
1832 1717
1833 private void DestroyBody() 1718 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1834 { 1719 }
1835 if (Body != IntPtr.Zero) 1720 } // end if (m_usePID)
1836 {
1837 _parent_scene.remActivePrim(this);
1838
1839 collide_geom = IntPtr.Zero;
1840 1721
1841 if (m_disabled) 1722 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1842 m_collisionCategories = 0; 1723 if (m_useHoverPID && !m_usePID)
1843 else if (m_isSelected) 1724 {
1844 m_collisionCategories = CollisionCategories.Selected; 1725//Console.WriteLine("Hover " + Name);
1845 else if (m_isVolumeDetect) 1726
1846 m_collisionCategories = CollisionCategories.VolumeDtc; 1727 // If we're using the PID controller, then we have no gravity
1847 else if (m_isphantom) 1728 fz = (-1 * _parent_scene.gravityz) * m_mass;
1848 m_collisionCategories = CollisionCategories.Phantom;
1849 else
1850 m_collisionCategories = CollisionCategories.Geom;
1851 1729
1852 m_collisionFlags = 0; 1730 // no lock; for now it's only called from within Simulate()
1853 1731
1854 if (prim_geom != IntPtr.Zero) 1732 // If the PID Controller isn't active then we set our force
1855 { 1733 // calculating base velocity to the current position
1856 if (m_NoColide)
1857 {
1858 d.GeomSetCategoryBits(prim_geom, 0);
1859 d.GeomSetCollideBits(prim_geom, 0);
1860 }
1861 else
1862 {
1863 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1864 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1865 }
1866 UpdateDataFromGeom();
1867 d.GeomSetBody(prim_geom, IntPtr.Zero);
1868 SetInStaticSpace(this);
1869 }
1870 1734
1871 if (!childPrim) 1735 if ((m_PIDTau < 1))
1872 {
1873 lock (childrenPrim)
1874 {
1875 foreach (OdePrim prm in childrenPrim)
1876 { 1736 {
1877 _parent_scene.remActivePrim(prm); 1737 PID_G = PID_G / m_PIDTau;
1738 }
1878 1739
1879 if (prm.m_isSelected) 1740 if ((PID_G - m_PIDTau) <= 0)
1880 prm.m_collisionCategories = CollisionCategories.Selected; 1741 {
1881 else if (prm.m_isVolumeDetect) 1742 PID_G = m_PIDTau + 1;
1882 prm.m_collisionCategories = CollisionCategories.VolumeDtc; 1743 }
1883 else if (prm.m_isphantom)
1884 prm.m_collisionCategories = CollisionCategories.Phantom;
1885 else
1886 prm.m_collisionCategories = CollisionCategories.Geom;
1887 1744
1888 prm.m_collisionFlags = 0; 1745 // Where are we, and where are we headed?
1746 d.Vector3 pos = d.BodyGetPosition(Body);
1747 d.Vector3 vel = d.BodyGetLinearVel(Body);
1889 1748
1890 if (prm.prim_geom != IntPtr.Zero) 1749 // Non-Vehicles have a limited set of Hover options.
1891 { 1750 // determine what our target height really is based on HoverType
1892 if (prm.m_NoColide) 1751 switch (m_PIDHoverType)
1752 {
1753 case PIDHoverType.Ground:
1754 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1755 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1756 break;
1757 case PIDHoverType.GroundAndWater:
1758 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1759 m_waterHeight = _parent_scene.GetWaterLevel();
1760 if (m_groundHeight > m_waterHeight)
1893 { 1761 {
1894 d.GeomSetCategoryBits(prm.prim_geom, 0); 1762 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1895 d.GeomSetCollideBits(prm.prim_geom, 0);
1896 } 1763 }
1897 else 1764 else
1898 { 1765 {
1899 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); 1766 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1900 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1901 } 1767 }
1902 prm.UpdateDataFromGeom(); 1768 break;
1903 SetInStaticSpace(prm);
1904 }
1905 prm.Body = IntPtr.Zero;
1906 prm._mass = prm.primMass;
1907 prm.m_collisionscore = 0;
1908 }
1909 }
1910 if (Amotor != IntPtr.Zero)
1911 {
1912 d.JointDestroy(Amotor);
1913 Amotor = IntPtr.Zero;
1914 }
1915 _parent_scene.remActiveGroup(this);
1916 d.BodyDestroy(Body);
1917 }
1918 Body = IntPtr.Zero;
1919 }
1920 _mass = primMass;
1921 m_collisionscore = 0;
1922 }
1923
1924 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1925 {
1926 d.Matrix3 mat = new d.Matrix3();
1927 d.Quaternion quat = new d.Quaternion();
1928
1929 d.Mass tmpdmass = new d.Mass { };
1930 d.Mass objdmass = new d.Mass { };
1931
1932 d.BodyGetMass(Body, out tmpdmass);
1933 objdmass = tmpdmass;
1934
1935 d.Vector3 dobjpos;
1936 d.Vector3 thispos;
1937 1769
1938 // get current object position and rotation 1770 } // end switch (m_PIDHoverType)
1939 dobjpos = d.BodyGetPosition(Body);
1940 1771
1941 // get prim own inertia in its local frame
1942 tmpdmass = primdMass;
1943 1772
1944 // transform to object frame 1773 _target_velocity =
1945 mat = d.GeomGetOffsetRotation(prim_geom); 1774 new Vector3(0.0f, 0.0f,
1946 d.MassRotate(ref tmpdmass, ref mat); 1775 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1776 );
1947 1777
1948 thispos = d.GeomGetOffsetPosition(prim_geom); 1778 // if velocity is zero, use position control; otherwise, velocity control
1949 d.MassTranslate(ref tmpdmass,
1950 thispos.X,
1951 thispos.Y,
1952 thispos.Z);
1953 1779
1954 // subtract current prim inertia from object 1780 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1955 DMassSubPartFromObj(ref tmpdmass, ref objdmass); 1781 {
1956 1782 // keep track of where we stopped. No more slippin' & slidin'
1957 // back prim own inertia 1783
1958 tmpdmass = primdMass; 1784 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1785 // react to the physics scene by moving it's position.
1786 // Avatar to Avatar collisions
1787 // Prim to avatar collisions
1788
1789 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1790 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1791 d.BodyAddForce(Body, 0, 0, fz);
1792 return;
1793 }
1794 else
1795 {
1796 _zeroFlag = false;
1959 1797
1960 // update to new position and orientation 1798 // We're flying and colliding with something
1961 _position = NewPos; 1799 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1962 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); 1800 }
1963 _orientation = newrot; 1801 }
1964 quat.X = newrot.X;
1965 quat.Y = newrot.Y;
1966 quat.Z = newrot.Z;
1967 quat.W = newrot.W;
1968 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1969 1802
1970 mat = d.GeomGetOffsetRotation(prim_geom); 1803 fx *= m_mass;
1971 d.MassRotate(ref tmpdmass, ref mat); 1804 fy *= m_mass;
1805 //fz *= m_mass;
1972 1806
1973 thispos = d.GeomGetOffsetPosition(prim_geom); 1807 fx += m_force.X;
1974 d.MassTranslate(ref tmpdmass, 1808 fy += m_force.Y;
1975 thispos.X, 1809 fz += m_force.Z;
1976 thispos.Y,
1977 thispos.Z);
1978 1810
1979 d.MassAdd(ref objdmass, ref tmpdmass); 1811 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1812 if (fx != 0 || fy != 0 || fz != 0)
1813 {
1814 //m_taintdisable = true;
1815 //base.RaiseOutOfBounds(Position);
1816 //d.BodySetLinearVel(Body, fx, fy, 0f);
1817 if (!d.BodyIsEnabled(Body))
1818 {
1819 // A physical body at rest on a surface will auto-disable after a while,
1820 // this appears to re-enable it incase the surface it is upon vanishes,
1821 // and the body should fall again.
1822 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1823 d.BodySetForce(Body, 0, 0, 0);
1824 enableBodySoft();
1825 }
1980 1826
1981 // fix all positions 1827 // 35x10 = 350n times the mass per second applied maximum.
1982 IntPtr g = d.BodyGetFirstGeom(Body); 1828 float nmax = 35f * m_mass;
1983 while (g != IntPtr.Zero) 1829 float nmin = -35f * m_mass;
1984 { 1830
1985 thispos = d.GeomGetOffsetPosition(g); 1831 if (fx > nmax)
1986 thispos.X -= objdmass.c.X; 1832 fx = nmax;
1987 thispos.Y -= objdmass.c.Y; 1833 if (fx < nmin)
1988 thispos.Z -= objdmass.c.Z; 1834 fx = nmin;
1989 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 1835 if (fy > nmax)
1990 g = d.dBodyGetNextGeom(g); 1836 fy = nmax;
1837 if (fy < nmin)
1838 fy = nmin;
1839 d.BodyAddForce(Body, fx, fy, fz);
1840//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1841 }
1842 }
1843 }
1844 else
1845 { // is not physical, or is not a body or is selected
1846 // _zeroPosition = d.BodyGetPosition(Body);
1847 return;
1848//Console.WriteLine("Nothing " + Name);
1849
1991 } 1850 }
1992 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1993
1994 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1995 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1996 d.BodySetMass(Body, ref objdmass);
1997 _mass = objdmass.mass;
1998 } 1851 }
1999 1852
2000 1853 private void rotate()
2001
2002 private void FixInertia(Vector3 NewPos)
2003 { 1854 {
2004 d.Matrix3 primmat = new d.Matrix3(); 1855 d.Quaternion myrot = new d.Quaternion();
2005 d.Mass tmpdmass = new d.Mass { }; 1856 myrot.X = _orientation.X;
2006 d.Mass objdmass = new d.Mass { }; 1857 myrot.Y = _orientation.Y;
2007 d.Mass primmass = new d.Mass { }; 1858 myrot.Z = _orientation.Z;
2008 1859 myrot.W = _orientation.W;
2009 d.Vector3 dobjpos; 1860 if (Body != IntPtr.Zero)
2010 d.Vector3 thispos;
2011
2012 d.BodyGetMass(Body, out objdmass);
2013
2014 // get prim own inertia in its local frame
2015 primmass = primdMass;
2016 // transform to object frame
2017 primmat = d.GeomGetOffsetRotation(prim_geom);
2018 d.MassRotate(ref primmass, ref primmat);
2019
2020 tmpdmass = primmass;
2021
2022 thispos = d.GeomGetOffsetPosition(prim_geom);
2023 d.MassTranslate(ref tmpdmass,
2024 thispos.X,
2025 thispos.Y,
2026 thispos.Z);
2027
2028 // subtract current prim inertia from object
2029 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2030
2031 // update to new position
2032 _position = NewPos;
2033 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2034
2035 thispos = d.GeomGetOffsetPosition(prim_geom);
2036 d.MassTranslate(ref primmass,
2037 thispos.X,
2038 thispos.Y,
2039 thispos.Z);
2040
2041 d.MassAdd(ref objdmass, ref primmass);
2042
2043 // fix all positions
2044 IntPtr g = d.BodyGetFirstGeom(Body);
2045 while (g != IntPtr.Zero)
2046 { 1861 {
2047 thispos = d.GeomGetOffsetPosition(g); 1862 // KF: If this is a root prim do BodySet
2048 thispos.X -= objdmass.c.X; 1863 d.BodySetQuaternion(Body, ref myrot);
2049 thispos.Y -= objdmass.c.Y; 1864 if (IsPhysical)
2050 thispos.Z -= objdmass.c.Z; 1865 {
2051 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 1866 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
2052 g = d.dBodyGetNextGeom(g); 1867 createAMotor(m_angularlock);
1868 }
2053 } 1869 }
2054 1870 else
2055 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2056
2057 // get current object position and rotation
2058 dobjpos = d.BodyGetPosition(Body);
2059
2060 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2061 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2062 d.BodySetMass(Body, ref objdmass);
2063 _mass = objdmass.mass;
2064 }
2065
2066 private void FixInertia(Quaternion newrot)
2067 {
2068 d.Matrix3 mat = new d.Matrix3();
2069 d.Quaternion quat = new d.Quaternion();
2070
2071 d.Mass tmpdmass = new d.Mass { };
2072 d.Mass objdmass = new d.Mass { };
2073 d.Vector3 dobjpos;
2074 d.Vector3 thispos;
2075
2076 d.BodyGetMass(Body, out objdmass);
2077
2078 // get prim own inertia in its local frame
2079 tmpdmass = primdMass;
2080 mat = d.GeomGetOffsetRotation(prim_geom);
2081 d.MassRotate(ref tmpdmass, ref mat);
2082 // transform to object frame
2083 thispos = d.GeomGetOffsetPosition(prim_geom);
2084 d.MassTranslate(ref tmpdmass,
2085 thispos.X,
2086 thispos.Y,
2087 thispos.Z);
2088
2089 // subtract current prim inertia from object
2090 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2091
2092 // update to new orientation
2093 _orientation = newrot;
2094 quat.X = newrot.X;
2095 quat.Y = newrot.Y;
2096 quat.Z = newrot.Z;
2097 quat.W = newrot.W;
2098 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2099
2100 tmpdmass = primdMass;
2101 mat = d.GeomGetOffsetRotation(prim_geom);
2102 d.MassRotate(ref tmpdmass, ref mat);
2103 d.MassTranslate(ref tmpdmass,
2104 thispos.X,
2105 thispos.Y,
2106 thispos.Z);
2107
2108 d.MassAdd(ref objdmass, ref tmpdmass);
2109
2110 // fix all positions
2111 IntPtr g = d.BodyGetFirstGeom(Body);
2112 while (g != IntPtr.Zero)
2113 { 1871 {
2114 thispos = d.GeomGetOffsetPosition(g); 1872 // daughter prim, do Geom set
2115 thispos.X -= objdmass.c.X; 1873 d.GeomSetQuaternion(prim_geom, ref myrot);
2116 thispos.Y -= objdmass.c.Y;
2117 thispos.Z -= objdmass.c.Z;
2118 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2119 g = d.dBodyGetNextGeom(g);
2120 } 1874 }
2121 1875
2122 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); 1876 resetCollisionAccounting();
2123 // get current object position and rotation 1877 m_taintrot = _orientation;
2124 dobjpos = d.BodyGetPosition(Body);
2125
2126 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2127 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2128 d.BodySetMass(Body, ref objdmass);
2129 _mass = objdmass.mass;
2130 } 1878 }
2131 1879
2132 1880 private void resetCollisionAccounting()
2133 #region Mass Calculation
2134
2135 private void UpdatePrimBodyData()
2136 { 1881 {
2137 primMass = m_density * primVolume; 1882 m_collisionscore = 0;
2138 1883 m_interpenetrationcount = 0;
2139 if (primMass <= 0) 1884 m_disabled = false;
2140 primMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1885 }
2141 if (primMass > _parent_scene.maximumMassObject)
2142 primMass = _parent_scene.maximumMassObject;
2143
2144 _mass = primMass; // just in case
2145
2146 d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z);
2147
2148 d.MassTranslate(ref primdMass,
2149 m_OBBOffset.X,
2150 m_OBBOffset.Y,
2151 m_OBBOffset.Z);
2152
2153 primOOBradiusSQ = m_OBB.LengthSquared();
2154 1886
2155 if (_triMeshData != IntPtr.Zero) 1887 /// <summary>
1888 /// Change prim in response to a disable taint.
1889 /// </summary>
1890 private void changedisable()
1891 {
1892 m_disabled = true;
1893 if (Body != IntPtr.Zero)
2156 { 1894 {
2157 float pc = m_physCost; 1895 d.BodyDisable(Body);
2158 float psf = primOOBradiusSQ; 1896 Body = IntPtr.Zero;
2159 psf *= 1.33f * .2f;
2160 pc *= psf;
2161 if (pc < 0.1f)
2162 pc = 0.1f;
2163
2164 m_physCost = pc;
2165 } 1897 }
2166 else
2167 m_physCost = 0.1f;
2168 1898
2169 m_streamCost = 1.0f; 1899 m_taintdisable = false;
2170 } 1900 }
2171 1901
2172 #endregion
2173
2174
2175 /// <summary> 1902 /// <summary>
2176 /// Add a child prim to this parent prim. 1903 /// Change prim in response to a physics status taint
2177 /// </summary> 1904 /// </summary>
2178 /// <param name="prim">Child prim</param> 1905 private void changePhysicsStatus()
2179 // I'm the parent
2180 // prim is the child
2181 public void ParentPrim(OdePrim prim)
2182 { 1906 {
2183 //Console.WriteLine("ParentPrim " + m_primName); 1907 if (IsPhysical)
2184 if (this.m_localID != prim.m_localID)
2185 { 1908 {
2186 DestroyBody(); // for now we need to rebuil entire object on link change 1909 if (Body == IntPtr.Zero)
2187
2188 lock (childrenPrim)
2189 { 1910 {
2190 // adopt the prim 1911 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2191 if (!childrenPrim.Contains(prim))
2192 childrenPrim.Add(prim);
2193
2194 // see if this prim has kids and adopt them also
2195 // should not happen for now
2196 foreach (OdePrim prm in prim.childrenPrim)
2197 { 1912 {
2198 if (!childrenPrim.Contains(prm)) 1913 changeshape();
2199 { 1914 }
2200 if (prm.Body != IntPtr.Zero) 1915 else
2201 { 1916 {
2202 if (prm.prim_geom != IntPtr.Zero) 1917 enableBody();
2203 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2204 if (prm.Body != prim.Body)
2205 prm.DestroyBody(); // don't loose bodies around
2206 prm.Body = IntPtr.Zero;
2207 }
2208
2209 childrenPrim.Add(prm);
2210 prm._parent = this;
2211 }
2212 } 1918 }
2213 } 1919 }
2214 //Remove old children from the prim
2215 prim.childrenPrim.Clear();
2216
2217 if (prim.Body != IntPtr.Zero)
2218 {
2219 if (prim.prim_geom != IntPtr.Zero)
2220 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2221 prim.DestroyBody(); // don't loose bodies around
2222 prim.Body = IntPtr.Zero;
2223 }
2224
2225 prim.childPrim = true;
2226 prim._parent = this;
2227
2228 MakeBody(); // full nasty reconstruction
2229 }
2230 }
2231
2232 private void UpdateChildsfromgeom()
2233 {
2234 if (childrenPrim.Count > 0)
2235 {
2236 foreach (OdePrim prm in childrenPrim)
2237 prm.UpdateDataFromGeom();
2238 } 1920 }
2239 } 1921 else
2240
2241 private void UpdateDataFromGeom()
2242 {
2243 if (prim_geom != IntPtr.Zero)
2244 { 1922 {
2245 d.Quaternion qtmp; 1923 if (Body != IntPtr.Zero)
2246 d.GeomCopyQuaternion(prim_geom, out qtmp); 1924 {
2247 _orientation.X = qtmp.X; 1925 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2248 _orientation.Y = qtmp.Y; 1926 {
2249 _orientation.Z = qtmp.Z; 1927 RemoveGeom();
2250 _orientation.W = qtmp.W;
2251
2252// Debug
2253 float qlen = _orientation.Length();
2254 if (qlen > 1.01f || qlen < 0.99)
2255 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
2256//
2257 _orientation.Normalize();
2258
2259 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2260 _position.X = lpos.X;
2261 _position.Y = lpos.Y;
2262 _position.Z = lpos.Z;
2263 }
2264 }
2265
2266 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2267 {
2268 // Okay, we have a delinked child.. destroy all body and remake
2269 if (odePrim != this && !childrenPrim.Contains(odePrim))
2270 return;
2271 1928
2272 DestroyBody(); 1929//Console.WriteLine("changePhysicsStatus for " + Name);
1930 changeadd();
1931 }
2273 1932
2274 if (odePrim == this) // delinking the root prim 1933 if (childPrim)
2275 {
2276 OdePrim newroot = null;
2277 lock (childrenPrim)
2278 {
2279 if (childrenPrim.Count > 0)
2280 { 1934 {
2281 newroot = childrenPrim[0]; 1935 if (_parent != null)
2282 childrenPrim.RemoveAt(0);
2283 foreach (OdePrim prm in childrenPrim)
2284 { 1936 {
2285 newroot.childrenPrim.Add(prm); 1937 OdePrim parent = (OdePrim)_parent;
1938 parent.ChildDelink(this);
2286 } 1939 }
2287 childrenPrim.Clear();
2288 } 1940 }
2289 if (newroot != null) 1941 else
2290 { 1942 {
2291 newroot.childPrim = false; 1943 disableBody();
2292 newroot._parent = null;
2293 if (remakebodies)
2294 newroot.MakeBody();
2295 } 1944 }
2296 } 1945 }
2297 } 1946 }
2298 1947
2299 else 1948 changeSelectedStatus();
2300 { 1949
2301 lock (childrenPrim) 1950 resetCollisionAccounting();
2302 { 1951 m_taintPhysics = IsPhysical;
2303 childrenPrim.Remove(odePrim);
2304 odePrim.childPrim = false;
2305 odePrim._parent = null;
2306 // odePrim.UpdateDataFromGeom();
2307 if (remakebodies)
2308 odePrim.MakeBody();
2309 }
2310 }
2311 if (remakebodies)
2312 MakeBody();
2313 } 1952 }
2314 1953
2315 protected void ChildRemove(OdePrim odePrim, bool reMakeBody) 1954 /// <summary>
1955 /// Change prim in response to a size taint.
1956 /// </summary>
1957 private void changesize()
2316 { 1958 {
2317 // Okay, we have a delinked child.. destroy all body and remake 1959#if SPAM
2318 if (odePrim != this && !childrenPrim.Contains(odePrim)) 1960 m_log.DebugFormat("[ODE PRIM]: Called changesize");
2319 return; 1961#endif
2320 1962
2321 DestroyBody(); 1963 if (_size.X <= 0) _size.X = 0.01f;
1964 if (_size.Y <= 0) _size.Y = 0.01f;
1965 if (_size.Z <= 0) _size.Z = 0.01f;
2322 1966
2323 if (odePrim == this) 1967 //kill body to rebuild
1968 if (IsPhysical && Body != IntPtr.Zero)
2324 { 1969 {
2325 OdePrim newroot = null; 1970 if (childPrim)
2326 lock (childrenPrim)
2327 { 1971 {
2328 if (childrenPrim.Count > 0) 1972 if (_parent != null)
2329 {
2330 newroot = childrenPrim[0];
2331 childrenPrim.RemoveAt(0);
2332 foreach (OdePrim prm in childrenPrim)
2333 {
2334 newroot.childrenPrim.Add(prm);
2335 }
2336 childrenPrim.Clear();
2337 }
2338 if (newroot != null)
2339 { 1973 {
2340 newroot.childPrim = false; 1974 OdePrim parent = (OdePrim)_parent;
2341 newroot._parent = null; 1975 parent.ChildDelink(this);
2342 newroot.MakeBody();
2343 } 1976 }
2344 } 1977 }
2345 if (reMakeBody) 1978 else
2346 MakeBody();
2347 return;
2348 }
2349 else
2350 {
2351 lock (childrenPrim)
2352 { 1979 {
2353 childrenPrim.Remove(odePrim); 1980 disableBody();
2354 odePrim.childPrim = false;
2355 odePrim._parent = null;
2356 if (reMakeBody)
2357 odePrim.MakeBody();
2358 } 1981 }
2359 } 1982 }
2360 MakeBody();
2361 }
2362 1983
2363 #region changes 1984 if (d.SpaceQuery(m_targetSpace, prim_geom))
1985 {
1986// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1987 d.SpaceRemove(m_targetSpace, prim_geom);
1988 }
2364 1989
2365 private void changeadd() 1990 RemoveGeom();
2366 {
2367 }
2368 1991
2369 private void changeAngularLock(Vector3 newLock) 1992 // we don't need to do space calculation because the client sends a position update also.
2370 { 1993
2371 // do we have a Physical object? 1994 IMesh mesh = null;
2372 if (Body != IntPtr.Zero) 1995
1996 // Construction of new prim
1997 if (_parent_scene.needsMeshing(_pbs))
2373 { 1998 {
2374 //Check that we have a Parent 1999 float meshlod = _parent_scene.meshSculptLOD;
2375 //If we have a parent then we're not authorative here 2000
2376 if (_parent == null) 2001 if (IsPhysical)
2002 meshlod = _parent_scene.MeshSculptphysicalLOD;
2003 // Don't need to re-enable body.. it's done in SetMesh
2004
2005 if (_parent_scene.needsMeshing(_pbs))
2377 { 2006 {
2378 if (!newLock.ApproxEquals(Vector3.One, 0f)) 2007 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2379 { 2008 if (mesh == null)
2380 createAMotor(newLock); 2009 CheckMeshAsset();
2381 }
2382 else
2383 {
2384 if (Amotor != IntPtr.Zero)
2385 {
2386 d.JointDestroy(Amotor);
2387 Amotor = IntPtr.Zero;
2388 }
2389 }
2390 } 2010 }
2011
2391 } 2012 }
2392 // Store this for later in case we get turned into a separate body
2393 m_angularlock = newLock;
2394 }
2395 2013
2396 private void changeLink(OdePrim NewParent) 2014 CreateGeom(m_targetSpace, mesh);
2397 { 2015 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2398 if (_parent == null && NewParent != null) 2016 d.Quaternion myrot = new d.Quaternion();
2017 myrot.X = _orientation.X;
2018 myrot.Y = _orientation.Y;
2019 myrot.Z = _orientation.Z;
2020 myrot.W = _orientation.W;
2021 d.GeomSetQuaternion(prim_geom, ref myrot);
2022
2023 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2024 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2399 { 2025 {
2400 NewParent.ParentPrim(this); 2026 // Re creates body on size.
2027 // EnableBody also does setMass()
2028 enableBody();
2029 d.BodyEnable(Body);
2401 } 2030 }
2402 else if (_parent != null) 2031
2032 changeSelectedStatus();
2033
2034 if (childPrim)
2403 { 2035 {
2404 if (_parent is OdePrim) 2036 if (_parent is OdePrim)
2405 { 2037 {
2406 if (NewParent != _parent) 2038 OdePrim parent = (OdePrim)_parent;
2407 { 2039 parent.ChildSetGeom(this);
2408 (_parent as OdePrim).ChildDelink(this, false); // for now...
2409 childPrim = false;
2410
2411 if (NewParent != null)
2412 {
2413 NewParent.ParentPrim(this);
2414 }
2415 }
2416 } 2040 }
2417 } 2041 }
2418 _parent = NewParent; 2042 resetCollisionAccounting();
2043 m_taintsize = _size;
2419 } 2044 }
2420 2045
2421 2046 /// <summary>
2422 private void Stop() 2047 /// Change prim in response to a float on water taint.
2048 /// </summary>
2049 /// <param name="timestep"></param>
2050 private void changefloatonwater()
2423 { 2051 {
2424 if (!childPrim) 2052 m_collidesWater = m_taintCollidesWater;
2425 {
2426 m_force = Vector3.Zero;
2427 m_forceacc = Vector3.Zero;
2428 m_angularForceacc = Vector3.Zero;
2429 _torque = Vector3.Zero;
2430 _velocity = Vector3.Zero;
2431 _acceleration = Vector3.Zero;
2432 m_rotationalVelocity = Vector3.Zero;
2433 _target_velocity = Vector3.Zero;
2434 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2435 m_vehicle.Stop();
2436 2053
2437 _zeroFlag = false; 2054 if (m_collidesWater)
2438 base.RequestPhysicsterseUpdate(); 2055 {
2056 m_collisionFlags |= CollisionCategories.Water;
2439 } 2057 }
2440 2058 else
2441 if (Body != IntPtr.Zero)
2442 { 2059 {
2443 d.BodySetForce(Body, 0f, 0f, 0f); 2060 m_collisionFlags &= ~CollisionCategories.Water;
2444 d.BodySetTorque(Body, 0f, 0f, 0f);
2445 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2446 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2447 } 2061 }
2062
2063 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2448 } 2064 }
2449 2065
2450 private void changePhantomStatus(bool newval) 2066 /// <summary>
2067 /// Change prim in response to a shape taint.
2068 /// </summary>
2069 private void changeshape()
2451 { 2070 {
2452 m_isphantom = newval; 2071 m_taintshape = false;
2453 2072
2454 UpdateCollisionCatFlags(); 2073 // Cleanup of old prim geometry and Bodies
2455 ApplyCollisionCatFlags(); 2074 if (IsPhysical && Body != IntPtr.Zero)
2456 } 2075 {
2076 if (childPrim)
2077 {
2078 if (_parent != null)
2079 {
2080 OdePrim parent = (OdePrim)_parent;
2081 parent.ChildDelink(this);
2082 }
2083 }
2084 else
2085 {
2086 disableBody();
2087 }
2088 }
2457 2089
2458/* not in use 2090 RemoveGeom();
2459 internal void ChildSelectedChange(bool childSelect)
2460 {
2461 if(childPrim)
2462 return;
2463 2091
2464 if (childSelect == m_isSelected) 2092 // we don't need to do space calculation because the client sends a position update also.
2465 return; 2093 if (_size.X <= 0) _size.X = 0.01f;
2094 if (_size.Y <= 0) _size.Y = 0.01f;
2095 if (_size.Z <= 0) _size.Z = 0.01f;
2096 // Construction of new prim
2097
2098 IMesh mesh = null;
2466 2099
2467 if (childSelect) 2100
2101 if (_parent_scene.needsMeshing(_pbs))
2468 { 2102 {
2469 DoSelectedStatus(true); 2103 // Don't need to re-enable body.. it's done in CreateMesh
2104 float meshlod = _parent_scene.meshSculptLOD;
2105
2106 if (IsPhysical)
2107 meshlod = _parent_scene.MeshSculptphysicalLOD;
2108
2109 // createmesh returns null when it doesn't mesh.
2110 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2111 if (mesh == null)
2112 CheckMeshAsset();
2470 } 2113 }
2471 2114
2472 else 2115 CreateGeom(m_targetSpace, mesh);
2116 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2117 d.Quaternion myrot = new d.Quaternion();
2118 //myrot.W = _orientation.w;
2119 myrot.W = _orientation.W;
2120 myrot.X = _orientation.X;
2121 myrot.Y = _orientation.Y;
2122 myrot.Z = _orientation.Z;
2123 d.GeomSetQuaternion(prim_geom, ref myrot);
2124
2125 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2126 if (IsPhysical && Body == IntPtr.Zero)
2473 { 2127 {
2474 foreach (OdePrim prm in childrenPrim) 2128 // Re creates body on size.
2129 // EnableBody also does setMass()
2130 enableBody();
2131 if (Body != IntPtr.Zero)
2475 { 2132 {
2476 if (prm.m_isSelected) 2133 d.BodyEnable(Body);
2477 return;
2478 } 2134 }
2479 DoSelectedStatus(false);
2480 } 2135 }
2481 }
2482*/
2483 private void changeSelectedStatus(bool newval)
2484 {
2485 if (m_lastdoneSelected == newval)
2486 return;
2487 2136
2488 m_lastdoneSelected = newval; 2137 changeSelectedStatus();
2489 DoSelectedStatus(newval);
2490 }
2491 2138
2492 private void CheckDelaySelect() 2139 if (childPrim)
2493 {
2494 if (m_delaySelect)
2495 { 2140 {
2496 DoSelectedStatus(m_isSelected); 2141 if (_parent is OdePrim)
2142 {
2143 OdePrim parent = (OdePrim)_parent;
2144 parent.ChildSetGeom(this);
2145 }
2497 } 2146 }
2147
2148 resetCollisionAccounting();
2149// m_taintshape = false;
2498 } 2150 }
2499 2151
2500 private void DoSelectedStatus(bool newval) 2152 /// <summary>
2153 /// Change prim in response to an add force taint.
2154 /// </summary>
2155 private void changeAddForce()
2501 { 2156 {
2502 m_isSelected = newval; 2157 if (!m_isSelected)
2503 Stop();
2504
2505 if (newval)
2506 { 2158 {
2507 if (!childPrim && Body != IntPtr.Zero) 2159 lock (m_forcelist)
2508 d.BodyDisable(Body);
2509
2510 if (m_delaySelect || m_isphysical)
2511 { 2160 {
2512 m_collisionCategories = CollisionCategories.Selected; 2161 //m_log.Info("[PHYSICS]: dequeing forcelist");
2513 m_collisionFlags = 0; 2162 if (IsPhysical)
2514
2515 if (!childPrim)
2516 { 2163 {
2517 foreach (OdePrim prm in childrenPrim) 2164 Vector3 iforce = Vector3.Zero;
2165 int i = 0;
2166 try
2518 { 2167 {
2519 prm.m_collisionCategories = m_collisionCategories; 2168 for (i = 0; i < m_forcelist.Count; i++)
2520 prm.m_collisionFlags = m_collisionFlags;
2521
2522 if (prm.prim_geom != null)
2523 { 2169 {
2524 2170
2525 if (prm.m_NoColide) 2171 iforce = iforce + (m_forcelist[i] * 100);
2526 {
2527 d.GeomSetCategoryBits(prm.prim_geom, 0);
2528 d.GeomSetCollideBits(prm.prim_geom, 0);
2529 }
2530 else
2531 {
2532 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2533 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2534 }
2535 } 2172 }
2536 prm.m_delaySelect = false;
2537 } 2173 }
2538 } 2174 catch (IndexOutOfRangeException)
2539// else if (_parent != null)
2540// ((OdePrim)_parent).ChildSelectedChange(true);
2541
2542
2543 if (prim_geom != null)
2544 {
2545 if (m_NoColide)
2546 { 2175 {
2547 d.GeomSetCategoryBits(prim_geom, 0); 2176 m_forcelist = new List<Vector3>();
2548 d.GeomSetCollideBits(prim_geom, 0); 2177 m_collisionscore = 0;
2549 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2178 m_interpenetrationcount = 0;
2550 { 2179 m_taintforce = false;
2551 d.GeomSetCategoryBits(collide_geom, 0); 2180 return;
2552 d.GeomSetCollideBits(collide_geom, 0);
2553 }
2554
2555 } 2181 }
2556 else 2182 catch (ArgumentOutOfRangeException)
2557 { 2183 {
2558 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 2184 m_forcelist = new List<Vector3>();
2559 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 2185 m_collisionscore = 0;
2560 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2186 m_interpenetrationcount = 0;
2561 { 2187 m_taintforce = false;
2562 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); 2188 return;
2563 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2564 }
2565 } 2189 }
2566 }
2567
2568 m_delaySelect = false;
2569 }
2570 else if(!m_isphysical)
2571 {
2572 m_delaySelect = true;
2573 }
2574 }
2575 else
2576 {
2577 if (!childPrim)
2578 {
2579 if (Body != IntPtr.Zero && !m_disabled)
2580 d.BodyEnable(Body); 2190 d.BodyEnable(Body);
2191 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2192 }
2193 m_forcelist.Clear();
2581 } 2194 }
2582// else if (_parent != null)
2583// ((OdePrim)_parent).ChildSelectedChange(false);
2584 2195
2585 UpdateCollisionCatFlags(); 2196 m_collisionscore = 0;
2586 ApplyCollisionCatFlags(); 2197 m_interpenetrationcount = 0;
2587
2588 m_delaySelect = false;
2589 } 2198 }
2590 2199
2591 resetCollisionAccounting(); 2200 m_taintforce = false;
2592 } 2201 }
2593 2202
2594 private void changePosition(Vector3 newPos) 2203 /// <summary>
2204 /// Change prim in response to a torque taint.
2205 /// </summary>
2206 private void changeSetTorque()
2595 { 2207 {
2596 CheckDelaySelect(); 2208 if (!m_isSelected)
2597 if (m_isphysical)
2598 { 2209 {
2599 if (childPrim) // inertia is messed, must rebuild 2210 if (IsPhysical && Body != IntPtr.Zero)
2600 { 2211 {
2601 if (m_building) 2212 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2602 {
2603 _position = newPos;
2604 }
2605
2606 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2607 {
2608 FixInertia(newPos);
2609 if (!d.BodyIsEnabled(Body))
2610 d.BodyEnable(Body);
2611 }
2612 }
2613 else
2614 {
2615 if (_position != newPos)
2616 {
2617 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2618 _position = newPos;
2619 }
2620 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2621 d.BodyEnable(Body);
2622 } 2213 }
2623 } 2214 }
2624 else
2625 {
2626 if (prim_geom != IntPtr.Zero)
2627 {
2628 if (newPos != _position)
2629 {
2630 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2631 _position = newPos;
2632 2215
2633 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); 2216 m_taintTorque = Vector3.Zero;
2634 }
2635 }
2636 }
2637 givefakepos--;
2638 if (givefakepos < 0)
2639 givefakepos = 0;
2640// changeSelectedStatus();
2641 resetCollisionAccounting();
2642 } 2217 }
2643 2218
2644 private void changeOrientation(Quaternion newOri) 2219 /// <summary>
2220 /// Change prim in response to an angular force taint.
2221 /// </summary>
2222 private void changeAddAngularForce()
2645 { 2223 {
2646 CheckDelaySelect(); 2224 if (!m_isSelected)
2647 if (m_isphysical)
2648 { 2225 {
2649 if (childPrim) // inertia is messed, must rebuild 2226 lock (m_angularforcelist)
2650 {
2651 if (m_building)
2652 {
2653 _orientation = newOri;
2654 }
2655/*
2656 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2657 {
2658 FixInertia(_position, newOri);
2659 if (!d.BodyIsEnabled(Body))
2660 d.BodyEnable(Body);
2661 }
2662*/
2663 }
2664 else
2665 { 2227 {
2666 if (newOri != _orientation) 2228 //m_log.Info("[PHYSICS]: dequeing forcelist");
2229 if (IsPhysical)
2667 { 2230 {
2668 d.Quaternion myrot = new d.Quaternion(); 2231 Vector3 iforce = Vector3.Zero;
2669 myrot.X = newOri.X; 2232 for (int i = 0; i < m_angularforcelist.Count; i++)
2670 myrot.Y = newOri.Y; 2233 {
2671 myrot.Z = newOri.Z; 2234 iforce = iforce + (m_angularforcelist[i] * 100);
2672 myrot.W = newOri.W; 2235 }
2673 d.GeomSetQuaternion(prim_geom, ref myrot);
2674 _orientation = newOri;
2675 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2676 createAMotor(m_angularlock);
2677 }
2678 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2679 d.BodyEnable(Body); 2236 d.BodyEnable(Body);
2680 } 2237 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2681 } 2238
2682 else
2683 {
2684 if (prim_geom != IntPtr.Zero)
2685 {
2686 if (newOri != _orientation)
2687 {
2688 d.Quaternion myrot = new d.Quaternion();
2689 myrot.X = newOri.X;
2690 myrot.Y = newOri.Y;
2691 myrot.Z = newOri.Z;
2692 myrot.W = newOri.W;
2693 d.GeomSetQuaternion(prim_geom, ref myrot);
2694 _orientation = newOri;
2695 } 2239 }
2240 m_angularforcelist.Clear();
2696 } 2241 }
2697 }
2698 givefakeori--;
2699 if (givefakeori < 0)
2700 givefakeori = 0;
2701 resetCollisionAccounting();
2702 }
2703 2242
2704 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) 2243 m_collisionscore = 0;
2705 { 2244 m_interpenetrationcount = 0;
2706 CheckDelaySelect();
2707 if (m_isphysical)
2708 {
2709 if (childPrim && m_building) // inertia is messed, must rebuild
2710 {
2711 _position = newPos;
2712 _orientation = newOri;
2713 }
2714 else
2715 {
2716 if (newOri != _orientation)
2717 {
2718 d.Quaternion myrot = new d.Quaternion();
2719 myrot.X = newOri.X;
2720 myrot.Y = newOri.Y;
2721 myrot.Z = newOri.Z;
2722 myrot.W = newOri.W;
2723 d.GeomSetQuaternion(prim_geom, ref myrot);
2724 _orientation = newOri;
2725 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2726 createAMotor(m_angularlock);
2727 }
2728 if (_position != newPos)
2729 {
2730 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2731 _position = newPos;
2732 }
2733 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2734 d.BodyEnable(Body);
2735 }
2736 } 2245 }
2737 else
2738 {
2739 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2740 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2741
2742 if (prim_geom != IntPtr.Zero)
2743 {
2744 if (newOri != _orientation)
2745 {
2746 d.Quaternion myrot = new d.Quaternion();
2747 myrot.X = newOri.X;
2748 myrot.Y = newOri.Y;
2749 myrot.Z = newOri.Z;
2750 myrot.W = newOri.W;
2751 d.GeomSetQuaternion(prim_geom, ref myrot);
2752 _orientation = newOri;
2753 }
2754 2246
2755 if (newPos != _position) 2247 m_taintaddangularforce = false;
2756 {
2757 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2758 _position = newPos;
2759
2760 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2761 }
2762 }
2763 }
2764 givefakepos--;
2765 if (givefakepos < 0)
2766 givefakepos = 0;
2767 givefakeori--;
2768 if (givefakeori < 0)
2769 givefakeori = 0;
2770 resetCollisionAccounting();
2771 } 2248 }
2772 2249
2773 private void changeDisable(bool disable) 2250 /// <summary>
2251 /// Change prim in response to a velocity taint.
2252 /// </summary>
2253 private void changevelocity()
2774 { 2254 {
2775 if (disable) 2255 if (!m_isSelected)
2776 {
2777 if (!m_disabled)
2778 disableBodySoft();
2779 }
2780 else
2781 { 2256 {
2782 if (m_disabled) 2257 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
2783 enableBodySoft(); 2258 // walking through a default rez size prim if it keeps kicking it around - justincc.
2784 } 2259 Thread.Sleep(20);
2785 }
2786 2260
2787 private void changePhysicsStatus(bool NewStatus) 2261 if (IsPhysical)
2788 {
2789 CheckDelaySelect();
2790
2791 m_isphysical = NewStatus;
2792
2793 if (!childPrim)
2794 {
2795 if (NewStatus)
2796 {
2797 if (Body == IntPtr.Zero)
2798 MakeBody();
2799 }
2800 else
2801 { 2262 {
2802 if (Body != IntPtr.Zero) 2263 if (Body != IntPtr.Zero)
2803 { 2264 {
2804 DestroyBody(); 2265 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2805 } 2266 }
2806 Stop();
2807 } 2267 }
2268
2269 //resetCollisionAccounting();
2808 } 2270 }
2809 2271
2810 resetCollisionAccounting(); 2272 m_taintVelocity = Vector3.Zero;
2811 } 2273 }
2812 2274
2813 private void changeSize(Vector3 newSize) 2275 internal void setPrimForRemoval()
2814 { 2276 {
2277 m_taintremove = true;
2815 } 2278 }
2816 2279
2817 private void changeShape(PrimitiveBaseShape newShape) 2280 public override bool Flying
2818 { 2281 {
2282 // no flying prims for you
2283 get { return false; }
2284 set { }
2819 } 2285 }
2820 2286
2821 private void changeAddPhysRep(ODEPhysRepData repData) 2287 public override bool IsColliding
2822 { 2288 {
2823 _size = repData.size; //?? 2289 get { return iscolliding; }
2824 _pbs = repData.pbs; 2290 set { iscolliding = value; }
2825 m_shapetype = repData.shapetype; 2291 }
2826
2827 m_mesh = repData.mesh;
2828
2829 m_assetID = repData.assetID;
2830 m_meshState = repData.meshState;
2831 2292
2832 m_hasOBB = repData.hasOBB; 2293 public override bool CollidingGround
2833 m_OBBOffset = repData.OBBOffset; 2294 {
2834 m_OBB = repData.OBB; 2295 get { return false; }
2296 set { return; }
2297 }
2835 2298
2836 primVolume = repData.volume; 2299 public override bool CollidingObj
2300 {
2301 get { return false; }
2302 set { return; }
2303 }
2837 2304
2838 CreateGeom(); 2305 public override bool ThrottleUpdates
2306 {
2307 get { return m_throttleUpdates; }
2308 set { m_throttleUpdates = value; }
2309 }
2839 2310
2840 if (prim_geom != IntPtr.Zero) 2311 public override bool Stopped
2841 { 2312 {
2842 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2313 get { return _zeroFlag; }
2843 d.Quaternion myrot = new d.Quaternion(); 2314 }
2844 myrot.X = _orientation.X;
2845 myrot.Y = _orientation.Y;
2846 myrot.Z = _orientation.Z;
2847 myrot.W = _orientation.W;
2848 d.GeomSetQuaternion(prim_geom, ref myrot);
2849 }
2850 2315
2851 if (!m_isphysical) 2316 public override Vector3 Position
2852 { 2317 {
2853 SetInStaticSpace(this); 2318 get { return _position; }
2854 UpdateCollisionCatFlags();
2855 ApplyCollisionCatFlags();
2856 }
2857 else
2858 MakeBody();
2859 2319
2860 if ((m_meshState & MeshState.NeedMask) != 0) 2320 set { _position = value;
2861 { 2321 //m_log.Info("[PHYSICS]: " + _position.ToString());
2862 repData.size = _size;
2863 repData.pbs = _pbs;
2864 repData.shapetype = m_shapetype;
2865 _parent_scene.m_meshWorker.RequestMesh(repData);
2866 } 2322 }
2867 } 2323 }
2868 2324
2869 private void changePhysRepData(ODEPhysRepData repData) 2325 public override Vector3 Size
2870 { 2326 {
2871 CheckDelaySelect(); 2327 get { return _size; }
2872 2328 set
2873 OdePrim parent = (OdePrim)_parent;
2874
2875 bool chp = childPrim;
2876
2877 if (chp)
2878 { 2329 {
2879 if (parent != null) 2330 if (value.IsFinite())
2880 { 2331 {
2881 parent.DestroyBody(); 2332 _size = value;
2333// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
2334 }
2335 else
2336 {
2337 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
2882 } 2338 }
2883 } 2339 }
2884 else 2340 }
2885 {
2886 DestroyBody();
2887 }
2888
2889 RemoveGeom();
2890
2891 _size = repData.size;
2892 _pbs = repData.pbs;
2893 m_shapetype = repData.shapetype;
2894
2895 m_mesh = repData.mesh;
2896
2897 m_assetID = repData.assetID;
2898 m_meshState = repData.meshState;
2899
2900 m_hasOBB = repData.hasOBB;
2901 m_OBBOffset = repData.OBBOffset;
2902 m_OBB = repData.OBB;
2903
2904 primVolume = repData.volume;
2905
2906 CreateGeom();
2907 2341
2908 if (prim_geom != IntPtr.Zero) 2342 public override float Mass
2909 { 2343 {
2910 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2344 get { return CalculateMass(); }
2911 d.Quaternion myrot = new d.Quaternion(); 2345 }
2912 myrot.X = _orientation.X;
2913 myrot.Y = _orientation.Y;
2914 myrot.Z = _orientation.Z;
2915 myrot.W = _orientation.W;
2916 d.GeomSetQuaternion(prim_geom, ref myrot);
2917 }
2918 2346
2919 if (m_isphysical) 2347 public override Vector3 Force
2348 {
2349 //get { return Vector3.Zero; }
2350 get { return m_force; }
2351 set
2920 { 2352 {
2921 if (chp) 2353 if (value.IsFinite())
2922 { 2354 {
2923 if (parent != null) 2355 m_force = value;
2924 {
2925 parent.MakeBody();
2926 }
2927 } 2356 }
2928 else 2357 else
2929 MakeBody(); 2358 {
2930 } 2359 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
2931 else 2360 }
2932 {
2933 SetInStaticSpace(this);
2934 UpdateCollisionCatFlags();
2935 ApplyCollisionCatFlags();
2936 } 2361 }
2362 }
2937 2363
2938 resetCollisionAccounting(); 2364 public override int VehicleType
2365 {
2366 get { return (int)m_vehicle.Type; }
2367 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2368 }
2939 2369
2940 if ((m_meshState & MeshState.NeedMask) != 0) 2370 public override void VehicleFloatParam(int param, float value)
2941 { 2371 {
2942 repData.size = _size; 2372 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2943 repData.pbs = _pbs;
2944 repData.shapetype = m_shapetype;
2945 _parent_scene.m_meshWorker.RequestMesh(repData);
2946 }
2947 } 2373 }
2948 2374
2949 private void changeFloatOnWater(bool newval) 2375 public override void VehicleVectorParam(int param, Vector3 value)
2950 { 2376 {
2951 m_collidesWater = newval; 2377 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2378 }
2952 2379
2953 UpdateCollisionCatFlags(); 2380 public override void VehicleRotationParam(int param, Quaternion rotation)
2954 ApplyCollisionCatFlags(); 2381 {
2382 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2955 } 2383 }
2956 2384
2957 private void changeSetTorque(Vector3 newtorque) 2385 public override void VehicleFlags(int param, bool remove)
2958 { 2386 {
2959 if (!m_isSelected) 2387 m_vehicle.ProcessVehicleFlags(param, remove);
2960 { 2388 }
2961 if (m_isphysical && Body != IntPtr.Zero)
2962 {
2963 if (m_disabled)
2964 enableBodySoft();
2965 else if (!d.BodyIsEnabled(Body))
2966 d.BodyEnable(Body);
2967 2389
2968 } 2390 public override void SetVolumeDetect(int param)
2969 _torque = newtorque; 2391 {
2392 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
2393 // possible collisions with this prim or for none of them.
2394 lock (_parent_scene.OdeLock)
2395 {
2396 m_isVolumeDetect = (param != 0);
2970 } 2397 }
2971 } 2398 }
2972 2399
2973 private void changeForce(Vector3 force) 2400 public override Vector3 CenterOfMass
2974 { 2401 {
2975 m_force = force; 2402 get { return Vector3.Zero; }
2976 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2977 d.BodyEnable(Body);
2978 } 2403 }
2979 2404
2980 private void changeAddForce(Vector3 theforce) 2405 public override Vector3 GeometricCenter
2981 { 2406 {
2982 m_forceacc += theforce; 2407 get { return Vector3.Zero; }
2983 if (!m_isSelected)
2984 {
2985 lock (this)
2986 {
2987 //m_log.Info("[PHYSICS]: dequeing forcelist");
2988 if (m_isphysical && Body != IntPtr.Zero)
2989 {
2990 if (m_disabled)
2991 enableBodySoft();
2992 else if (!d.BodyIsEnabled(Body))
2993 d.BodyEnable(Body);
2994 }
2995 }
2996 m_collisionscore = 0;
2997 }
2998 } 2408 }
2999 2409
3000 // actually angular impulse 2410 public override PrimitiveBaseShape Shape
3001 private void changeAddAngularImpulse(Vector3 aimpulse)
3002 { 2411 {
3003 m_angularForceacc += aimpulse * m_invTimeStep; 2412 set
3004 if (!m_isSelected)
3005 { 2413 {
3006 lock (this) 2414 _pbs = value;
3007 { 2415 m_assetFailed = false;
3008 if (m_isphysical && Body != IntPtr.Zero) 2416 m_taintshape = true;
3009 {
3010 if (m_disabled)
3011 enableBodySoft();
3012 else if (!d.BodyIsEnabled(Body))
3013 d.BodyEnable(Body);
3014 }
3015 }
3016 m_collisionscore = 0;
3017 } 2417 }
3018 } 2418 }
3019 2419
3020 private void changevelocity(Vector3 newVel) 2420 public override Vector3 Velocity
3021 { 2421 {
3022 float len = newVel.LengthSquared(); 2422 get
3023 if (len > 100000.0f) // limit to 100m/s
3024 { 2423 {
3025 len = 100.0f / (float)Math.Sqrt(len); 2424 // Average previous velocity with the new one so
3026 newVel *= len; 2425 // client object interpolation works a 'little' better
3027 } 2426 if (_zeroFlag)
2427 return Vector3.Zero;
3028 2428
3029 if (!m_isSelected) 2429 Vector3 returnVelocity = Vector3.Zero;
2430 returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
2431 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
2432 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
2433 return returnVelocity;
2434 }
2435 set
3030 { 2436 {
3031 if (Body != IntPtr.Zero) 2437 if (value.IsFinite())
3032 { 2438 {
3033 if (m_disabled) 2439 _velocity = value;
3034 enableBodySoft();
3035 else if (!d.BodyIsEnabled(Body))
3036 d.BodyEnable(Body);
3037 2440
3038 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); 2441 m_taintVelocity = value;
2442 _parent_scene.AddPhysicsActorTaint(this);
2443 }
2444 else
2445 {
2446 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
3039 } 2447 }
3040 //resetCollisionAccounting(); 2448
3041 } 2449 }
3042 _velocity = newVel;
3043 } 2450 }
3044 2451
3045 private void changeangvelocity(Vector3 newAngVel) 2452 public override Vector3 Torque
3046 { 2453 {
3047 float len = newAngVel.LengthSquared(); 2454 get
3048 if (len > 144.0f) // limit to 12rad/s
3049 { 2455 {
3050 len = 12.0f / (float)Math.Sqrt(len); 2456 if (!IsPhysical || Body == IntPtr.Zero)
3051 newAngVel *= len; 2457 return Vector3.Zero;
2458
2459 return _torque;
3052 } 2460 }
3053 2461
3054 if (!m_isSelected) 2462 set
3055 { 2463 {
3056 if (Body != IntPtr.Zero) 2464 if (value.IsFinite())
3057 { 2465 {
3058 if (m_disabled) 2466 m_taintTorque = value;
3059 enableBodySoft(); 2467 _parent_scene.AddPhysicsActorTaint(this);
3060 else if (!d.BodyIsEnabled(Body)) 2468 }
3061 d.BodyEnable(Body); 2469 else
3062 2470 {
3063 2471 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
3064 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
3065 } 2472 }
3066 //resetCollisionAccounting();
3067 } 2473 }
3068 m_rotationalVelocity = newAngVel;
3069 } 2474 }
3070 2475
3071 private void changeVolumedetetion(bool newVolDtc) 2476 public override float CollisionScore
3072 {
3073 m_isVolumeDetect = newVolDtc;
3074 m_fakeisVolumeDetect = newVolDtc;
3075 UpdateCollisionCatFlags();
3076 ApplyCollisionCatFlags();
3077 }
3078
3079 protected void changeBuilding(bool newbuilding)
3080 { 2477 {
3081 // Check if we need to do anything 2478 get { return m_collisionscore; }
3082 if (newbuilding == m_building) 2479 set { m_collisionscore = value; }
3083 return;
3084
3085 if ((bool)newbuilding)
3086 {
3087 m_building = true;
3088 if (!childPrim)
3089 DestroyBody();
3090 }
3091 else
3092 {
3093 m_building = false;
3094 CheckDelaySelect();
3095 if (!childPrim)
3096 MakeBody();
3097 }
3098 if (!childPrim && childrenPrim.Count > 0)
3099 {
3100 foreach (OdePrim prm in childrenPrim)
3101 prm.changeBuilding(m_building); // call directly
3102 }
3103 } 2480 }
3104 2481
3105 public void changeSetVehicle(VehicleData vdata) 2482 public override bool Kinematic
3106 { 2483 {
3107 if (m_vehicle == null) 2484 get { return false; }
3108 m_vehicle = new ODEDynamics(this); 2485 set { }
3109 m_vehicle.DoSetVehicle(vdata);
3110 } 2486 }
3111 2487
3112 private void changeVehicleType(int value) 2488 public override Quaternion Orientation
3113 { 2489 {
3114 if (value == (int)Vehicle.TYPE_NONE) 2490 get { return _orientation; }
3115 { 2491 set
3116 if (m_vehicle != null)
3117 m_vehicle = null;
3118 }
3119 else
3120 { 2492 {
3121 if (m_vehicle == null) 2493 if (QuaternionIsFinite(value))
3122 m_vehicle = new ODEDynamics(this); 2494 _orientation = value;
3123 2495 else
3124 m_vehicle.ProcessTypeChange((Vehicle)value); 2496 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
3125 } 2497 }
3126 } 2498 }
3127 2499
3128 private void changeVehicleFloatParam(strVehicleFloatParam fp) 2500 private static bool QuaternionIsFinite(Quaternion q)
3129 { 2501 {
3130 if (m_vehicle == null) 2502 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3131 return; 2503 return false;
3132 2504 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3133 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); 2505 return false;
2506 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2507 return false;
2508 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2509 return false;
2510 return true;
3134 } 2511 }
3135 2512
3136 private void changeVehicleVectorParam(strVehicleVectorParam vp) 2513 public override Vector3 Acceleration
3137 { 2514 {
3138 if (m_vehicle == null) 2515 get { return _acceleration; }
3139 return; 2516 set { _acceleration = value; }
3140 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3141 } 2517 }
3142 2518
3143 private void changeVehicleRotationParam(strVehicleQuatParam qp) 2519 public override void AddForce(Vector3 force, bool pushforce)
3144 { 2520 {
3145 if (m_vehicle == null) 2521 if (force.IsFinite())
3146 return; 2522 {
3147 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); 2523 lock (m_forcelist)
3148 } 2524 m_forcelist.Add(force);
3149 2525
3150 private void changeVehicleFlags(strVehicleBoolParam bp) 2526 m_taintforce = true;
3151 { 2527 }
3152 if (m_vehicle == null) 2528 else
3153 return; 2529 {
3154 m_vehicle.ProcessVehicleFlags(bp.param, bp.value); 2530 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
2531 }
2532 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
3155 } 2533 }
3156 2534
3157 private void changeBuoyancy(float b) 2535 public override void AddAngularForce(Vector3 force, bool pushforce)
3158 { 2536 {
3159 m_buoyancy = b; 2537 if (force.IsFinite())
2538 {
2539 m_angularforcelist.Add(force);
2540 m_taintaddangularforce = true;
2541 }
2542 else
2543 {
2544 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2545 }
3160 } 2546 }
3161 2547
3162 private void changePIDTarget(Vector3 trg) 2548 public override Vector3 RotationalVelocity
3163 { 2549 {
3164 m_PIDTarget = trg; 2550 get
3165 } 2551 {
2552 Vector3 pv = Vector3.Zero;
2553 if (_zeroFlag)
2554 return pv;
2555 m_lastUpdateSent = false;
3166 2556
3167 private void changePIDTau(float tau) 2557 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
3168 { 2558 return pv;
3169 m_PIDTau = tau; 2559
2560 return m_rotationalVelocity;
2561 }
2562 set
2563 {
2564 if (value.IsFinite())
2565 {
2566 m_rotationalVelocity = value;
2567 }
2568 else
2569 {
2570 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2571 }
2572 }
3170 } 2573 }
3171 2574
3172 private void changePIDActive(bool val) 2575 public override void CrossingFailure()
3173 { 2576 {
3174 m_usePID = val; 2577 m_crossingfailures++;
2578 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2579 {
2580 base.RaiseOutOfBounds(_position);
2581 return;
2582 }
2583 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2584 {
2585 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2586 }
3175 } 2587 }
3176 2588
3177 private void changePIDHoverHeight(float val) 2589 public override float Buoyancy
3178 { 2590 {
3179 m_PIDHoverHeight = val; 2591 get { return m_buoyancy; }
3180 if (val == 0) 2592 set { m_buoyancy = value; }
3181 m_useHoverPID = false;
3182 } 2593 }
3183 2594
3184 private void changePIDHoverType(PIDHoverType type) 2595 public override void link(PhysicsActor obj)
3185 { 2596 {
3186 m_PIDHoverType = type; 2597 m_taintparent = obj;
3187 } 2598 }
3188 2599
3189 private void changePIDHoverTau(float tau) 2600 public override void delink()
3190 { 2601 {
3191 m_PIDHoverTau = tau; 2602 m_taintparent = null;
3192 } 2603 }
3193 2604
3194 private void changePIDHoverActive(bool active) 2605 public override void LockAngularMotion(Vector3 axis)
3195 { 2606 {
3196 m_useHoverPID = active; 2607 // reverse the zero/non zero values for ODE.
2608 if (axis.IsFinite())
2609 {
2610 axis.X = (axis.X > 0) ? 1f : 0f;
2611 axis.Y = (axis.Y > 0) ? 1f : 0f;
2612 axis.Z = (axis.Z > 0) ? 1f : 0f;
2613 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2614 m_taintAngularLock = axis;
2615 }
2616 else
2617 {
2618 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
2619 }
3197 } 2620 }
3198 2621
3199 #endregion 2622 internal void UpdatePositionAndVelocity()
3200
3201 public void Move()
3202 { 2623 {
3203 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 2624 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
3204 !m_disabled && !m_isSelected && !m_building && !m_outbounds) 2625 if (_parent == null)
3205 { 2626 {
3206 if (!d.BodyIsEnabled(Body)) 2627 Vector3 pv = Vector3.Zero;
3207 { 2628 bool lastZeroFlag = _zeroFlag;
3208 // let vehicles sleep 2629 float m_minvelocity = 0;
3209 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 2630 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
3210 return; 2631 {
3211 2632 d.Vector3 vec = d.BodyGetPosition(Body);
3212 if (++bodydisablecontrol < 20) 2633 d.Quaternion ori = d.BodyGetQuaternion(Body);
3213 return; 2634 d.Vector3 vel = d.BodyGetLinearVel(Body);
3214 2635 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3215 2636 d.Vector3 torque = d.BodyGetTorque(Body);
3216 d.BodyEnable(Body); 2637 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3217 } 2638 Vector3 l_position = Vector3.Zero;
3218 2639 Quaternion l_orientation = Quaternion.Identity;
3219 bodydisablecontrol = 0; 2640
3220 2641 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
3221 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator 2642 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3222 2643 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3223 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 2644 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3224 { 2645 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3225 // 'VEHICLES' are dealt with in ODEDynamics.cs 2646
3226 m_vehicle.Step(); 2647 m_lastposition = _position;
3227 return; 2648 m_lastorientation = _orientation;
3228 } 2649
3229 2650 l_position.X = vec.X;
3230 float fx = 0; 2651 l_position.Y = vec.Y;
3231 float fy = 0; 2652 l_position.Z = vec.Z;
3232 float fz = 0; 2653 l_orientation.X = ori.X;
3233 2654 l_orientation.Y = ori.Y;
3234 float m_mass = _mass; 2655 l_orientation.Z = ori.Z;
3235 2656 l_orientation.W = ori.W;
3236 if (m_usePID && m_PIDTau > 0) 2657
3237 { 2658 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
3238 // for now position error
3239 _target_velocity =
3240 new Vector3(
3241 (m_PIDTarget.X - lpos.X),
3242 (m_PIDTarget.Y - lpos.Y),
3243 (m_PIDTarget.Z - lpos.Z)
3244 );
3245
3246 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3247 {
3248 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3249 d.BodySetLinearVel(Body, 0, 0, 0);
3250 return;
3251 }
3252 else
3253 {
3254 _zeroFlag = false;
3255
3256 float tmp = 1 / m_PIDTau;
3257 _target_velocity *= tmp;
3258
3259 // apply limits
3260 tmp = _target_velocity.Length();
3261 if (tmp > 50.0f)
3262 {
3263 tmp = 50 / tmp;
3264 _target_velocity *= tmp;
3265 }
3266 else if (tmp < 0.05f)
3267 {
3268 tmp = 0.05f / tmp;
3269 _target_velocity *= tmp;
3270 }
3271
3272 d.Vector3 vel = d.BodyGetLinearVel(Body);
3273 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3274 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3275 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3276// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3277 }
3278 } // end if (m_usePID)
3279
3280 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3281 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3282 {
3283
3284 // Non-Vehicles have a limited set of Hover options.
3285 // determine what our target height really is based on HoverType
3286
3287 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3288
3289 switch (m_PIDHoverType)
3290 {
3291 case PIDHoverType.Ground:
3292 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3293 break;
3294
3295 case PIDHoverType.GroundAndWater:
3296 m_waterHeight = _parent_scene.GetWaterLevel();
3297 if (m_groundHeight > m_waterHeight)
3298 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3299 else
3300 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3301 break;
3302 } // end switch (m_PIDHoverType)
3303
3304 // don't go underground unless volumedetector
3305
3306 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3307 { 2659 {
3308 d.Vector3 vel = d.BodyGetLinearVel(Body); 2660 //base.RaiseOutOfBounds(l_position);
3309 2661
3310 fz = (m_targetHoverHeight - lpos.Z); 2662 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3311
3312 // if error is zero, use position control; otherwise, velocity control
3313 if (Math.Abs(fz) < 0.01f)
3314 { 2663 {
3315 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); 2664 _position = l_position;
3316 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 2665 //_parent_scene.remActivePrim(this);
2666 if (_parent == null)
2667 base.RequestPhysicsterseUpdate();
2668 return;
3317 } 2669 }
3318 else 2670 else
3319 { 2671 {
3320 _zeroFlag = false; 2672 if (_parent == null)
3321 fz /= m_PIDHoverTau; 2673 base.RaiseOutOfBounds(l_position);
3322 2674 return;
3323 float tmp = Math.Abs(fz);
3324 if (tmp > 50)
3325 fz = 50 * Math.Sign(fz);
3326 else if (tmp < 0.1)
3327 fz = 0.1f * Math.Sign(fz);
3328
3329 fz = ((fz - vel.Z) * m_invTimeStep);
3330 } 2675 }
3331 } 2676 }
3332 }
3333 else
3334 {
3335 float b = (1.0f - m_buoyancy);
3336 fx = _parent_scene.gravityx * b;
3337 fy = _parent_scene.gravityy * b;
3338 fz = _parent_scene.gravityz * b;
3339 }
3340
3341 fx *= m_mass;
3342 fy *= m_mass;
3343 fz *= m_mass;
3344
3345 // constant force
3346 fx += m_force.X;
3347 fy += m_force.Y;
3348 fz += m_force.Z;
3349
3350 fx += m_forceacc.X;
3351 fy += m_forceacc.Y;
3352 fz += m_forceacc.Z;
3353
3354 m_forceacc = Vector3.Zero;
3355
3356 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3357 if (fx != 0 || fy != 0 || fz != 0)
3358 {
3359 d.BodyAddForce(Body, fx, fy, fz);
3360 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3361 }
3362 2677
3363 Vector3 trq; 2678 if (l_position.Z < 0)
3364
3365 trq = _torque;
3366 trq += m_angularForceacc;
3367 m_angularForceacc = Vector3.Zero;
3368 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3369 {
3370 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3371 }
3372 }
3373 else
3374 { // is not physical, or is not a body or is selected
3375 // _zeroPosition = d.BodyGetPosition(Body);
3376 return;
3377 //Console.WriteLine("Nothing " + Name);
3378
3379 }
3380 }
3381
3382 public void UpdatePositionAndVelocity()
3383 {
3384 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3385 {
3386 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3387 {
3388 bool lastZeroFlag = _zeroFlag;
3389
3390 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
3391
3392 // check outside region
3393 if (lpos.Z < -100 || lpos.Z > 100000f)
3394 { 2679 {
3395 m_outbounds = true; 2680 // This is so prim that get lost underground don't fall forever and suck up
2681 //
2682 // Sim resources and memory.
2683 // Disables the prim's movement physics....
2684 // It's a hack and will generate a console message if it fails.
2685
2686 //IsPhysical = false;
2687 if (_parent == null)
2688 base.RaiseOutOfBounds(_position);
3396 2689
3397 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3398 _acceleration.X = 0; 2690 _acceleration.X = 0;
3399 _acceleration.Y = 0; 2691 _acceleration.Y = 0;
3400 _acceleration.Z = 0; 2692 _acceleration.Z = 0;
@@ -3406,437 +2698,589 @@ namespace OpenSim.Region.Physics.OdePlugin
3406 m_rotationalVelocity.Y = 0; 2698 m_rotationalVelocity.Y = 0;
3407 m_rotationalVelocity.Z = 0; 2699 m_rotationalVelocity.Z = 0;
3408 2700
3409 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 2701 if (_parent == null)
3410 d.BodySetAngularVel(Body, 0, 0, 0); // stop it 2702 base.RequestPhysicsterseUpdate();
3411 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3412 m_lastposition = _position;
3413 m_lastorientation = _orientation;
3414
3415 base.RequestPhysicsterseUpdate();
3416 2703
3417// throttleCounter = 0; 2704 m_throttleUpdates = false;
2705 throttleCounter = 0;
3418 _zeroFlag = true; 2706 _zeroFlag = true;
3419 2707 //outofBounds = true;
3420 disableBodySoft(); // disable it and colisions
3421 base.RaiseOutOfBounds(_position);
3422 return;
3423 } 2708 }
3424 2709
3425 if (lpos.X < 0f) 2710 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
3426 { 2711//Console.WriteLine("Adiff " + Name + " = " + Adiff);
3427 _position.X = Util.Clip(lpos.X, -2f, -0.1f); 2712 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3428 m_outbounds = true; 2713 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3429 } 2714 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3430 else if (lpos.X > _parent_scene.WorldExtents.X) 2715// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2716 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3431 { 2717 {
3432 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); 2718 _zeroFlag = true;
3433 m_outbounds = true; 2719//Console.WriteLine("ZFT 2");
3434 } 2720 m_throttleUpdates = false;
3435 if (lpos.Y < 0f)
3436 {
3437 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3438 m_outbounds = true;
3439 } 2721 }
3440 else if (lpos.Y > _parent_scene.WorldExtents.Y) 2722 else
3441 { 2723 {
3442 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); 2724 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3443 m_outbounds = true; 2725 _zeroFlag = false;
2726 m_lastUpdateSent = false;
2727 //m_throttleUpdates = false;
3444 } 2728 }
3445 2729
3446 if (m_outbounds) 2730 if (_zeroFlag)
3447 { 2731 {
3448 m_lastposition = _position; 2732 _velocity.X = 0.0f;
3449 m_lastorientation = _orientation; 2733 _velocity.Y = 0.0f;
3450 2734 _velocity.Z = 0.0f;
3451 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3452 m_rotationalVelocity.X = dtmp.X;
3453 m_rotationalVelocity.Y = dtmp.Y;
3454 m_rotationalVelocity.Z = dtmp.Z;
3455
3456 dtmp = d.BodyGetLinearVel(Body);
3457 _velocity.X = dtmp.X;
3458 _velocity.Y = dtmp.Y;
3459 _velocity.Z = dtmp.Z;
3460
3461 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3462 d.BodySetAngularVel(Body, 0, 0, 0);
3463 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3464 disableBodySoft(); // stop collisions
3465 UnSubscribeEvents();
3466
3467 base.RequestPhysicsterseUpdate();
3468 return;
3469 }
3470 2735
3471 d.Quaternion ori; 2736 _acceleration.X = 0;
3472 d.GeomCopyQuaternion(prim_geom, out ori); 2737 _acceleration.Y = 0;
3473 2738 _acceleration.Z = 0;
3474 // decide if moving 2739
3475 // use positions since this are integrated quantities 2740 //_orientation.w = 0f;
3476 // tolerance values depende a lot on simulation noise... 2741 //_orientation.X = 0f;
3477 // use simple math.abs since we dont need to be exact 2742 //_orientation.Y = 0f;
3478 2743 //_orientation.Z = 0f;
3479 if ( 2744 m_rotationalVelocity.X = 0;
3480 (Math.Abs(_position.X - lpos.X) < 0.001f) 2745 m_rotationalVelocity.Y = 0;
3481 && (Math.Abs(_position.Y - lpos.Y) < 0.001f) 2746 m_rotationalVelocity.Z = 0;
3482 && (Math.Abs(_position.Z - lpos.Z) < 0.001f) 2747 if (!m_lastUpdateSent)
3483 && (Math.Abs(_orientation.X - ori.X) < 0.0001f) 2748 {
3484 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) 2749 m_throttleUpdates = false;
3485 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W 2750 throttleCounter = 0;
3486 ) 2751 m_rotationalVelocity = pv;
3487 { 2752
3488 _zeroFlag = true; 2753 if (_parent == null)
2754 {
2755 base.RequestPhysicsterseUpdate();
2756 }
2757
2758 m_lastUpdateSent = true;
2759 }
3489 } 2760 }
3490 else 2761 else
3491 _zeroFlag = false;
3492
3493 // update velocities and aceleration
3494 if (!(_zeroFlag && lastZeroFlag))
3495 { 2762 {
3496 d.Vector3 vel = d.BodyGetLinearVel(Body); 2763 if (lastZeroFlag != _zeroFlag)
2764 {
2765 if (_parent == null)
2766 {
2767 base.RequestPhysicsterseUpdate();
2768 }
2769 }
2770
2771 m_lastVelocity = _velocity;
3497 2772
3498 _acceleration = _velocity; 2773 _position = l_position;
3499 2774
3500 if ((Math.Abs(vel.X) < 0.001f) && 2775 _velocity.X = vel.X;
3501 (Math.Abs(vel.Y) < 0.001f) && 2776 _velocity.Y = vel.Y;
3502 (Math.Abs(vel.Z) < 0.001f)) 2777 _velocity.Z = vel.Z;
2778
2779 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2780 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2781 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2782
2783 // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
2784 // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
2785 // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
2786 // adding these logical exclusion situations to maintain this where I think it was intended to be.
2787 if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
3503 { 2788 {
3504 _velocity = Vector3.Zero; 2789 m_minvelocity = 0.5f;
3505 float t = -m_invTimeStep;
3506 _acceleration = _acceleration * t;
3507 } 2790 }
3508 else 2791 else
3509 { 2792 {
3510 _velocity.X = vel.X; 2793 m_minvelocity = 0.02f;
3511 _velocity.Y = vel.Y;
3512 _velocity.Z = vel.Z;
3513 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3514 }
3515
3516 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3517 (Math.Abs(_acceleration.Y) < 0.01f) &&
3518 (Math.Abs(_acceleration.Z) < 0.01f))
3519 {
3520 _acceleration = Vector3.Zero;
3521 } 2794 }
3522 2795
3523 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && 2796 if (_velocity.ApproxEquals(pv, m_minvelocity))
3524 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3525 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3526 )
3527 { 2797 {
3528 m_rotationalVelocity = Vector3.Zero; 2798 m_rotationalVelocity = pv;
3529 } 2799 }
3530 else 2800 else
3531 { 2801 {
3532 vel = d.BodyGetAngularVel(Body); 2802 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
3533 m_rotationalVelocity.X = vel.X;
3534 m_rotationalVelocity.Y = vel.Y;
3535 m_rotationalVelocity.Z = vel.Z;
3536 } 2803 }
3537 }
3538 2804
3539 if (_zeroFlag) 2805 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
3540 { 2806 _orientation.X = ori.X;
3541 if (lastZeroFlag) 2807 _orientation.Y = ori.Y;
2808 _orientation.Z = ori.Z;
2809 _orientation.W = ori.W;
2810 m_lastUpdateSent = false;
2811 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3542 { 2812 {
3543 _velocity = Vector3.Zero; 2813 if (_parent == null)
3544 _acceleration = Vector3.Zero; 2814 {
3545 m_rotationalVelocity = Vector3.Zero; 2815 base.RequestPhysicsterseUpdate();
2816 }
3546 } 2817 }
3547 2818 else
3548 if (!m_lastUpdateSent)
3549 { 2819 {
3550 base.RequestPhysicsterseUpdate(); 2820 throttleCounter++;
3551 if (lastZeroFlag)
3552 m_lastUpdateSent = true;
3553 } 2821 }
3554 return;
3555 } 2822 }
2823 m_lastposition = l_position;
2824 }
2825 else
2826 {
2827 // Not a body.. so Make sure the client isn't interpolating
2828 _velocity.X = 0;
2829 _velocity.Y = 0;
2830 _velocity.Z = 0;
3556 2831
3557 _position.X = lpos.X; 2832 _acceleration.X = 0;
3558 _position.Y = lpos.Y; 2833 _acceleration.Y = 0;
3559 _position.Z = lpos.Z; 2834 _acceleration.Z = 0;
3560 2835
3561 _orientation.X = ori.X; 2836 m_rotationalVelocity.X = 0;
3562 _orientation.Y = ori.Y; 2837 m_rotationalVelocity.Y = 0;
3563 _orientation.Z = ori.Z; 2838 m_rotationalVelocity.Z = 0;
3564 _orientation.W = ori.W; 2839 _zeroFlag = true;
3565 base.RequestPhysicsterseUpdate();
3566 m_lastUpdateSent = false;
3567 } 2840 }
3568 } 2841 }
3569 } 2842 }
3570 2843
3571 internal static bool QuaternionIsFinite(Quaternion q) 2844 public override bool FloatOnWater
3572 { 2845 {
3573 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) 2846 set {
3574 return false; 2847 m_taintCollidesWater = value;
3575 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) 2848 _parent_scene.AddPhysicsActorTaint(this);
3576 return false; 2849 }
3577 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3578 return false;
3579 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3580 return false;
3581 return true;
3582 } 2850 }
3583 2851
3584 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) 2852 public override void SetMomentum(Vector3 momentum)
3585 { 2853 {
3586 // assumes object center of mass is zero 2854 }
3587 float smass = part.mass; 2855
3588 theobj.mass -= smass; 2856 public override Vector3 PIDTarget
2857 {
2858 set
2859 {
2860 if (value.IsFinite())
2861 {
2862 m_PIDTarget = value;
2863 }
2864 else
2865 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
2866 }
2867 }
2868 public override bool PIDActive { set { m_usePID = value; } }
2869 public override float PIDTau { set { m_PIDTau = value; } }
2870
2871 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2872 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2873 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2874 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2875
2876 public override Quaternion APIDTarget{ set { return; } }
3589 2877
3590 smass *= 1.0f / (theobj.mass); ; 2878 public override bool APIDActive{ set { return; } }
3591 2879
3592 theobj.c.X -= part.c.X * smass; 2880 public override float APIDStrength{ set { return; } }
3593 theobj.c.Y -= part.c.Y * smass;
3594 theobj.c.Z -= part.c.Z * smass;
3595 2881
3596 theobj.I.M00 -= part.I.M00; 2882 public override float APIDDamping{ set { return; } }
3597 theobj.I.M01 -= part.I.M01;
3598 theobj.I.M02 -= part.I.M02;
3599 theobj.I.M10 -= part.I.M10;
3600 theobj.I.M11 -= part.I.M11;
3601 theobj.I.M12 -= part.I.M12;
3602 theobj.I.M20 -= part.I.M20;
3603 theobj.I.M21 -= part.I.M21;
3604 theobj.I.M22 -= part.I.M22;
3605 }
3606 2883
3607 private void donullchange() 2884 private void createAMotor(Vector3 axis)
3608 { 2885 {
3609 } 2886 if (Body == IntPtr.Zero)
2887 return;
3610 2888
3611 public bool DoAChange(changes what, object arg) 2889 if (Amotor != IntPtr.Zero)
3612 {
3613 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
3614 { 2890 {
3615 return false; 2891 d.JointDestroy(Amotor);
2892 Amotor = IntPtr.Zero;
3616 } 2893 }
3617 2894
3618 // nasty switch 2895 float axisnum = 3;
3619 switch (what)
3620 {
3621 case changes.Add:
3622 changeadd();
3623 break;
3624
3625 case changes.AddPhysRep:
3626 changeAddPhysRep((ODEPhysRepData)arg);
3627 break;
3628
3629 case changes.Remove:
3630 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3631 //When we return true, it destroys all of the prims in the linkset anyway
3632 if (_parent != null)
3633 {
3634 OdePrim parent = (OdePrim)_parent;
3635 parent.ChildRemove(this, false);
3636 }
3637 else
3638 ChildRemove(this, false);
3639 2896
3640 m_vehicle = null; 2897 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
3641 RemoveGeom();
3642 m_targetSpace = IntPtr.Zero;
3643 UnSubscribeEvents();
3644 return true;
3645 2898
3646 case changes.Link: 2899 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
3647 OdePrim tmp = (OdePrim)arg;
3648 changeLink(tmp);
3649 break;
3650 2900
3651 case changes.DeLink: 2901
3652 changeLink(null); 2902 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
3653 break; 2903 d.Mass objMass;
2904 d.MassSetZero(out objMass);
2905 DMassCopy(ref pMass, ref objMass);
3654 2906
3655 case changes.Position: 2907 //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
3656 changePosition((Vector3)arg);
3657 break;
3658 2908
3659 case changes.Orientation: 2909 Matrix4 dMassMat = FromDMass(objMass);
3660 changeOrientation((Quaternion)arg);
3661 break;
3662 2910
3663 case changes.PosOffset: 2911 Matrix4 mathmat = Inverse(dMassMat);
3664 donullchange();
3665 break;
3666 2912
3667 case changes.OriOffset: 2913 /*
3668 donullchange(); 2914 //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
3669 break;
3670 2915
3671 case changes.Velocity: 2916 mathmat = Inverse(mathmat);
3672 changevelocity((Vector3)arg);
3673 break;
3674 2917
3675// case changes.Acceleration:
3676// changeacceleration((Vector3)arg);
3677// break;
3678 2918
3679 case changes.AngVelocity: 2919 objMass = FromMatrix4(mathmat, ref objMass);
3680 changeangvelocity((Vector3)arg); 2920 //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
3681 break;
3682 2921
3683 case changes.Force: 2922 mathmat = Inverse(mathmat);
3684 changeForce((Vector3)arg); 2923 */
3685 break; 2924 if (axis.X == 0)
2925 {
2926 mathmat.M33 = 50.0000001f;
2927 //objMass.I.M22 = 0;
2928 }
2929 if (axis.Y == 0)
2930 {
2931 mathmat.M22 = 50.0000001f;
2932 //objMass.I.M11 = 0;
2933 }
2934 if (axis.Z == 0)
2935 {
2936 mathmat.M11 = 50.0000001f;
2937 //objMass.I.M00 = 0;
2938 }
2939
2940
3686 2941
3687 case changes.Torque: 2942 mathmat = Inverse(mathmat);
3688 changeSetTorque((Vector3)arg); 2943 objMass = FromMatrix4(mathmat, ref objMass);
3689 break; 2944 //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2945
2946 //return;
2947 if (d.MassCheck(ref objMass))
2948 {
2949 d.BodySetMass(Body, ref objMass);
2950 }
2951 else
2952 {
2953 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
2954 }
3690 2955
3691 case changes.AddForce: 2956 if (axisnum <= 0)
3692 changeAddForce((Vector3)arg); 2957 return;
3693 break; 2958 // int dAMotorEuler = 1;
3694 2959
3695 case changes.AddAngForce: 2960 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3696 changeAddAngularImpulse((Vector3)arg); 2961 d.JointAttach(Amotor, Body, IntPtr.Zero);
3697 break; 2962 d.JointSetAMotorMode(Amotor, 0);
3698 2963
3699 case changes.AngLock: 2964 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3700 changeAngularLock((Vector3)arg); 2965 int i = 0;
3701 break;
3702 2966
3703 case changes.Size: 2967 if (axis.X == 0)
3704 changeSize((Vector3)arg); 2968 {
3705 break; 2969 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
2970 i++;
2971 }
3706 2972
3707 case changes.Shape: 2973 if (axis.Y == 0)
3708 changeShape((PrimitiveBaseShape)arg); 2974 {
3709 break; 2975 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
2976 i++;
2977 }
3710 2978
3711 case changes.PhysRepData: 2979 if (axis.Z == 0)
3712 changePhysRepData((ODEPhysRepData) arg); 2980 {
3713 break; 2981 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
2982 i++;
2983 }
3714 2984
3715 case changes.CollidesWater: 2985 for (int j = 0; j < (int)axisnum; j++)
3716 changeFloatOnWater((bool)arg); 2986 {
3717 break; 2987 //d.JointSetAMotorAngle(Amotor, j, 0);
2988 }
3718 2989
3719 case changes.VolumeDtc: 2990 //d.JointSetAMotorAngle(Amotor, 1, 0);
3720 changeVolumedetetion((bool)arg); 2991 //d.JointSetAMotorAngle(Amotor, 2, 0);
3721 break;
3722 2992
3723 case changes.Phantom: 2993 // These lowstops and high stops are effectively (no wiggle room)
3724 changePhantomStatus((bool)arg); 2994 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3725 break; 2995 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
2996 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
2997 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
2998 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
2999 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3000 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3001 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3002 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3003 }
3726 3004
3727 case changes.Physical: 3005 private Matrix4 FromDMass(d.Mass pMass)
3728 changePhysicsStatus((bool)arg); 3006 {
3729 break; 3007 Matrix4 obj;
3008 obj.M11 = pMass.I.M00;
3009 obj.M12 = pMass.I.M01;
3010 obj.M13 = pMass.I.M02;
3011 obj.M14 = 0;
3012 obj.M21 = pMass.I.M10;
3013 obj.M22 = pMass.I.M11;
3014 obj.M23 = pMass.I.M12;
3015 obj.M24 = 0;
3016 obj.M31 = pMass.I.M20;
3017 obj.M32 = pMass.I.M21;
3018 obj.M33 = pMass.I.M22;
3019 obj.M34 = 0;
3020 obj.M41 = 0;
3021 obj.M42 = 0;
3022 obj.M43 = 0;
3023 obj.M44 = 1;
3024 return obj;
3025 }
3730 3026
3731 case changes.Selected: 3027 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3732 changeSelectedStatus((bool)arg); 3028 {
3733 break; 3029 obj.I.M00 = pMat[0, 0];
3030 obj.I.M01 = pMat[0, 1];
3031 obj.I.M02 = pMat[0, 2];
3032 obj.I.M10 = pMat[1, 0];
3033 obj.I.M11 = pMat[1, 1];
3034 obj.I.M12 = pMat[1, 2];
3035 obj.I.M20 = pMat[2, 0];
3036 obj.I.M21 = pMat[2, 1];
3037 obj.I.M22 = pMat[2, 2];
3038 return obj;
3039 }
3734 3040
3735 case changes.disabled: 3041 public override void SubscribeEvents(int ms)
3736 changeDisable((bool)arg); 3042 {
3737 break; 3043 m_eventsubscription = ms;
3044 _parent_scene.AddCollisionEventReporting(this);
3045 }
3738 3046
3739 case changes.building: 3047 public override void UnSubscribeEvents()
3740 changeBuilding((bool)arg); 3048 {
3741 break; 3049 _parent_scene.RemoveCollisionEventReporting(this);
3050 m_eventsubscription = 0;
3051 }
3742 3052
3743 case changes.VehicleType: 3053 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3744 changeVehicleType((int)arg); 3054 {
3745 break; 3055 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
3056 }
3746 3057
3747 case changes.VehicleFlags: 3058 public void SendCollisions()
3748 changeVehicleFlags((strVehicleBoolParam) arg); 3059 {
3749 break; 3060 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
3061 {
3062 base.SendCollisionUpdate(CollisionEventsThisFrame);
3750 3063
3751 case changes.VehicleFloatParam: 3064 if (CollisionEventsThisFrame.Count > 0)
3752 changeVehicleFloatParam((strVehicleFloatParam) arg); 3065 {
3753 break; 3066 m_collisionsOnPreviousFrame = true;
3067 CollisionEventsThisFrame.Clear();
3068 }
3069 else
3070 {
3071 m_collisionsOnPreviousFrame = false;
3072 }
3073 }
3074 }
3754 3075
3755 case changes.VehicleVectorParam: 3076 public override bool SubscribedEvents()
3756 changeVehicleVectorParam((strVehicleVectorParam) arg); 3077 {
3757 break; 3078 if (m_eventsubscription > 0)
3079 return true;
3080 return false;
3081 }
3758 3082
3759 case changes.VehicleRotationParam: 3083 public static Matrix4 Inverse(Matrix4 pMat)
3760 changeVehicleRotationParam((strVehicleQuatParam) arg); 3084 {
3761 break; 3085 if (determinant3x3(pMat) == 0)
3086 {
3087 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3088 }
3762 3089
3763 case changes.SetVehicle: 3090 return (Adjoint(pMat) / determinant3x3(pMat));
3764 changeSetVehicle((VehicleData) arg); 3091 }
3765 break;
3766 3092
3767 case changes.Buoyancy: 3093 public static Matrix4 Adjoint(Matrix4 pMat)
3768 changeBuoyancy((float)arg); 3094 {
3769 break; 3095 Matrix4 adjointMatrix = new Matrix4();
3096 for (int i=0; i<4; i++)
3097 {
3098 for (int j=0; j<4; j++)
3099 {
3100 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3101 }
3102 }
3770 3103
3771 case changes.PIDTarget: 3104 adjointMatrix = Transpose(adjointMatrix);
3772 changePIDTarget((Vector3)arg); 3105 return adjointMatrix;
3773 break; 3106 }
3774 3107
3775 case changes.PIDTau: 3108 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3776 changePIDTau((float)arg); 3109 {
3777 break; 3110 Matrix4 minor = new Matrix4();
3111 int m = 0, n = 0;
3112 for (int i = 0; i < 4; i++)
3113 {
3114 if (i == iRow)
3115 continue;
3116 n = 0;
3117 for (int j = 0; j < 4; j++)
3118 {
3119 if (j == iCol)
3120 continue;
3121 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3122 n++;
3123 }
3124 m++;
3125 }
3778 3126
3779 case changes.PIDActive: 3127 return minor;
3780 changePIDActive((bool)arg); 3128 }
3781 break;
3782 3129
3783 case changes.PIDHoverHeight: 3130 public static Matrix4 Transpose(Matrix4 pMat)
3784 changePIDHoverHeight((float)arg); 3131 {
3785 break; 3132 Matrix4 transposeMatrix = new Matrix4();
3133 for (int i = 0; i < 4; i++)
3134 for (int j = 0; j < 4; j++)
3135 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3136 return transposeMatrix;
3137 }
3786 3138
3787 case changes.PIDHoverType: 3139 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3788 changePIDHoverType((PIDHoverType)arg); 3140 {
3789 break; 3141 switch (r)
3142 {
3143 case 0:
3144 switch (c)
3145 {
3146 case 0:
3147 pMat.M11 = val;
3148 break;
3149 case 1:
3150 pMat.M12 = val;
3151 break;
3152 case 2:
3153 pMat.M13 = val;
3154 break;
3155 case 3:
3156 pMat.M14 = val;
3157 break;
3158 }
3790 3159
3791 case changes.PIDHoverTau:
3792 changePIDHoverTau((float)arg);
3793 break; 3160 break;
3161 case 1:
3162 switch (c)
3163 {
3164 case 0:
3165 pMat.M21 = val;
3166 break;
3167 case 1:
3168 pMat.M22 = val;
3169 break;
3170 case 2:
3171 pMat.M23 = val;
3172 break;
3173 case 3:
3174 pMat.M24 = val;
3175 break;
3176 }
3794 3177
3795 case changes.PIDHoverActive:
3796 changePIDHoverActive((bool)arg);
3797 break; 3178 break;
3179 case 2:
3180 switch (c)
3181 {
3182 case 0:
3183 pMat.M31 = val;
3184 break;
3185 case 1:
3186 pMat.M32 = val;
3187 break;
3188 case 2:
3189 pMat.M33 = val;
3190 break;
3191 case 3:
3192 pMat.M34 = val;
3193 break;
3194 }
3798 3195
3799 case changes.Null:
3800 donullchange();
3801 break; 3196 break;
3197 case 3:
3198 switch (c)
3199 {
3200 case 0:
3201 pMat.M41 = val;
3202 break;
3203 case 1:
3204 pMat.M42 = val;
3205 break;
3206 case 2:
3207 pMat.M43 = val;
3208 break;
3209 case 3:
3210 pMat.M44 = val;
3211 break;
3212 }
3802 3213
3803
3804
3805 default:
3806 donullchange();
3807 break; 3214 break;
3808 } 3215 }
3809 return false;
3810 } 3216 }
3811 3217
3812 public void AddChange(changes what, object arg) 3218 private static float determinant3x3(Matrix4 pMat)
3813 { 3219 {
3814 _parent_scene.AddChange((PhysicsActor) this, what, arg); 3220 float det = 0;
3815 } 3221 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3222 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3223 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3224 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3225 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3226 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3816 3227
3817 3228 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3818 private struct strVehicleBoolParam 3229 return det;
3819 { 3230 }
3820 public int param; 3231
3821 public bool value; 3232 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3233 {
3234 dst.c.W = src.c.W;
3235 dst.c.X = src.c.X;
3236 dst.c.Y = src.c.Y;
3237 dst.c.Z = src.c.Z;
3238 dst.mass = src.mass;
3239 dst.I.M00 = src.I.M00;
3240 dst.I.M01 = src.I.M01;
3241 dst.I.M02 = src.I.M02;
3242 dst.I.M10 = src.I.M10;
3243 dst.I.M11 = src.I.M11;
3244 dst.I.M12 = src.I.M12;
3245 dst.I.M20 = src.I.M20;
3246 dst.I.M21 = src.I.M21;
3247 dst.I.M22 = src.I.M22;
3822 } 3248 }
3823 3249
3824 private struct strVehicleFloatParam 3250 public override void SetMaterial(int pMaterial)
3825 { 3251 {
3826 public int param; 3252 m_material = pMaterial;
3827 public float value;
3828 } 3253 }
3829 3254
3830 private struct strVehicleQuatParam 3255 private void CheckMeshAsset()
3831 { 3256 {
3832 public int param; 3257 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
3833 public Quaternion value; 3258 {
3259 m_assetFailed = true;
3260 Util.FireAndForget(delegate
3261 {
3262 RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
3263 if (assetProvider != null)
3264 assetProvider(_pbs.SculptTexture, MeshAssetReveived);
3265 });
3266 }
3834 } 3267 }
3835 3268
3836 private struct strVehicleVectorParam 3269 void MeshAssetReveived(AssetBase asset)
3837 { 3270 {
3838 public int param; 3271 if (asset.Data != null && asset.Data.Length > 0)
3839 public Vector3 value; 3272 {
3840 } 3273 if (!_pbs.SculptEntry)
3274 return;
3275 if (_pbs.SculptTexture.ToString() != asset.ID)
3276 return;
3277
3278 _pbs.SculptData = new byte[asset.Data.Length];
3279 asset.Data.CopyTo(_pbs.SculptData, 0);
3280 m_assetFailed = false;
3281 m_taintshape = true;
3282 _parent_scene.AddPhysicsActorTaint(this);
3283 }
3284 }
3841 } 3285 }
3842} 3286} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 03048a4..7a50c4c 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -25,21 +25,26 @@
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28//#define USE_DRAWSTUFF
28//#define SPAM 29//#define SPAM
29 30
30using System; 31using System;
31using System.Collections.Generic; 32using System.Collections.Generic;
33using System.Diagnostics;
34using System.IO;
35using System.Linq;
32using System.Reflection; 36using System.Reflection;
33using System.Runtime.InteropServices; 37using System.Runtime.InteropServices;
34using System.Threading; 38using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net; 39using log4net;
38using Nini.Config; 40using Nini.Config;
39using OdeAPI; 41using Ode.NET;
42using OpenMetaverse;
43#if USE_DRAWSTUFF
44using Drawstuff.NET;
45#endif
40using OpenSim.Framework; 46using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager; 47using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43 48
44namespace OpenSim.Region.Physics.OdePlugin 49namespace OpenSim.Region.Physics.OdePlugin
45{ 50{
@@ -50,42 +55,29 @@ namespace OpenSim.Region.Physics.OdePlugin
50 End = 2 55 End = 2
51 } 56 }
52 57
53 public struct sCollisionData 58// public struct sCollisionData
54 { 59// {
55 public uint ColliderLocalId; 60// public uint ColliderLocalId;
56 public uint CollidedWithLocalId; 61// public uint CollidedWithLocalId;
57 public int NumberOfCollisions; 62// public int NumberOfCollisions;
58 public int CollisionType; 63// public int CollisionType;
59 public int StatusIndicator; 64// public int StatusIndicator;
60 public int lastframe; 65// public int lastframe;
61 } 66// }
62
63
64 // colision flags of things others can colide with
65 // rays, sensors, probes removed since can't be colided with
66 // The top space where things are placed provided further selection
67 // ie physical are in active space nonphysical in static
68 // this should be exclusive as possible
69 67
70 [Flags] 68 [Flags]
71 public enum CollisionCategories : uint 69 public enum CollisionCategories : int
72 { 70 {
73 Disabled = 0, 71 Disabled = 0,
74 //by 'things' types 72 Geom = 0x00000001,
75 Space = 0x01, 73 Body = 0x00000002,
76 Geom = 0x02, // aka prim/part 74 Space = 0x00000004,
77 Character = 0x04, 75 Character = 0x00000008,
78 Land = 0x08, 76 Land = 0x00000010,
79 Water = 0x010, 77 Water = 0x00000020,
80 78 Wind = 0x00000040,
81 // by state 79 Sensor = 0x00000080,
82 Phantom = 0x01000, 80 Selected = 0x00000100
83 VolumeDtc = 0x02000,
84 Selected = 0x04000,
85 NoShape = 0x08000,
86
87
88 All = 0xffffffff
89 } 81 }
90 82
91 /// <summary> 83 /// <summary>
@@ -106,213 +98,400 @@ namespace OpenSim.Region.Physics.OdePlugin
106 /// <summary></summary> 98 /// <summary></summary>
107 Plastic = 5, 99 Plastic = 5,
108 /// <summary></summary> 100 /// <summary></summary>
109 Rubber = 6, 101 Rubber = 6
110
111 light = 7 // compatibility with old viewers
112 }
113
114 public enum changes : int
115 {
116 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
117 Remove,
118 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
119 // or removes from a object if arg is null
120 DeLink,
121 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
122 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
123 PosOffset, // not in use
124 // arg Vector3 new position in local coords. Changes prim position in object
125 OriOffset, // not in use
126 // arg Vector3 new position in local coords. Changes prim position in object
127 Velocity,
128 AngVelocity,
129 Acceleration,
130 Force,
131 Torque,
132 Momentum,
133
134 AddForce,
135 AddAngForce,
136 AngLock,
137
138 Buoyancy,
139
140 PIDTarget,
141 PIDTau,
142 PIDActive,
143
144 PIDHoverHeight,
145 PIDHoverType,
146 PIDHoverTau,
147 PIDHoverActive,
148
149 Size,
150 Shape,
151 PhysRepData,
152 AddPhysRep,
153
154 CollidesWater,
155 VolumeDtc,
156
157 Physical,
158 Phantom,
159 Selected,
160 disabled,
161 building,
162
163 VehicleType,
164 VehicleFloatParam,
165 VehicleVectorParam,
166 VehicleRotationParam,
167 VehicleFlags,
168 SetVehicle,
169
170 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
171 } 102 }
172 103
173 public struct ODEchangeitem
174 {
175 public PhysicsActor actor;
176 public OdeCharacter character;
177 public changes what;
178 public Object arg;
179 }
180
181 public class OdeScene : PhysicsScene 104 public class OdeScene : PhysicsScene
182 { 105 {
183 private readonly ILog m_log; 106 private readonly ILog m_log;
184 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); 107 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
185 108
186 public bool OdeUbitLib = false; 109 /// <summary>
187// private int threadid = 0; 110 /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances.
188 private Random fluidRandomizer = new Random(Environment.TickCount); 111 /// </summary>
112 /// <remarks>
113 /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
114 /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts
115 /// uses a static cache at the ODE level.
116 ///
117 /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
118 /// to
119 ///
120 /// mono() [0x489171]
121 /// mono() [0x4d154f]
122 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
123 /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
124 ///
125 /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
126 /// causes OpenSimulator to immediately crash with a native stack trace similar to
127 ///
128 /// mono() [0x489171]
129 /// mono() [0x4d154f]
130 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
131 /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
132 /// </remarks>
133 internal static Object UniversalColliderSyncObject = new Object();
134
135 /// <summary>
136 /// Is stats collecting enabled for this ODE scene?
137 /// </summary>
138 public bool CollectStats { get; set; }
139
140 /// <summary>
141 /// Statistics for this scene.
142 /// </summary>
143 private Dictionary<string, float> m_stats = new Dictionary<string, float>();
144
145 /// <summary>
146 /// Stat name for total number of avatars in this ODE scene.
147 /// </summary>
148 public const string ODETotalAvatarsStatName = "ODETotalAvatars";
149
150 /// <summary>
151 /// Stat name for total number of prims in this ODE scene.
152 /// </summary>
153 public const string ODETotalPrimsStatName = "ODETotalPrims";
154
155 /// <summary>
156 /// Stat name for total number of prims with active physics in this ODE scene.
157 /// </summary>
158 public const string ODEActivePrimsStatName = "ODEActivePrims";
159
160 /// <summary>
161 /// Stat name for the total time spent in ODE frame processing.
162 /// </summary>
163 /// <remarks>
164 /// A sanity check for the main scene loop physics time.
165 /// </remarks>
166 public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
167
168 /// <summary>
169 /// Stat name for time spent processing avatar taints per frame
170 /// </summary>
171 public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
172
173 /// <summary>
174 /// Stat name for time spent processing prim taints per frame
175 /// </summary>
176 public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
177
178 /// <summary>
179 /// Stat name for time spent calculating avatar forces per frame.
180 /// </summary>
181 public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
182
183 /// <summary>
184 /// Stat name for time spent calculating prim forces per frame
185 /// </summary>
186 public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
187
188 /// <summary>
189 /// Stat name for time spent fulfilling raycasting requests per frame
190 /// </summary>
191 public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
192
193 /// <summary>
194 /// Stat name for time spent in native code that actually steps through the simulation.
195 /// </summary>
196 public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
197
198 /// <summary>
199 /// Stat name for the number of milliseconds that ODE spends in native space collision code.
200 /// </summary>
201 public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
202
203 /// <summary>
204 /// Stat name for milliseconds that ODE spends in native geom collision code.
205 /// </summary>
206 public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
207
208 /// <summary>
209 /// Time spent in collision processing that is not spent in native space or geom collision code.
210 /// </summary>
211 public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
212
213 /// <summary>
214 /// Stat name for time spent notifying listeners of collisions
215 /// </summary>
216 public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
217
218 /// <summary>
219 /// Stat name for milliseconds spent updating avatar position and velocity
220 /// </summary>
221 public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
222
223 /// <summary>
224 /// Stat name for the milliseconds spent updating prim position and velocity
225 /// </summary>
226 public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
227
228 /// <summary>
229 /// Stat name for avatar collisions with another entity.
230 /// </summary>
231 public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
232
233 /// <summary>
234 /// Stat name for prim collisions with another entity.
235 /// </summary>
236 public const string ODEPrimContactsStatName = "ODEPrimContacts";
237
238 /// <summary>
239 /// Used to hold tick numbers for stat collection purposes.
240 /// </summary>
241 private int m_nativeCollisionStartTick;
242
243 /// <summary>
244 /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
245 /// </summary>
246 private bool m_inCollisionTiming;
247
248 /// <summary>
249 /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
250 /// collisions occured using the _perloopcontact if stats collection is enabled.
251 /// </summary>
252 private int m_tempAvatarCollisionsThisFrame;
189 253
190 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; 254 /// <summary>
191 const float MaxERP = 0.8f; 255 /// Used in calculating physics frame time dilation
192 const float minERP = 0.1f; 256 /// </summary>
193 const float comumContactCFM = 0.0001f; 257 private int tickCountFrameRun;
194
195 float frictionMovementMult = 0.8f;
196 258
197 float TerrainBounce = 0.1f; 259 /// <summary>
198 float TerrainFriction = 0.3f; 260 /// Used in calculating physics frame time dilation
261 /// </summary>
262 private int latertickcount;
199 263
200 public float AvatarFriction = 0;// 0.9f * 0.5f; 264 private Random fluidRandomizer = new Random(Environment.TickCount);
201 265
202 private const uint m_regionWidth = Constants.RegionSize; 266 private const uint m_regionWidth = Constants.RegionSize;
203 private const uint m_regionHeight = Constants.RegionSize; 267 private const uint m_regionHeight = Constants.RegionSize;
204 268
205 public float ODE_STEPSIZE = 0.020f; 269 private float ODE_STEPSIZE = 0.0178f;
206 public float HalfOdeStep = 0.01f; 270 private float metersInSpace = 29.9f;
207 public int odetimestepMS = 20; // rounded
208 private float metersInSpace = 25.6f;
209 private float m_timeDilation = 1.0f; 271 private float m_timeDilation = 1.0f;
210 272
211 private DateTime m_lastframe;
212 private DateTime m_lastMeshExpire;
213
214 public float gravityx = 0f; 273 public float gravityx = 0f;
215 public float gravityy = 0f; 274 public float gravityy = 0f;
216 public float gravityz = -9.8f; 275 public float gravityz = -9.8f;
217 276
277 public float AvatarTerminalVelocity { get; set; }
278
279 private float contactsurfacelayer = 0.001f;
280
281 private int worldHashspaceLow = -4;
282 private int worldHashspaceHigh = 128;
283
284 private int smallHashspaceLow = -4;
285 private int smallHashspaceHigh = 66;
286
218 private float waterlevel = 0f; 287 private float waterlevel = 0f;
219 private int framecount = 0; 288 private int framecount = 0;
289 //private int m_returncollisions = 10;
290
291 private readonly IntPtr contactgroup;
292
293 internal IntPtr WaterGeom;
220 294
221 private int m_meshExpireCntr; 295 private float nmTerrainContactFriction = 255.0f;
296 private float nmTerrainContactBounce = 0.1f;
297 private float nmTerrainContactERP = 0.1025f;
222 298
223// private IntPtr WaterGeom = IntPtr.Zero; 299 private float mTerrainContactFriction = 75f;
224// private IntPtr WaterHeightmapData = IntPtr.Zero; 300 private float mTerrainContactBounce = 0.1f;
225// private GCHandle WaterMapHandler = new GCHandle(); 301 private float mTerrainContactERP = 0.05025f;
226 302
227 public float avPIDD = 2200f; // make it visible 303 private float nmAvatarObjectContactFriction = 250f;
228 public float avPIDP = 900f; // make it visible 304 private float nmAvatarObjectContactBounce = 0.1f;
305
306 private float mAvatarObjectContactFriction = 75f;
307 private float mAvatarObjectContactBounce = 0.1f;
308
309 private float avPIDD = 3200f;
310 private float avPIDP = 1400f;
229 private float avCapRadius = 0.37f; 311 private float avCapRadius = 0.37f;
230 private float avDensity = 3f; 312 private float avStandupTensor = 2000000f;
313
314 /// <summary>
315 /// true = old compatibility mode with leaning capsule; false = new corrected mode
316 /// </summary>
317 /// <remarks>
318 /// Even when set to false, the capsule still tilts but this is done in a different way.
319 /// </remarks>
320 public bool IsAvCapsuleTilted { get; private set; }
321
322 private float avDensity = 80f;
323// private float avHeightFudgeFactor = 0.52f;
231 private float avMovementDivisorWalk = 1.3f; 324 private float avMovementDivisorWalk = 1.3f;
232 private float avMovementDivisorRun = 0.8f; 325 private float avMovementDivisorRun = 0.8f;
233 private float minimumGroundFlightOffset = 3f; 326 private float minimumGroundFlightOffset = 3f;
234 public float maximumMassObject = 10000.01f; 327 public float maximumMassObject = 10000.01f;
235 328
329 public bool meshSculptedPrim = true;
330 public bool forceSimplePrimMeshing = false;
331
332 public float meshSculptLOD = 32;
333 public float MeshSculptphysicalLOD = 16;
236 334
237 public float geomDefaultDensity = 10.000006836f; 335 public float geomDefaultDensity = 10.000006836f;
238 336
239 public int geomContactPointsStartthrottle = 3; 337 public int geomContactPointsStartthrottle = 3;
240 public int geomUpdatesPerThrottledUpdate = 15; 338 public int geomUpdatesPerThrottledUpdate = 15;
339 private const int avatarExpectedContacts = 3;
241 340
242 public float bodyPIDD = 35f; 341 public float bodyPIDD = 35f;
243 public float bodyPIDG = 25; 342 public float bodyPIDG = 25;
244 343
245// public int geomCrossingFailuresBeforeOutofbounds = 6; 344 public int geomCrossingFailuresBeforeOutofbounds = 5;
345
346 public float bodyMotorJointMaxforceTensor = 2;
246 347
247 public int bodyFramesAutoDisable = 5; 348 public int bodyFramesAutoDisable = 20;
248 349
350 private float[] _watermap;
351 private bool m_filterCollisions = true;
249 352
250 private d.NearCallback nearCallback; 353 private d.NearCallback nearCallback;
354 public d.TriCallback triCallback;
355 public d.TriArrayCallback triArrayCallback;
251 356
252 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); 357 /// <summary>
253 private HashSet<OdePrim> _prims = new HashSet<OdePrim>(); 358 /// Avatars in the physics scene.
254 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); 359 /// </summary>
255 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>(); 360 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
256 361
257 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>(); 362 /// <summary>
363 /// Prims in the physics scene.
364 /// </summary>
365 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
258 366
259 /// <summary> 367 /// <summary>
260 /// A list of actors that should receive collision events. 368 /// Prims in the physics scene that are subject to physics, not just collisions.
261 /// </summary> 369 /// </summary>
262 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); 370 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
263 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
264
265 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
266// public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
267 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
268 371
269 private float contactsurfacelayer = 0.002f; 372 /// <summary>
373 /// Prims that the simulator has created/deleted/updated and so need updating in ODE.
374 /// </summary>
375 private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>();
270 376
271 private int contactsPerCollision = 80; 377 /// <summary>
272 internal IntPtr ContactgeomsArray = IntPtr.Zero; 378 /// Record a character that has taints to be processed.
273 private IntPtr GlobalContactsArray = IntPtr.Zero; 379 /// </summary>
380 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
274 381
275 const int maxContactsbeforedeath = 4000; 382 /// <summary>
276 private volatile int m_global_contactcount = 0; 383 /// Keep record of contacts in the physics loop so that we can remove duplicates.
384 /// </summary>
385 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
277 386
278 private IntPtr contactgroup; 387 /// <summary>
388 /// A dictionary of actors that should receive collision events.
389 /// </summary>
390 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
279 391
280 public ContactData[] m_materialContactsData = new ContactData[8]; 392 /// <summary>
393 /// A dictionary of collision event changes that are waiting to be processed.
394 /// </summary>
395 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
281 396
282 private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>(); 397 /// <summary>
283 private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>(); 398 /// Maps a unique geometry id (a memory location) to a physics actor name.
284 private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>(); 399 /// </summary>
285 400 /// <remarks>
286 private int m_physicsiterations = 10; 401 /// Only actors participating in collisions have geometries. This has to be maintained separately from
287 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag 402 /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
288// private PhysicsActor PANull = new NullPhysicsActor(); 403 /// apart from the singleton PANull
289 private float step_time = 0.0f; 404 /// </remarks>
405 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
290 406
291 public IntPtr world; 407 /// <summary>
408 /// Maps a unique geometry id (a memory location) to a physics actor.
409 /// </summary>
410 /// <remarks>
411 /// Only actors participating in collisions have geometries.
412 /// </remarks>
413 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
414
415 /// <summary>
416 /// Defects list to remove characters that no longer have finite positions due to some other bug.
417 /// </summary>
418 /// <remarks>
419 /// Used repeatedly in Simulate() but initialized once here.
420 /// </remarks>
421 private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
292 422
423 private bool m_NINJA_physics_joints_enabled = false;
424 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
425 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
426 private d.ContactGeom[] contacts;
293 427
294 // split the spaces acording to contents type 428 /// <summary>
295 // ActiveSpace contains characters and active prims 429 /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
296 // StaticSpace contains land and other that is mostly static in enviroment 430 /// </summary>
297 // this can contain subspaces, like the grid in staticspace 431 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
298 // as now space only contains this 2 top spaces 432
433 /// <summary>
434 /// can lock for longer. accessed only by OdeScene.
435 /// </summary>
436 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>();
299 437
300 public IntPtr TopSpace; // the global space 438 /// <summary>
301 public IntPtr ActiveSpace; // space for active prims 439 /// can lock for longer. accessed only by OdeScene.
302 public IntPtr StaticSpace; // space for the static things around 440 /// </summary>
303 public IntPtr GroundSpace; // space for ground 441 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>();
304 442
305 // some speedup variables 443 /// <summary>
306 private int spaceGridMaxX; 444 /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
307 private int spaceGridMaxY; 445 /// </summary>
308 private float spacesPerMeter; 446 private readonly List<string> requestedJointsToBeDeleted = new List<string>();
447
448 private Object externalJointRequestsLock = new Object();
449 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
450 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
451 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
452 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
453
454 private d.Contact contact;
455 private d.Contact TerrainContact;
456 private d.Contact AvatarMovementprimContact;
457 private d.Contact AvatarMovementTerrainContact;
458 private d.Contact WaterContact;
459 private d.Contact[,] m_materialContacts;
460
461//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
462//Ckrinke private int m_randomizeWater = 200;
463 private int m_physicsiterations = 10;
464 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
465 private readonly PhysicsActor PANull = new NullPhysicsActor();
466// private float step_time = 0.0f;
467//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
468//Ckrinke private int ms = 0;
469 public IntPtr world;
470 //private bool returncollisions = false;
471 // private uint obj1LocalID = 0;
472 private uint obj2LocalID = 0;
473 //private int ctype = 0;
474 private OdeCharacter cc1;
475 private OdePrim cp1;
476 private OdeCharacter cc2;
477 private OdePrim cp2;
478 private int p1ExpectedPoints = 0;
479 private int p2ExpectedPoints = 0;
480 //private int cStartStop = 0;
481 //private string cDictKey = "";
482
483 public IntPtr space;
484
485 //private IntPtr tmpSpace;
486 // split static geometry collision handling into spaces of 30 meters
487 public IntPtr[,] staticPrimspace;
309 488
310 // split static geometry collision into a grid as before 489 /// <summary>
311 private IntPtr[,] staticPrimspace; 490 /// Used to lock the entire physics scene. Locked during the main part of Simulate()
312 private IntPtr[] staticPrimspaceOffRegion; 491 /// </summary>
492 internal Object OdeLock = new Object();
313 493
314 public Object OdeLock; 494 private bool _worldInitialized = false;
315 public static Object SimulationLock;
316 495
317 public IMesher mesher; 496 public IMesher mesher;
318 497
@@ -322,340 +501,461 @@ namespace OpenSim.Region.Physics.OdePlugin
322 public int physics_logging_interval = 0; 501 public int physics_logging_interval = 0;
323 public bool physics_logging_append_existing_logfile = false; 502 public bool physics_logging_append_existing_logfile = false;
324 503
504
505 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
506 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
507
508 // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
509 // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
510 // TODO: unused: private uint heightmapWidthSamples;
511 // TODO: unused: private uint heightmapHeightSamples;
512
513 private volatile int m_global_contactcount = 0;
514
325 private Vector3 m_worldOffset = Vector3.Zero; 515 private Vector3 m_worldOffset = Vector3.Zero;
326 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); 516 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
327 private PhysicsScene m_parentScene = null; 517 private PhysicsScene m_parentScene = null;
328 518
329 private ODERayCastRequestManager m_rayCastManager; 519 private ODERayCastRequestManager m_rayCastManager;
330 public ODEMeshWorker m_meshWorker;
331
332/* maybe needed if ode uses tls
333 private void checkThread()
334 {
335 520
336 int th = Thread.CurrentThread.ManagedThreadId;
337 if(th != threadid)
338 {
339 threadid = th;
340 d.AllocateODEDataForThread(~0U);
341 }
342 }
343 */
344 /// <summary> 521 /// <summary>
345 /// Initiailizes the scene 522 /// Initiailizes the scene
346 /// Sets many properties that ODE requires to be stable 523 /// Sets many properties that ODE requires to be stable
347 /// These settings need to be tweaked 'exactly' right or weird stuff happens. 524 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
348 /// </summary> 525 /// </summary>
349 public OdeScene(string sceneIdentifier) 526 /// <param value="name">Name of the scene. Useful in debug messages.</param>
350 { 527 public OdeScene(string name)
351 m_log 528 {
352 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); 529 m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name);
353
354// checkThread();
355 Name = sceneIdentifier;
356 530
357 OdeLock = new Object(); 531 Name = name;
358 SimulationLock = new Object();
359 532
360 nearCallback = near; 533 nearCallback = near;
361 534 triCallback = TriCallback;
535 triArrayCallback = TriArrayCallback;
362 m_rayCastManager = new ODERayCastRequestManager(this); 536 m_rayCastManager = new ODERayCastRequestManager(this);
363
364 537
365 lock (OdeLock) 538 // Create the world and the first space
366 { 539 world = d.WorldCreate();
367 // Create the world and the first space 540 space = d.HashSpaceCreate(IntPtr.Zero);
368 try
369 {
370 world = d.WorldCreate();
371 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
372 541
373 // now the major subspaces 542 contactgroup = d.JointGroupCreate(0);
374 ActiveSpace = d.HashSpaceCreate(TopSpace);
375 StaticSpace = d.HashSpaceCreate(TopSpace);
376 GroundSpace = d.HashSpaceCreate(TopSpace);
377 }
378 catch
379 {
380 // i must RtC#FM
381 }
382
383 d.HashSpaceSetLevels(TopSpace, -2, 8);
384 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
385 d.HashSpaceSetLevels(StaticSpace, -2, 8);
386 d.HashSpaceSetLevels(GroundSpace, 0, 8);
387 543
388 // demote to second level 544 d.WorldSetAutoDisableFlag(world, false);
389 d.SpaceSetSublevel(ActiveSpace, 1);
390 d.SpaceSetSublevel(StaticSpace, 1);
391 d.SpaceSetSublevel(GroundSpace, 1);
392 545
393 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | 546 #if USE_DRAWSTUFF
394 CollisionCategories.Geom | 547 Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
395 CollisionCategories.Character | 548 viewthread.Start();
396 CollisionCategories.Phantom | 549 #endif
397 CollisionCategories.VolumeDtc
398 ));
399 d.GeomSetCollideBits(ActiveSpace, 0);
400 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
401 CollisionCategories.Geom |
402 CollisionCategories.Land |
403 CollisionCategories.Water |
404 CollisionCategories.Phantom |
405 CollisionCategories.VolumeDtc
406 ));
407 d.GeomSetCollideBits(StaticSpace, 0);
408 550
409 d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); 551 _watermap = new float[258 * 258];
410 d.GeomSetCollideBits(GroundSpace, 0);
411 552
412 contactgroup = d.JointGroupCreate(0); 553 // Zero out the prim spaces array (we split our space into smaller spaces so
413 //contactgroup 554 // we can hit test less.
555 }
414 556
415 d.WorldSetAutoDisableFlag(world, false); 557#if USE_DRAWSTUFF
416 } 558 public void startvisualization(object o)
559 {
560 ds.Functions fn;
561 fn.version = ds.VERSION;
562 fn.start = new ds.CallbackFunction(start);
563 fn.step = new ds.CallbackFunction(step);
564 fn.command = new ds.CallbackFunction(command);
565 fn.stop = null;
566 fn.path_to_textures = "./textures";
567 string[] args = new string[0];
568 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
417 } 569 }
570#endif
418 571
419 // Initialize the mesh plugin 572 // Initialize the mesh plugin
420// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
421 public override void Initialise(IMesher meshmerizer, IConfigSource config) 573 public override void Initialise(IMesher meshmerizer, IConfigSource config)
422 { 574 {
423// checkThread(); 575 InitializeExtraStats();
576
424 mesher = meshmerizer; 577 mesher = meshmerizer;
425 m_config = config; 578 m_config = config;
579 // Defaults
426 580
427 string ode_config = d.GetConfiguration(); 581 if (Environment.OSVersion.Platform == PlatformID.Unix)
428 if (ode_config != null && ode_config != "")
429 { 582 {
430 m_log.WarnFormat("ODE configuration: {0}", ode_config); 583 avPIDD = 3200.0f;
431 584 avPIDP = 1400.0f;
432 if (ode_config.Contains("ODE_Ubit")) 585 avStandupTensor = 2000000f;
433 { 586 }
434 OdeUbitLib = true; 587 else
435 } 588 {
589 avPIDD = 2200.0f;
590 avPIDP = 900.0f;
591 avStandupTensor = 550000f;
436 } 592 }
437
438 /*
439 if (region != null)
440 {
441 WorldExtents.X = region.RegionSizeX;
442 WorldExtents.Y = region.RegionSizeY;
443 }
444 */
445
446 // Defaults
447 593
448 int contactsPerCollision = 80; 594 int contactsPerCollision = 80;
449 595
450 IConfig physicsconfig = null;
451
452 if (m_config != null) 596 if (m_config != null)
453 { 597 {
454 physicsconfig = m_config.Configs["ODEPhysicsSettings"]; 598 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
455 if (physicsconfig != null) 599 if (physicsconfig != null)
456 { 600 {
457 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); 601 CollectStats = physicsconfig.GetBoolean("collect_stats", false);
458 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); 602
459 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); 603 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
604 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
605 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
606
607 float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
608 AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
609 if (AvatarTerminalVelocity != avatarTerminalVelocity)
610 {
611 m_log.WarnFormat(
612 "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}",
613 avatarTerminalVelocity, AvatarTerminalVelocity);
614 }
615
616 worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
617 worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
618
619 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
620 smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
621 smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
622
623 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
624
625 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
626 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
627 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
460 628
461 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); 629 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
630 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
631 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
462 632
463 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); 633 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
634 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
635
636 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
637 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
464 638
465 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); 639 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
466 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); 640 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
467 641
468 avDensity = physicsconfig.GetFloat("av_density", avDensity); 642 avDensity = physicsconfig.GetFloat("av_density", 80f);
469 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); 643// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
470 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); 644 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
471 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); 645 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
646 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
647 IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
472 648
473 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); 649 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
474 650
475 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); 651 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
476 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); 652 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
477// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); 653 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
654
655 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
656 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
657
658 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
659 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
478 660
479 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); 661 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
480 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); 662 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
663 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
664 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
665 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
666
667 if (Environment.OSVersion.Platform == PlatformID.Unix)
668 {
669 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
670 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
671 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
672 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
673 }
674 else
675 {
676 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
677 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
678 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
679 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
680 }
481 681
482 physics_logging = physicsconfig.GetBoolean("physics_logging", false); 682 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
483 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); 683 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
484 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); 684 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
485 685
486 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); 686 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
487 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); 687 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
688 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
488 } 689 }
489 } 690 }
490 691
491 m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); 692 contacts = new d.ContactGeom[contactsPerCollision];
492 693
493 HalfOdeStep = ODE_STEPSIZE * 0.5f; 694 staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
494 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); 695
495 696 // Centeral contact friction and bounce
496 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); 697 // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
497 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); 698 // an avatar falls through in Z but not in X or Y when walking on a prim.
498 699 contact.surface.mode |= d.ContactFlags.SoftERP;
499 m_materialContactsData[(int)Material.Stone].mu = 0.8f; 700 contact.surface.mu = nmAvatarObjectContactFriction;
500 m_materialContactsData[(int)Material.Stone].bounce = 0.4f; 701 contact.surface.bounce = nmAvatarObjectContactBounce;
702 contact.surface.soft_cfm = 0.010f;
703 contact.surface.soft_erp = 0.010f;
704
705 // Terrain contact friction and Bounce
706 // This is the *non* moving version. Use this when an avatar
707 // isn't moving to keep it in place better
708 TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
709 TerrainContact.surface.mu = nmTerrainContactFriction;
710 TerrainContact.surface.bounce = nmTerrainContactBounce;
711 TerrainContact.surface.soft_erp = nmTerrainContactERP;
712
713 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
714 WaterContact.surface.mu = 0f; // No friction
715 WaterContact.surface.bounce = 0.0f; // No bounce
716 WaterContact.surface.soft_cfm = 0.010f;
717 WaterContact.surface.soft_erp = 0.010f;
718
719 // Prim contact friction and bounce
720 // THis is the *non* moving version of friction and bounce
721 // Use this when an avatar comes in contact with a prim
722 // and is moving
723 AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
724 AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
725
726 // Terrain contact friction bounce and various error correcting calculations
727 // Use this when an avatar is in contact with the terrain and moving.
728 AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
729 AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
730 AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
731 AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
501 732
502 m_materialContactsData[(int)Material.Metal].mu = 0.3f; 733 /*
503 m_materialContactsData[(int)Material.Metal].bounce = 0.4f; 734 <summary></summary>
504 735 Stone = 0,
505 m_materialContactsData[(int)Material.Glass].mu = 0.2f; 736 /// <summary></summary>
506 m_materialContactsData[(int)Material.Glass].bounce = 0.7f; 737 Metal = 1,
507 738 /// <summary></summary>
508 m_materialContactsData[(int)Material.Wood].mu = 0.6f; 739 Glass = 2,
509 m_materialContactsData[(int)Material.Wood].bounce = 0.5f; 740 /// <summary></summary>
510 741 Wood = 3,
511 m_materialContactsData[(int)Material.Flesh].mu = 0.9f; 742 /// <summary></summary>
512 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; 743 Flesh = 4,
744 /// <summary></summary>
745 Plastic = 5,
746 /// <summary></summary>
747 Rubber = 6
748 */
513 749
514 m_materialContactsData[(int)Material.Plastic].mu = 0.4f; 750 m_materialContacts = new d.Contact[7,2];
515 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; 751
752 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
753 m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
754 m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
755 m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
756 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
757 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
758
759 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
760 m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
761 m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
762 m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
763 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
764 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
765
766 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
767 m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
768 m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
769 m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
770 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
771 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
772
773 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
774 m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
775 m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
776 m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
777 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
778 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
779
780 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
781 m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
782 m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
783 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
784 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
785 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
516 786
517 m_materialContactsData[(int)Material.Rubber].mu = 0.9f; 787 /*
518 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; 788 private float nmAvatarObjectContactFriction = 250f;
789 private float nmAvatarObjectContactBounce = 0.1f;
519 790
520 m_materialContactsData[(int)Material.light].mu = 0.0f; 791 private float mAvatarObjectContactFriction = 75f;
521 m_materialContactsData[(int)Material.light].bounce = 0.0f; 792 private float mAvatarObjectContactBounce = 0.1f;
793 */
794 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
795 m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
796 m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
797 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
798 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
799 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
800
801 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
802 m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
803 m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
804 m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
805 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
806 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
807
808 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
809 m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
810 m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
811 m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
812 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
813 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
814
815 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
816 m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
817 m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
818 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
819 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
820 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
821
822 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
823 m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
824 m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
825 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
826 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
827 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
828
829 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
830 m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
831 m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
832 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
833 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
834 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
835
836 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
837 m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
838 m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
839 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
840 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
841 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
842
843 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
844 m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
845 m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
846 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
847 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
848 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
849
850 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
851 m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
852 m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
853 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
854 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
855 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
856
857 d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
522 858
523 // Set the gravity,, don't disable things automatically (we set it explicitly on some things) 859 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
524 860
525 d.WorldSetGravity(world, gravityx, gravityy, gravityz); 861 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
526 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); 862 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
527 863
528 d.WorldSetLinearDamping(world, 0.002f); 864 d.WorldSetLinearDamping(world, 256f);
529 d.WorldSetAngularDamping(world, 0.002f); 865 d.WorldSetAngularDamping(world, 256f);
530 d.WorldSetAngularDampingThreshold(world, 0f); 866 d.WorldSetAngularDampingThreshold(world, 256f);
531 d.WorldSetLinearDampingThreshold(world, 0f); 867 d.WorldSetLinearDampingThreshold(world, 256f);
532 d.WorldSetMaxAngularSpeed(world, 100f); 868 d.WorldSetMaxAngularSpeed(world, 256f);
533
534 d.WorldSetCFM(world,1e-6f); // a bit harder than default
535 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
536 d.WorldSetERP(world, 0.6f); // higher than original
537 869
538 // Set how many steps we go without running collision testing 870 // Set how many steps we go without running collision testing
539 // This is in addition to the step size. 871 // This is in addition to the step size.
540 // Essentially Steps * m_physicsiterations 872 // Essentially Steps * m_physicsiterations
541 d.WorldSetQuickStepNumIterations(world, m_physicsiterations); 873 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
874 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
542 875
543 d.WorldSetContactMaxCorrectingVel(world, 60.0f); 876 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
544 877 {
545 spacesPerMeter = 1 / metersInSpace; 878 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
546 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
547 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
548
549 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
550
551 // create all spaces now
552 int i, j;
553 IntPtr newspace;
554
555 for (i = 0; i < spaceGridMaxX; i++)
556 for (j = 0; j < spaceGridMaxY; j++)
557 {
558 newspace = d.HashSpaceCreate(StaticSpace);
559 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
560 waitForSpaceUnlock(newspace);
561 d.SpaceSetSublevel(newspace, 2);
562 d.HashSpaceSetLevels(newspace, -2, 8);
563 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
564 CollisionCategories.Geom |
565 CollisionCategories.Land |
566 CollisionCategories.Water |
567 CollisionCategories.Phantom |
568 CollisionCategories.VolumeDtc
569 ));
570 d.GeomSetCollideBits(newspace, 0);
571
572 staticPrimspace[i, j] = newspace;
573 }
574 // let this now be real maximum values
575 spaceGridMaxX--;
576 spaceGridMaxY--;
577
578 // create 4 off world spaces (x<0,x>max,y<0,y>max)
579 staticPrimspaceOffRegion = new IntPtr[4];
580
581 for (i = 0; i < 4; i++)
582 { 879 {
583 newspace = d.HashSpaceCreate(StaticSpace); 880 staticPrimspace[i, j] = IntPtr.Zero;
584 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
585 waitForSpaceUnlock(newspace);
586 d.SpaceSetSublevel(newspace, 2);
587 d.HashSpaceSetLevels(newspace, -2, 8);
588 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
589 CollisionCategories.Geom |
590 CollisionCategories.Land |
591 CollisionCategories.Water |
592 CollisionCategories.Phantom |
593 CollisionCategories.VolumeDtc
594 ));
595 d.GeomSetCollideBits(newspace, 0);
596
597 staticPrimspaceOffRegion[i] = newspace;
598 } 881 }
882 }
599 883
600 m_lastframe = DateTime.UtcNow; 884 _worldInitialized = true;
601 m_lastMeshExpire = m_lastframe;
602 } 885 }
603 886
604 internal void waitForSpaceUnlock(IntPtr space) 887// internal void waitForSpaceUnlock(IntPtr space)
605 { 888// {
606 //if (space != IntPtr.Zero) 889// //if (space != IntPtr.Zero)
607 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing 890// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
608 } 891// }
892
893// /// <summary>
894// /// Debug space message for printing the space that a prim/avatar is in.
895// /// </summary>
896// /// <param name="pos"></param>
897// /// <returns>Returns which split up space the given position is in.</returns>
898// public string whichspaceamIin(Vector3 pos)
899// {
900// return calculateSpaceForGeom(pos).ToString();
901// }
609 902
610 #region Collision Detection 903 #region Collision Detection
611 904
612 // sets a global contact for a joint for contactgeom , and base contact description) 905 /// <summary>
613 906 /// Collides two geometries.
614 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) 907 /// </summary>
908 /// <returns></returns>
909 /// <param name='geom1'></param>
910 /// <param name='geom2'>/param>
911 /// <param name='maxContacts'></param>
912 /// <param name='contactsArray'></param>
913 /// <param name='contactGeomSize'></param>
914 private int CollideGeoms(
915 IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
615 { 916 {
616 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) 917 int count;
617 return IntPtr.Zero;
618 918
619 float erp = contactGeom.depth; 919 lock (OdeScene.UniversalColliderSyncObject)
620 erp *= erpscale; 920 {
621 if (erp < minERP) 921 // We do this inside the lock so that we don't count any delay in acquiring it
622 erp = minERP; 922 if (CollectStats)
623 else if (erp > MaxERP) 923 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
624 erp = MaxERP;
625
626 float depth = contactGeom.depth * dscale;
627 if (depth > 0.5f)
628 depth = 0.5f;
629 924
630 d.Contact newcontact = new d.Contact(); 925 count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
631 newcontact.geom.depth = depth; 926 }
632 newcontact.geom.g1 = contactGeom.g1;
633 newcontact.geom.g2 = contactGeom.g2;
634 newcontact.geom.pos = contactGeom.pos;
635 newcontact.geom.normal = contactGeom.normal;
636 newcontact.geom.side1 = contactGeom.side1;
637 newcontact.geom.side2 = contactGeom.side2;
638 927
639 // this needs bounce also 928 // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
640 newcontact.surface.mode = comumContactFlags; 929 // negligable
641 newcontact.surface.mu = mu; 930 if (CollectStats)
642 newcontact.surface.bounce = bounce; 931 m_stats[ODENativeGeomCollisionFrameMsStatName]
643 newcontact.surface.soft_cfm = cfm; 932 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
644 newcontact.surface.soft_erp = erp;
645 933
646 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); 934 return count;
647 Marshal.StructureToPtr(newcontact, contact, true);
648 return d.JointCreateContactPtr(world, contactgroup, contact);
649 } 935 }
650 936
651 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) 937 /// <summary>
938 /// Collide two spaces or a space and a geometry.
939 /// </summary>
940 /// <param name='space1'></param>
941 /// <param name='space2'>/param>
942 /// <param name='data'></param>
943 private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
652 { 944 {
653 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) 945 if (CollectStats)
654 return false; 946 {
947 m_inCollisionTiming = true;
948 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
949 }
655 950
656 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); 951 d.SpaceCollide2(space1, space2, data, nearCallback);
657 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); 952
658 return true; 953 if (CollectStats && m_inCollisionTiming)
954 {
955 m_stats[ODENativeSpaceCollisionFrameMsStatName]
956 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
957 m_inCollisionTiming = false;
958 }
659 } 959 }
660 960
661 /// <summary> 961 /// <summary>
@@ -664,50 +964,76 @@ namespace OpenSim.Region.Physics.OdePlugin
664 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> 964 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
665 /// <param name="g1">a geometry or space</param> 965 /// <param name="g1">a geometry or space</param>
666 /// <param name="g2">another geometry or space</param> 966 /// <param name="g2">another geometry or space</param>
667 ///
668
669 private void near(IntPtr space, IntPtr g1, IntPtr g2) 967 private void near(IntPtr space, IntPtr g1, IntPtr g2)
670 { 968 {
671 // no lock here! It's invoked from within Simulate(), which is thread-locked 969 if (CollectStats && m_inCollisionTiming)
970 {
971 m_stats[ODENativeSpaceCollisionFrameMsStatName]
972 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
973 m_inCollisionTiming = false;
974 }
672 975
673 if (m_global_contactcount >= maxContactsbeforedeath) 976// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
674 return; 977 // no lock here! It's invoked from within Simulate(), which is thread-locked
675 978
676 // Test if we're colliding a geom with a space. 979 // Test if we're colliding a geom with a space.
677 // If so we have to drill down into the space recursively 980 // If so we have to drill down into the space recursively
678 981
679 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
680 return;
681
682 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) 982 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
683 { 983 {
984 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
985 return;
986
987 // Separating static prim geometry spaces.
684 // We'll be calling near recursivly if one 988 // We'll be calling near recursivly if one
685 // of them is a space to find all of the 989 // of them is a space to find all of the
686 // contact points in the space 990 // contact points in the space
687 try 991 try
688 { 992 {
689 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); 993 CollideSpaces(g1, g2, IntPtr.Zero);
690 } 994 }
691 catch (AccessViolationException) 995 catch (AccessViolationException)
692 { 996 {
693 m_log.Warn("[PHYSICS]: Unable to collide test a space"); 997 m_log.Error("[ODE SCENE]: Unable to collide test a space");
694 return; 998 return;
695 } 999 }
696 //here one should check collisions of geoms inside a space 1000 //Colliding a space or a geom with a space or a geom. so drill down
697 // but on each space we only should have geoms that not colide amoung each other 1001
698 // so we don't dig inside spaces 1002 //Collide all geoms in each space..
1003 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
1004 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
699 return; 1005 return;
700 } 1006 }
701 1007
702 // get geom bodies to check if we already a joint contact 1008 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
703 // guess this shouldn't happen now 1009 return;
1010
704 IntPtr b1 = d.GeomGetBody(g1); 1011 IntPtr b1 = d.GeomGetBody(g1);
705 IntPtr b2 = d.GeomGetBody(g2); 1012 IntPtr b2 = d.GeomGetBody(g2);
706 1013
707 // d.GeomClassID id = d.GeomGetClass(g1); 1014 // d.GeomClassID id = d.GeomGetClass(g1);
708 1015
1016 String name1 = null;
1017 String name2 = null;
1018
1019 if (!geom_name_map.TryGetValue(g1, out name1))
1020 {
1021 name1 = "null";
1022 }
1023 if (!geom_name_map.TryGetValue(g2, out name2))
1024 {
1025 name2 = "null";
1026 }
1027
1028 //if (id == d.GeomClassId.TriMeshClass)
1029 //{
1030 // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
1031 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1032 //}
1033
709 // Figure out how many contact points we have 1034 // Figure out how many contact points we have
710 int count = 0; 1035 int count = 0;
1036
711 try 1037 try
712 { 1038 {
713 // Colliding Geom To Geom 1039 // Colliding Geom To Geom
@@ -719,611 +1045,914 @@ namespace OpenSim.Region.Physics.OdePlugin
719 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) 1045 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
720 return; 1046 return;
721 1047
722// debug 1048 count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
723 PhysicsActor dp2;
724 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
725 {
726 d.AABB aabb;
727 d.GeomGetAABB(g2, out aabb);
728 float x = aabb.MaxX - aabb.MinX;
729 float y = aabb.MaxY - aabb.MinY;
730 float z = aabb.MaxZ - aabb.MinZ;
731 if (x > 60.0f || y > 60.0f || z > 60.0f)
732 {
733 if (!actor_name_map.TryGetValue(g2, out dp2))
734 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
735 else
736 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
737 dp2.Name, dp2.Size, x, y, z,
738 dp2.Position.ToString(),
739 dp2.Orientation.ToString(),
740 dp2.Orientation.Length());
741 return;
742 }
743 }
744//
745
746 1049
1050 // All code after this is only relevant if we have any collisions
1051 if (count <= 0)
1052 return;
747 1053
748 if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || 1054 if (count > contacts.Length)
749 d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) 1055 m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
750 {
751 int cflags;
752 unchecked
753 {
754 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
755 }
756 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
757 }
758 else
759 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
760 } 1056 }
761 catch (SEHException) 1057 catch (SEHException)
762 { 1058 {
763 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); 1059 m_log.Error(
764// ode.drelease(world); 1060 "[ODE SCENE]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
765 base.TriggerPhysicsBasedRestart(); 1061 base.TriggerPhysicsBasedRestart();
766 } 1062 }
767 catch (Exception e) 1063 catch (Exception e)
768 { 1064 {
769 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); 1065 m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message);
770 return; 1066 return;
771 } 1067 }
772 1068
773 // contacts done
774 if (count == 0)
775 return;
776
777 // try get physical actors
778 PhysicsActor p1; 1069 PhysicsActor p1;
779 PhysicsActor p2; 1070 PhysicsActor p2;
780 1071
1072 p1ExpectedPoints = 0;
1073 p2ExpectedPoints = 0;
1074
781 if (!actor_name_map.TryGetValue(g1, out p1)) 1075 if (!actor_name_map.TryGetValue(g1, out p1))
782 { 1076 {
783 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); 1077 p1 = PANull;
784 return;
785 } 1078 }
786 1079
787 if (!actor_name_map.TryGetValue(g2, out p2)) 1080 if (!actor_name_map.TryGetValue(g2, out p2))
788 { 1081 {
789 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); 1082 p2 = PANull;
790 return;
791 } 1083 }
792 1084
793 // update actors collision score 1085 ContactPoint maxDepthContact = new ContactPoint();
794 if (p1.CollisionScore >= float.MaxValue - count) 1086 if (p1.CollisionScore + count >= float.MaxValue)
795 p1.CollisionScore = 0; 1087 p1.CollisionScore = 0;
796 p1.CollisionScore += count; 1088 p1.CollisionScore += count;
797 1089
798 if (p2.CollisionScore >= float.MaxValue - count) 1090 if (p2.CollisionScore + count >= float.MaxValue)
799 p2.CollisionScore = 0; 1091 p2.CollisionScore = 0;
800 p2.CollisionScore += count; 1092 p2.CollisionScore += count;
801 1093
802 // get first contact 1094 for (int i = 0; i < count; i++)
803 d.ContactGeom curContact = new d.ContactGeom(); 1095 {
804 if (!GetCurContactGeom(0, ref curContact)) 1096 d.ContactGeom curContact = contacts[i];
805 return; 1097
806 // for now it's the one with max depth 1098 if (curContact.depth > maxDepthContact.PenetrationDepth)
807 ContactPoint maxDepthContact = new ContactPoint( 1099 {
1100 maxDepthContact = new ContactPoint(
808 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), 1101 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
809 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), 1102 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
810 curContact.depth 1103 curContact.depth
811 ); 1104 );
812 // do volume detection case 1105 }
813 if (
814 (p1.IsVolumeDtc || p2.IsVolumeDtc))
815 {
816 collision_accounting_events(p1, p2, maxDepthContact);
817 return;
818 }
819 1106
820 // big messy collision analises 1107 //m_log.Warn("[CCOUNT]: " + count);
1108 IntPtr joint;
1109 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
1110 // allows us to have different settings
1111
1112 // We only need to test p2 for 'jump crouch purposes'
1113 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
1114 {
1115 // Testing if the collision is at the feet of the avatar
821 1116
822 Vector3 normoverride = Vector3.Zero; //damm c# 1117 //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
1118 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
1119 p2.IsColliding = true;
1120 }
1121 else
1122 {
1123 p2.IsColliding = true;
1124 }
1125
1126 //if ((framecount % m_returncollisions) == 0)
823 1127
824 float mu = 0; 1128 switch (p1.PhysicsActorType)
825 float bounce = 0; 1129 {
826 float cfm = 0.0001f; 1130 case (int)ActorTypes.Agent:
827 float erpscale = 1.0f; 1131 p1ExpectedPoints = avatarExpectedContacts;
828 float dscale = 1.0f; 1132 p2.CollidingObj = true;
829 bool IgnoreNegSides = false; 1133 break;
1134 case (int)ActorTypes.Prim:
1135 if (p1 != null && p1 is OdePrim)
1136 p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
830 1137
831 ContactData contactdata1 = new ContactData(0, 0, false); 1138 if (p2.Velocity.LengthSquared() > 0.0f)
832 ContactData contactdata2 = new ContactData(0, 0, false); 1139 p2.CollidingObj = true;
1140 break;
1141 case (int)ActorTypes.Unknown:
1142 p2.CollidingGround = true;
1143 break;
1144 default:
1145 p2.CollidingGround = true;
1146 break;
1147 }
833 1148
834 bool dop1foot = false; 1149 // we don't want prim or avatar to explode
835 bool dop2foot = false;
836 bool ignore = false;
837 bool AvanormOverride = false;
838 1150
839 switch (p1.PhysicsActorType) 1151 #region InterPenetration Handling - Unintended physics explosions
840 { 1152# region disabled code1
841 case (int)ActorTypes.Agent: 1153
1154 if (curContact.depth >= 0.08f)
1155 {
1156 //This is disabled at the moment only because it needs more tweaking
1157 //It will eventually be uncommented
1158 /*
1159 if (contact.depth >= 1.00f)
842 { 1160 {
843 AvanormOverride = true; 1161 //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
844 Vector3 tmp = p2.Position - p1.Position; 1162 }
845 normoverride = p2.Velocity - p1.Velocity;
846 mu = normoverride.LengthSquared();
847 1163
848 if (mu > 1e-6) 1164 //If you interpenetrate a prim with an agent
1165 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1166 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
1167 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1168 p2.PhysicsActorType == (int) ActorTypes.Prim))
1169 {
1170
1171 //contact.depth = contact.depth * 4.15f;
1172 /*
1173 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
849 { 1174 {
850 mu = 1.0f / (float)Math.Sqrt(mu); 1175 p2.CollidingObj = true;
851 normoverride *= mu; 1176 contact.depth = 0.003f;
852 mu = Vector3.Dot(tmp, normoverride); 1177 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
853 if (mu > 0) 1178 OdeCharacter character = (OdeCharacter) p2;
854 normoverride *= -1; 1179 character.SetPidStatus(true);
1180 contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
1181
855 } 1182 }
856 else 1183 else
857 { 1184 {
858 tmp.Normalize();
859 normoverride = -tmp;
860 }
861 1185
862 switch (p2.PhysicsActorType) 1186 //contact.depth = 0.0000000f;
1187 }
1188 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
863 { 1189 {
864 case (int)ActorTypes.Agent:
865 p1.CollidingObj = true;
866 p2.CollidingObj = true;
867 break;
868 1190
869 case (int)ActorTypes.Prim: 1191 p1.CollidingObj = true;
870 if (p2.Velocity.LengthSquared() > 0.0f) 1192 contact.depth = 0.003f;
871 p2.CollidingObj = true; 1193 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
872 dop1foot = true; 1194 contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
873 break; 1195 OdeCharacter character = (OdeCharacter)p1;
1196 character.SetPidStatus(true);
1197 }
1198 else
1199 {
874 1200
875 default: 1201 //contact.depth = 0.0000000f;
876 ignore = true; // avatar to terrain and water ignored
877 break;
878 } 1202 }
879 break; 1203
1204
1205
880 } 1206 }
881 1207*/
882 case (int)ActorTypes.Prim: 1208 // If you interpenetrate a prim with another prim
883 switch (p2.PhysicsActorType) 1209 /*
1210 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
884 { 1211 {
885 case (int)ActorTypes.Agent: 1212 #region disabledcode2
886 AvanormOverride = true; 1213 //OdePrim op1 = (OdePrim)p1;
887 1214 //OdePrim op2 = (OdePrim)p2;
888 Vector3 tmp = p2.Position - p1.Position; 1215 //op1.m_collisionscore++;
889 normoverride = p2.Velocity - p1.Velocity; 1216 //op2.m_collisionscore++;
890 mu = normoverride.LengthSquared(); 1217
891 if (mu > 1e-6) 1218 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
892 { 1219 //{
893 mu = 1.0f / (float)Math.Sqrt(mu); 1220 //op1.m_taintdisable = true;
894 normoverride *= mu; 1221 //AddPhysicsActorTaint(p1);
895 mu = Vector3.Dot(tmp, normoverride); 1222 //op2.m_taintdisable = true;
896 if (mu > 0) 1223 //AddPhysicsActorTaint(p2);
897 normoverride *= -1; 1224 //}
898 } 1225
899 else 1226 //if (contact.depth >= 0.25f)
900 { 1227 //{
901 tmp.Normalize(); 1228 // Don't collide, one or both prim will expld.
902 normoverride = -tmp; 1229
903 } 1230 //op1.m_interpenetrationcount++;
904 1231 //op2.m_interpenetrationcount++;
905 bounce = 0; 1232 //interpenetrations_before_disable = 200;
906 mu = 0; 1233 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
907 cfm = 0.0001f; 1234 //{
908 1235 //op1.m_taintdisable = true;
909 dop2foot = true; 1236 //AddPhysicsActorTaint(p1);
910 if (p1.Velocity.LengthSquared() > 0.0f) 1237 //}
911 p1.CollidingObj = true; 1238 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
912 break; 1239 //{
913 1240 // op2.m_taintdisable = true;
914 case (int)ActorTypes.Prim: 1241 //AddPhysicsActorTaint(p2);
915 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) 1242 //}
1243
1244 //contact.depth = contact.depth / 8f;
1245 //contact.normal = new d.Vector3(0, 0, 1);
1246 //}
1247 //if (op1.m_disabled || op2.m_disabled)
1248 //{
1249 //Manually disabled objects stay disabled
1250 //contact.depth = 0f;
1251 //}
1252 #endregion
1253 }
1254 */
1255#endregion
1256 if (curContact.depth >= 1.00f)
1257 {
1258 //m_log.Info("[P]: " + contact.depth.ToString());
1259 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1260 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
1261 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1262 p2.PhysicsActorType == (int) ActorTypes.Unknown))
1263 {
1264 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
916 { 1265 {
917 p1.CollidingObj = true; 1266 if (p2 is OdeCharacter)
918 p2.CollidingObj = true; 1267 {
1268 OdeCharacter character = (OdeCharacter) p2;
1269
1270 //p2.CollidingObj = true;
1271 curContact.depth = 0.00000003f;
1272 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
1273 curContact.pos =
1274 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1275 curContact.pos.Y + (p1.Size.Y/2),
1276 curContact.pos.Z + (p1.Size.Z/2));
1277 character.SetPidStatus(true);
1278 }
919 } 1279 }
920 p1.getContactData(ref contactdata1);
921 p2.getContactData(ref contactdata2);
922 bounce = contactdata1.bounce * contactdata2.bounce;
923 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
924
925 cfm = p1.Mass;
926 if (cfm > p2.Mass)
927 cfm = p2.Mass;
928 dscale = 10 / cfm;
929 dscale = (float)Math.Sqrt(dscale);
930 if (dscale > 1.0f)
931 dscale = 1.0f;
932 erpscale = cfm * 0.01f;
933 cfm = 0.0001f / cfm;
934 if (cfm > 0.01f)
935 cfm = 0.01f;
936 else if (cfm < 0.00001f)
937 cfm = 0.00001f;
938
939 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
940 mu *= frictionMovementMult;
941 1280
942 break; 1281 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
943
944 case (int)ActorTypes.Ground:
945 p1.getContactData(ref contactdata1);
946 bounce = contactdata1.bounce * TerrainBounce;
947 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
948 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
949 mu *= frictionMovementMult;
950 p1.CollidingGround = true;
951
952 cfm = p1.Mass;
953 dscale = 10 / cfm;
954 dscale = (float)Math.Sqrt(dscale);
955 if (dscale > 1.0f)
956 dscale = 1.0f;
957 erpscale = cfm * 0.01f;
958 cfm = 0.0001f / cfm;
959 if (cfm > 0.01f)
960 cfm = 0.01f;
961 else if (cfm < 0.00001f)
962 cfm = 0.00001f;
963
964 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
965 { 1282 {
966 if (curContact.side1 > 0) 1283 if (p1 is OdeCharacter)
967 IgnoreNegSides = true; 1284 {
1285 OdeCharacter character = (OdeCharacter) p1;
1286
1287 //p2.CollidingObj = true;
1288 curContact.depth = 0.00000003f;
1289 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1290 curContact.pos =
1291 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1292 curContact.pos.Y + (p1.Size.Y/2),
1293 curContact.pos.Z + (p1.Size.Z/2));
1294 character.SetPidStatus(true);
1295 }
968 } 1296 }
969 break; 1297 }
970
971 case (int)ActorTypes.Water:
972 default:
973 ignore = true;
974 break;
975 } 1298 }
976 break; 1299 }
977
978 case (int)ActorTypes.Ground:
979 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
980 {
981 p2.CollidingGround = true;
982 p2.getContactData(ref contactdata2);
983 bounce = contactdata2.bounce * TerrainBounce;
984 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
985
986 cfm = p2.Mass;
987 dscale = 10 / cfm;
988 dscale = (float)Math.Sqrt(dscale);
989
990 if (dscale > 1.0f)
991 dscale = 1.0f;
992
993 erpscale = cfm * 0.01f;
994 cfm = 0.0001f / cfm;
995 if (cfm > 0.01f)
996 cfm = 0.01f;
997 else if (cfm < 0.00001f)
998 cfm = 0.00001f;
999 1300
1000 if (curContact.side1 > 0) // should be 2 ? 1301 #endregion
1001 IgnoreNegSides = true;
1002 1302
1003 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) 1303 // Logic for collision handling
1004 mu *= frictionMovementMult; 1304 // Note, that if *all* contacts are skipped (VolumeDetect)
1005 } 1305 // The prim still detects (and forwards) collision events but
1006 else 1306 // appears to be phantom for the world
1007 ignore = true; 1307 Boolean skipThisContact = false;
1008 break;
1009 1308
1010 case (int)ActorTypes.Water: 1309 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1011 default: 1310 skipThisContact = true; // No collision on volume detect prims
1012 break;
1013 }
1014 if (ignore)
1015 return;
1016 1311
1017 IntPtr Joint; 1312 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1313 skipThisContact = true; // No collision on volume detect prims
1018 1314
1019 int i = 0; 1315 if (!skipThisContact && curContact.depth < 0f)
1020 while(true) 1316 skipThisContact = true;
1021 {
1022 1317
1023 if (IgnoreNegSides && curContact.side1 < 0) 1318 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1024 { 1319 skipThisContact = true;
1025 if (++i >= count)
1026 break;
1027 1320
1028 if (!GetCurContactGeom(i, ref curContact)) 1321 const int maxContactsbeforedeath = 4000;
1029 break; 1322 joint = IntPtr.Zero;
1030 }
1031 else
1032 1323
1324 if (!skipThisContact)
1033 { 1325 {
1326 _perloopContact.Add(curContact);
1034 1327
1035 if (AvanormOverride) 1328 if (name1 == "Terrain" || name2 == "Terrain")
1036 { 1329 {
1037 if (curContact.depth > 0.3f) 1330 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
1331 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1038 { 1332 {
1039 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) 1333 p2ExpectedPoints = avatarExpectedContacts;
1040 p1.IsColliding = true; 1334 // Avatar is moving on terrain, use the movement terrain contact
1041 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) 1335 AvatarMovementTerrainContact.geom = curContact;
1042 p2.IsColliding = true;
1043 curContact.normal.X = normoverride.X;
1044 curContact.normal.Y = normoverride.Y;
1045 curContact.normal.Z = normoverride.Z;
1046 }
1047 1336
1337 if (m_global_contactcount < maxContactsbeforedeath)
1338 {
1339 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1340 m_global_contactcount++;
1341 }
1342 }
1048 else 1343 else
1049 { 1344 {
1050 if (dop1foot) 1345 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1346 {
1347 p2ExpectedPoints = avatarExpectedContacts;
1348 // Avatar is standing on terrain, use the non moving terrain contact
1349 TerrainContact.geom = curContact;
1350
1351 if (m_global_contactcount < maxContactsbeforedeath)
1352 {
1353 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1354 m_global_contactcount++;
1355 }
1356 }
1357 else
1051 { 1358 {
1052 float sz = p1.Size.Z; 1359 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1053 Vector3 vtmp = p1.Position;
1054 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1055 if (ppos > 0f)
1056 { 1360 {
1057 if (!p1.Flying) 1361 // prim prim contact
1362 // int pj294950 = 0;
1363 int movintYN = 0;
1364 int material = (int) Material.Wood;
1365 // prim terrain contact
1366 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1058 { 1367 {
1059 d.AABB aabb; 1368 movintYN = 1;
1060 d.GeomGetAABB(g2, out aabb); 1369 }
1061 float tmp = vtmp.Z - sz * .18f;
1062 1370
1063 if (aabb.MaxZ < tmp) 1371 if (p2 is OdePrim)
1064 { 1372 {
1065 vtmp.X = curContact.pos.X - vtmp.X; 1373 material = ((OdePrim) p2).m_material;
1066 vtmp.Y = curContact.pos.Y - vtmp.Y; 1374 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1067 vtmp.Z = -0.2f; 1375 }
1068 vtmp.Normalize(); 1376
1069 curContact.normal.X = vtmp.X; 1377 // Unnessesary because p1 is defined above
1070 curContact.normal.Y = vtmp.Y; 1378 //if (p1 is OdePrim)
1071 curContact.normal.Z = vtmp.Z; 1379 // {
1072 } 1380 // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
1381 // }
1382 //m_log.DebugFormat("Material: {0}", material);
1383
1384 m_materialContacts[material, movintYN].geom = curContact;
1385
1386 if (m_global_contactcount < maxContactsbeforedeath)
1387 {
1388 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1389 m_global_contactcount++;
1073 } 1390 }
1074 } 1391 }
1075 else 1392 else
1076 p1.IsColliding = true; 1393 {
1394 int movintYN = 0;
1395 // prim terrain contact
1396 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1397 {
1398 movintYN = 1;
1399 }
1077 1400
1078 } 1401 int material = (int)Material.Wood;
1079 1402
1080 if (dop2foot) 1403 if (p2 is OdePrim)
1081 {
1082 float sz = p2.Size.Z;
1083 Vector3 vtmp = p2.Position;
1084 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1085 if (ppos > 0f)
1086 {
1087 if (!p2.Flying)
1088 { 1404 {
1089 d.AABB aabb; 1405 material = ((OdePrim)p2).m_material;
1090 d.GeomGetAABB(g1, out aabb); 1406 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1091 float tmp = vtmp.Z - sz * .18f; 1407 }
1092 1408
1093 if (aabb.MaxZ < tmp) 1409 //m_log.DebugFormat("Material: {0}", material);
1094 { 1410 m_materialContacts[material, movintYN].geom = curContact;
1095 vtmp.X = curContact.pos.X - vtmp.X; 1411
1096 vtmp.Y = curContact.pos.Y - vtmp.Y; 1412 if (m_global_contactcount < maxContactsbeforedeath)
1097 vtmp.Z = -0.2f; 1413 {
1098 vtmp.Normalize(); 1414 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1099 curContact.normal.X = vtmp.X; 1415 m_global_contactcount++;
1100 curContact.normal.Y = vtmp.Y;
1101 curContact.normal.Z = vtmp.Z;
1102 }
1103 } 1416 }
1104 } 1417 }
1105 else
1106 p2.IsColliding = true;
1107
1108 } 1418 }
1109 } 1419 }
1420 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1421 //{
1422 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1423 //}
1424 }
1425 else if (name1 == "Water" || name2 == "Water")
1426 {
1427 /*
1428 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1429 {
1430 }
1431 else
1432 {
1433 }
1434 */
1435 //WaterContact.surface.soft_cfm = 0.0000f;
1436 //WaterContact.surface.soft_erp = 0.00000f;
1437 if (curContact.depth > 0.1f)
1438 {
1439 curContact.depth *= 52;
1440 //contact.normal = new d.Vector3(0, 0, 1);
1441 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1442 }
1443
1444 WaterContact.geom = curContact;
1445
1446 if (m_global_contactcount < maxContactsbeforedeath)
1447 {
1448 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1449 m_global_contactcount++;
1450 }
1451 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
1110 } 1452 }
1453 else
1454 {
1455 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1456 {
1457 p2ExpectedPoints = avatarExpectedContacts;
1458 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1459 {
1460 // Avatar is moving on a prim, use the Movement prim contact
1461 AvatarMovementprimContact.geom = curContact;
1111 1462
1112 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); 1463 if (m_global_contactcount < maxContactsbeforedeath)
1113 d.JointAttach(Joint, b1, b2); 1464 {
1465 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1466 m_global_contactcount++;
1467 }
1468 }
1469 else
1470 {
1471 // Avatar is standing still on a prim, use the non movement contact
1472 contact.geom = curContact;
1114 1473
1115 if (++m_global_contactcount >= maxContactsbeforedeath) 1474 if (m_global_contactcount < maxContactsbeforedeath)
1116 break; 1475 {
1476 joint = d.JointCreateContact(world, contactgroup, ref contact);
1477 m_global_contactcount++;
1478 }
1479 }
1480 }
1481 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1482 {
1483 //p1.PhysicsActorType
1484 int material = (int)Material.Wood;
1117 1485
1118 if (++i >= count) 1486 if (p2 is OdePrim)
1119 break; 1487 {
1488 material = ((OdePrim)p2).m_material;
1489 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1490 }
1491
1492 //m_log.DebugFormat("Material: {0}", material);
1493 m_materialContacts[material, 0].geom = curContact;
1120 1494
1121 if (!GetCurContactGeom(i, ref curContact)) 1495 if (m_global_contactcount < maxContactsbeforedeath)
1122 break; 1496 {
1497 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1498 m_global_contactcount++;
1499 }
1500 }
1501 }
1123 1502
1124 if (curContact.depth > maxDepthContact.PenetrationDepth) 1503 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1125 { 1504 {
1126 maxDepthContact.Position.X = curContact.pos.X; 1505 d.JointAttach(joint, b1, b2);
1127 maxDepthContact.Position.Y = curContact.pos.Y; 1506 m_global_contactcount++;
1128 maxDepthContact.Position.Z = curContact.pos.Z;
1129 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1130 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1131 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1132 maxDepthContact.PenetrationDepth = curContact.depth;
1133 } 1507 }
1134 } 1508 }
1135 }
1136 1509
1137 collision_accounting_events(p1, p2, maxDepthContact); 1510 collision_accounting_events(p1, p2, maxDepthContact);
1138
1139/*
1140 if (notskipedcount > geomContactPointsStartthrottle)
1141 {
1142 // If there are more then 3 contact points, it's likely
1143 // that we've got a pile of objects, so ...
1144 // We don't want to send out hundreds of terse updates over and over again
1145 // so lets throttle them and send them again after it's somewhat sorted out.
1146 this needs checking so out for now
1147 if (b1 != IntPtr.Zero)
1148 p1.ThrottleUpdates = true;
1149 if (b2 != IntPtr.Zero)
1150 p2.ThrottleUpdates = true;
1151 1511
1512 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
1513 {
1514 // If there are more then 3 contact points, it's likely
1515 // that we've got a pile of objects, so ...
1516 // We don't want to send out hundreds of terse updates over and over again
1517 // so lets throttle them and send them again after it's somewhat sorted out.
1518 p2.ThrottleUpdates = true;
1519 }
1520 //m_log.Debug(count.ToString());
1521 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1152 } 1522 }
1153 */
1154 } 1523 }
1155 1524
1156 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) 1525 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1157 { 1526 {
1158 uint obj2LocalID = 0; 1527 if (!m_filterCollisions)
1528 return false;
1159 1529
1160 bool p1events = p1.SubscribedEvents(); 1530 bool result = false;
1161 bool p2events = p2.SubscribedEvents();
1162 1531
1163 if (p1.IsVolumeDtc) 1532 ActorTypes at = (ActorTypes)atype;
1164 p2events = false;
1165 if (p2.IsVolumeDtc)
1166 p1events = false;
1167 1533
1168 if (!p2events && !p1events) 1534 foreach (d.ContactGeom contact in _perloopContact)
1169 return; 1535 {
1536 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1537 //{
1538 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1539 if (at == ActorTypes.Agent)
1540 {
1541 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
1542 && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
1543 && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1544 {
1545 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1546 {
1547 //contactGeom.depth *= .00005f;
1548 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1549 // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1550 result = true;
1551 break;
1552 }
1553// else
1554// {
1555// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1556// }
1557 }
1558// else
1559// {
1560// //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1561// //int i = 0;
1562// }
1563 }
1564 else if (at == ActorTypes.Prim)
1565 {
1566 //d.AABB aabb1 = new d.AABB();
1567 //d.AABB aabb2 = new d.AABB();
1170 1568
1171 Vector3 vel = Vector3.Zero; 1569 //d.GeomGetAABB(contactGeom.g2, out aabb2);
1172 if (p2 != null && p2.IsPhysical) 1570 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1173 vel = p2.Velocity; 1571 //aabb1.
1572 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1573 {
1574 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1575 {
1576 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1577 {
1578 result = true;
1579 break;
1580 }
1581 }
1582 //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1583 //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1584 }
1585 }
1586 }
1174 1587
1175 if (p1 != null && p1.IsPhysical) 1588 return result;
1176 vel -= p1.Velocity; 1589 }
1177 1590
1178 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); 1591 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1592 {
1593 // obj1LocalID = 0;
1594 //returncollisions = false;
1595 obj2LocalID = 0;
1596 //ctype = 0;
1597 //cStartStop = 0;
1598 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1599 return;
1179 1600
1180 switch ((ActorTypes)p1.PhysicsActorType) 1601 switch ((ActorTypes)p2.PhysicsActorType)
1181 { 1602 {
1182 case ActorTypes.Agent: 1603 case ActorTypes.Agent:
1604 cc2 = (OdeCharacter)p2;
1605
1606 // obj1LocalID = cc2.m_localID;
1607 switch ((ActorTypes)p1.PhysicsActorType)
1608 {
1609 case ActorTypes.Agent:
1610 cc1 = (OdeCharacter)p1;
1611 obj2LocalID = cc1.LocalID;
1612 cc1.AddCollisionEvent(cc2.LocalID, contact);
1613 //ctype = (int)CollisionCategories.Character;
1614
1615 //if (cc1.CollidingObj)
1616 //cStartStop = (int)StatusIndicators.Generic;
1617 //else
1618 //cStartStop = (int)StatusIndicators.Start;
1619
1620 //returncollisions = true;
1621 break;
1622
1623 case ActorTypes.Prim:
1624 if (p1 is OdePrim)
1625 {
1626 cp1 = (OdePrim) p1;
1627 obj2LocalID = cp1.LocalID;
1628 cp1.AddCollisionEvent(cc2.LocalID, contact);
1629 }
1630 //ctype = (int)CollisionCategories.Geom;
1631
1632 //if (cp1.CollidingObj)
1633 //cStartStop = (int)StatusIndicators.Generic;
1634 //else
1635 //cStartStop = (int)StatusIndicators.Start;
1636
1637 //returncollisions = true;
1638 break;
1639
1640 case ActorTypes.Ground:
1641 case ActorTypes.Unknown:
1642 obj2LocalID = 0;
1643 //ctype = (int)CollisionCategories.Land;
1644 //returncollisions = true;
1645 break;
1646 }
1647
1648 cc2.AddCollisionEvent(obj2LocalID, contact);
1649 break;
1650
1183 case ActorTypes.Prim: 1651 case ActorTypes.Prim:
1652
1653 if (p2 is OdePrim)
1184 { 1654 {
1185 switch ((ActorTypes)p2.PhysicsActorType) 1655 cp2 = (OdePrim) p2;
1656
1657 // obj1LocalID = cp2.m_localID;
1658 switch ((ActorTypes) p1.PhysicsActorType)
1186 { 1659 {
1187 case ActorTypes.Agent: 1660 case ActorTypes.Agent:
1661 if (p1 is OdeCharacter)
1662 {
1663 cc1 = (OdeCharacter) p1;
1664 obj2LocalID = cc1.LocalID;
1665 cc1.AddCollisionEvent(cp2.LocalID, contact);
1666 //ctype = (int)CollisionCategories.Character;
1667
1668 //if (cc1.CollidingObj)
1669 //cStartStop = (int)StatusIndicators.Generic;
1670 //else
1671 //cStartStop = (int)StatusIndicators.Start;
1672 //returncollisions = true;
1673 }
1674 break;
1188 case ActorTypes.Prim: 1675 case ActorTypes.Prim:
1189 if (p2events) 1676
1677 if (p1 is OdePrim)
1190 { 1678 {
1191 AddCollisionEventReporting(p2); 1679 cp1 = (OdePrim) p1;
1192 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); 1680 obj2LocalID = cp1.LocalID;
1681 cp1.AddCollisionEvent(cp2.LocalID, contact);
1682 //ctype = (int)CollisionCategories.Geom;
1683
1684 //if (cp1.CollidingObj)
1685 //cStartStop = (int)StatusIndicators.Generic;
1686 //else
1687 //cStartStop = (int)StatusIndicators.Start;
1688
1689 //returncollisions = true;
1193 } 1690 }
1194 obj2LocalID = p2.ParentActor.LocalID;
1195 break; 1691 break;
1196 1692
1197 case ActorTypes.Ground: 1693 case ActorTypes.Ground:
1198 case ActorTypes.Unknown: 1694 case ActorTypes.Unknown:
1199 default:
1200 obj2LocalID = 0; 1695 obj2LocalID = 0;
1696 //ctype = (int)CollisionCategories.Land;
1697
1698 //returncollisions = true;
1201 break; 1699 break;
1202 } 1700 }
1203 if (p1events) 1701
1702 cp2.AddCollisionEvent(obj2LocalID, contact);
1703 }
1704 break;
1705 }
1706 //if (returncollisions)
1707 //{
1708
1709 //lock (m_storedCollisions)
1710 //{
1711 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1712 //if (m_storedCollisions.ContainsKey(cDictKey))
1713 //{
1714 //sCollisionData objd = m_storedCollisions[cDictKey];
1715 //objd.NumberOfCollisions += 1;
1716 //objd.lastframe = framecount;
1717 //m_storedCollisions[cDictKey] = objd;
1718 //}
1719 //else
1720 //{
1721 //sCollisionData objd = new sCollisionData();
1722 //objd.ColliderLocalId = obj1LocalID;
1723 //objd.CollidedWithLocalId = obj2LocalID;
1724 //objd.CollisionType = ctype;
1725 //objd.NumberOfCollisions = 1;
1726 //objd.lastframe = framecount;
1727 //objd.StatusIndicator = cStartStop;
1728 //m_storedCollisions.Add(cDictKey, objd);
1729 //}
1730 //}
1731 // }
1732 }
1733
1734 private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1735 {
1736 /* String name1 = null;
1737 String name2 = null;
1738
1739 if (!geom_name_map.TryGetValue(trimesh, out name1))
1204 { 1740 {
1205 contact.SurfaceNormal = -contact.SurfaceNormal; 1741 name1 = "null";
1206 AddCollisionEventReporting(p1);
1207 p1.AddCollisionEvent(obj2LocalID, contact);
1208 } 1742 }
1209 break; 1743 if (!geom_name_map.TryGetValue(refObject, out name2))
1210 }
1211 case ActorTypes.Ground:
1212 case ActorTypes.Unknown:
1213 default:
1214 {
1215 if (p2events && !p2.IsVolumeDtc)
1216 { 1744 {
1217 AddCollisionEventReporting(p2); 1745 name2 = "null";
1218 p2.AddCollisionEvent(0, contact);
1219 } 1746 }
1220 break; 1747
1221 } 1748 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
1222 } 1749 */
1750 return 1;
1751 }
1752
1753 private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1754 {
1755// String name1 = null;
1756// String name2 = null;
1757//
1758// if (!geom_name_map.TryGetValue(trimesh, out name1))
1759// {
1760// name1 = "null";
1761// }
1762//
1763// if (!geom_name_map.TryGetValue(refObject, out name2))
1764// {
1765// name2 = "null";
1766// }
1767
1768 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1769
1770 d.Vector3 v0 = new d.Vector3();
1771 d.Vector3 v1 = new d.Vector3();
1772 d.Vector3 v2 = new d.Vector3();
1773
1774 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1775 // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
1776
1777 return 1;
1223 } 1778 }
1224 1779
1225 /// <summary> 1780 /// <summary>
1226 /// This is our collision testing routine in ODE 1781 /// This is our collision testing routine in ODE
1227 /// </summary> 1782 /// </summary>
1228 /// <param name="timeStep"></param>
1229 private void collision_optimized() 1783 private void collision_optimized()
1230 { 1784 {
1231 lock (_characters) 1785 _perloopContact.Clear();
1232 { 1786
1787 foreach (OdeCharacter chr in _characters)
1788 {
1789 // Reset the collision values to false
1790 // since we don't know if we're colliding yet
1791 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1792 continue;
1793
1794 chr.IsColliding = false;
1795 chr.CollidingGround = false;
1796 chr.CollidingObj = false;
1797
1798 // Test the avatar's geometry for collision with the space
1799 // This will return near and the space that they are the closest to
1800 // And we'll run this again against the avatar and the space segment
1801 // This will return with a bunch of possible objects in the space segment
1802 // and we'll run it again on all of them.
1233 try 1803 try
1234 { 1804 {
1235 foreach (OdeCharacter chr in _characters) 1805 CollideSpaces(space, chr.Shell, IntPtr.Zero);
1236 {
1237 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1238 continue;
1239
1240 chr.IsColliding = false;
1241 // chr.CollidingGround = false; not done here
1242 chr.CollidingObj = false;
1243 // do colisions with static space
1244 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1245 // no coll with gnd
1246 }
1247 } 1806 }
1248 catch (AccessViolationException) 1807 catch (AccessViolationException)
1249 { 1808 {
1250 m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); 1809 m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name);
1251 } 1810 }
1252 1811
1812 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1813 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1814 //{
1815 //chr.Position.Z = terrainheight + 10.0f;
1816 //forcedZ = true;
1817 //}
1253 } 1818 }
1254 1819
1255 lock (_activeprims) 1820 if (CollectStats)
1256 { 1821 {
1257 foreach (OdePrim aprim in _activeprims) 1822 m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
1258 { 1823 m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
1259 aprim.CollisionScore = 0;
1260 aprim.IsColliding = false;
1261 }
1262 } 1824 }
1263 1825
1264 // collide active prims with static enviroment 1826 List<OdePrim> removeprims = null;
1265 lock (_activegroups) 1827 foreach (OdePrim chr in _activeprims)
1266 { 1828 {
1267 try 1829 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
1268 { 1830 {
1269 foreach (OdePrim prm in _activegroups) 1831 try
1270 { 1832 {
1271 if (!prm.m_outbounds) 1833 lock (chr)
1272 { 1834 {
1273 if (d.BodyIsEnabled(prm.Body)) 1835 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1836 {
1837 CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
1838 }
1839 else
1274 { 1840 {
1275 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); 1841 if (removeprims == null)
1276 d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); 1842 {
1843 removeprims = new List<OdePrim>();
1844 }
1845 removeprims.Add(chr);
1846 m_log.Error(
1847 "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!");
1277 } 1848 }
1278 } 1849 }
1279 } 1850 }
1851 catch (AccessViolationException)
1852 {
1853 m_log.Error("[ODE SCENE]: Unable to space collide");
1854 }
1280 } 1855 }
1281 catch (AccessViolationException) 1856 }
1857
1858 if (CollectStats)
1859 m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
1860
1861 if (removeprims != null)
1862 {
1863 foreach (OdePrim chr in removeprims)
1282 { 1864 {
1283 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); 1865 _activeprims.Remove(chr);
1284 } 1866 }
1285 } 1867 }
1286 // finally colide active things amoung them 1868 }
1287 try 1869
1870 #endregion
1871
1872 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1873 {
1874 m_worldOffset = offset;
1875 WorldExtents = new Vector2(extents.X, extents.Y);
1876 m_parentScene = pScene;
1877 }
1878
1879 // Recovered for use by fly height. Kitto Flora
1880 internal float GetTerrainHeightAtXY(float x, float y)
1881 {
1882 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1883 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1884
1885 IntPtr heightFieldGeom = IntPtr.Zero;
1886
1887 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1288 { 1888 {
1289 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); 1889 if (heightFieldGeom != IntPtr.Zero)
1890 {
1891 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1892 {
1893
1894 int index;
1895
1896
1897 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1898 (int)x < 0.001f || (int)y < 0.001f)
1899 return 0;
1900
1901 x = x - offsetX;
1902 y = y - offsetY;
1903
1904 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1905
1906 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1907 {
1908 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1909 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1910 }
1911
1912 else
1913 return 0f;
1914 }
1915 else
1916 {
1917 return 0f;
1918 }
1919
1920 }
1921 else
1922 {
1923 return 0f;
1924 }
1925
1290 } 1926 }
1291 catch (AccessViolationException) 1927 else
1292 { 1928 {
1293 m_log.Warn("[PHYSICS]: Unable to collide in Active space"); 1929 return 0f;
1294 } 1930 }
1295// _perloopContact.Clear(); 1931 }
1296 } 1932// End recovered. Kitto Flora
1297 1933
1298 #endregion
1299 /// <summary> 1934 /// <summary>
1300 /// Add actor to the list that should receive collision events in the simulate loop. 1935 /// Add actor to the list that should receive collision events in the simulate loop.
1301 /// </summary> 1936 /// </summary>
1302 /// <param name="obj"></param> 1937 /// <param name="obj"></param>
1303 public void AddCollisionEventReporting(PhysicsActor obj) 1938 internal void AddCollisionEventReporting(PhysicsActor obj)
1304 { 1939 {
1305 if (!_collisionEventPrim.Contains(obj)) 1940// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
1306 _collisionEventPrim.Add(obj); 1941
1942 lock (m_collisionEventActorsChanges)
1943 m_collisionEventActorsChanges[obj.LocalID] = obj;
1307 } 1944 }
1308 1945
1309 /// <summary> 1946 /// <summary>
1310 /// Remove actor from the list that should receive collision events in the simulate loop. 1947 /// Remove actor from the list that should receive collision events in the simulate loop.
1311 /// </summary> 1948 /// </summary>
1312 /// <param name="obj"></param> 1949 /// <param name="obj"></param>
1313 public void RemoveCollisionEventReporting(PhysicsActor obj) 1950 internal void RemoveCollisionEventReporting(PhysicsActor obj)
1314 { 1951 {
1315 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) 1952// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
1316 _collisionEventPrimRemove.Add(obj);
1317 }
1318 1953
1319 public override float TimeDilation 1954 lock (m_collisionEventActorsChanges)
1320 { 1955 m_collisionEventActorsChanges[obj.LocalID] = null;
1321 get { return m_timeDilation; }
1322 }
1323
1324 public override bool SupportsNINJAJoints
1325 {
1326 get { return false; }
1327 } 1956 }
1328 1957
1329 #region Add/Remove Entities 1958 #region Add/Remove Entities
@@ -1334,118 +1963,470 @@ namespace OpenSim.Region.Physics.OdePlugin
1334 pos.X = position.X; 1963 pos.X = position.X;
1335 pos.Y = position.Y; 1964 pos.Y = position.Y;
1336 pos.Z = position.Z; 1965 pos.Z = position.Z;
1337 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); 1966
1967 OdeCharacter newAv
1968 = new OdeCharacter(
1969 avName, this, pos, size, avPIDD, avPIDP,
1970 avCapRadius, avStandupTensor, avDensity,
1971 avMovementDivisorWalk, avMovementDivisorRun);
1972
1338 newAv.Flying = isFlying; 1973 newAv.Flying = isFlying;
1339 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; 1974 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1340 1975
1341 return newAv; 1976 return newAv;
1342 } 1977 }
1343 1978
1344 public void AddCharacter(OdeCharacter chr) 1979 public override void RemoveAvatar(PhysicsActor actor)
1980 {
1981// m_log.DebugFormat(
1982// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
1983// actor.Name, actor.LocalID, Name);
1984
1985 ((OdeCharacter) actor).Destroy();
1986 }
1987
1988 internal void AddCharacter(OdeCharacter chr)
1345 { 1989 {
1346 lock (_characters) 1990 if (!_characters.Contains(chr))
1347 { 1991 {
1348 if (!_characters.Contains(chr)) 1992 _characters.Add(chr);
1349 { 1993
1350 _characters.Add(chr); 1994// m_log.DebugFormat(
1351 if (chr.bad) 1995// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}",
1352 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); 1996// chr.Name, chr.LocalID, Name, _characters.Count);
1353 } 1997
1998 if (chr.bad)
1999 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
2000 }
2001 else
2002 {
2003 m_log.ErrorFormat(
2004 "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!",
2005 chr.Name, chr.LocalID);
1354 } 2006 }
1355 } 2007 }
1356 2008
1357 public void RemoveCharacter(OdeCharacter chr) 2009 internal void RemoveCharacter(OdeCharacter chr)
1358 { 2010 {
1359 lock (_characters) 2011 if (_characters.Contains(chr))
1360 { 2012 {
1361 if (_characters.Contains(chr)) 2013 _characters.Remove(chr);
1362 { 2014
1363 _characters.Remove(chr); 2015// m_log.DebugFormat(
1364 } 2016// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}",
2017// chr.Name, chr.LocalID, Name, _characters.Count);
2018 }
2019 else
2020 {
2021 m_log.ErrorFormat(
2022 "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!",
2023 chr.Name, chr.LocalID);
1365 } 2024 }
1366 } 2025 }
1367 2026
1368 public void BadCharacter(OdeCharacter chr) 2027 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
2028 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1369 { 2029 {
1370 lock (_badCharacter) 2030 Vector3 pos = position;
2031 Vector3 siz = size;
2032 Quaternion rot = rotation;
2033
2034 OdePrim newPrim;
2035 lock (OdeLock)
1371 { 2036 {
1372 if (!_badCharacter.Contains(chr)) 2037 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
1373 _badCharacter.Add(chr); 2038
2039 lock (_prims)
2040 _prims.Add(newPrim);
1374 } 2041 }
2042 newPrim.LocalID = localID;
2043 return newPrim;
1375 } 2044 }
1376 2045
1377 public override void RemoveAvatar(PhysicsActor actor) 2046 /// <summary>
2047 /// Make this prim subject to physics.
2048 /// </summary>
2049 /// <param name="prim"></param>
2050 internal void ActivatePrim(OdePrim prim)
1378 { 2051 {
1379 //m_log.Debug("[PHYSICS]:ODELOCK"); 2052 // adds active prim.. (ones that should be iterated over in collisions_optimized
1380 ((OdeCharacter) actor).Destroy(); 2053 if (!_activeprims.Contains(prim))
2054 _activeprims.Add(prim);
2055 //else
2056 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
2057 }
2058
2059 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
2060 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
2061 {
2062// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
2063
2064 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1381 } 2065 }
1382 2066
2067 public override float TimeDilation
2068 {
2069 get { return m_timeDilation; }
2070 }
2071
2072 public override bool SupportsNINJAJoints
2073 {
2074 get { return m_NINJA_physics_joints_enabled; }
2075 }
2076
2077 // internal utility function: must be called within a lock (OdeLock)
2078 private void InternalAddActiveJoint(PhysicsJoint joint)
2079 {
2080 activeJoints.Add(joint);
2081 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
2082 }
1383 2083
1384 public void addActivePrim(OdePrim activatePrim) 2084 // internal utility function: must be called within a lock (OdeLock)
2085 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1385 { 2086 {
1386 // adds active prim.. 2087 pendingJoints.Add(joint);
1387 lock (_activeprims) 2088 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
2089 }
2090
2091 // internal utility function: must be called within a lock (OdeLock)
2092 private void InternalRemovePendingJoint(PhysicsJoint joint)
2093 {
2094 pendingJoints.Remove(joint);
2095 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
2096 }
2097
2098 // internal utility function: must be called within a lock (OdeLock)
2099 private void InternalRemoveActiveJoint(PhysicsJoint joint)
2100 {
2101 activeJoints.Remove(joint);
2102 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
2103 }
2104
2105 public override void DumpJointInfo()
2106 {
2107 string hdr = "[NINJA] JOINTINFO: ";
2108 foreach (PhysicsJoint j in pendingJoints)
2109 {
2110 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2111 }
2112 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
2113 foreach (string jointName in SOPName_to_pendingJoint.Keys)
2114 {
2115 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
2116 }
2117 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
2118 foreach (PhysicsJoint j in activeJoints)
2119 {
2120 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2121 }
2122 m_log.Debug(hdr + activeJoints.Count + " total active joints");
2123 foreach (string jointName in SOPName_to_activeJoint.Keys)
2124 {
2125 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
2126 }
2127 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
2128
2129 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
2130 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
2131 foreach (string actorName in joints_connecting_actor.Keys)
1388 { 2132 {
1389 if (!_activeprims.Contains(activatePrim)) 2133 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1390 _activeprims.Add(activatePrim); 2134 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
2135 {
2136 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2137 }
2138 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1391 } 2139 }
1392 } 2140 }
1393 2141
1394 public void addActiveGroups(OdePrim activatePrim) 2142 public override void RequestJointDeletion(string ObjectNameInScene)
1395 { 2143 {
1396 lock (_activegroups) 2144 lock (externalJointRequestsLock)
1397 { 2145 {
1398 if (!_activegroups.Contains(activatePrim)) 2146 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1399 _activegroups.Add(activatePrim); 2147 {
2148 requestedJointsToBeDeleted.Add(ObjectNameInScene);
2149 }
1400 } 2150 }
1401 } 2151 }
1402 2152
1403 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, 2153 private void DeleteRequestedJoints()
1404 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1405 { 2154 {
1406 OdePrim newPrim; 2155 List<string> myRequestedJointsToBeDeleted;
1407 lock (OdeLock) 2156 lock (externalJointRequestsLock)
1408 { 2157 {
1409 newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); 2158 // make a local copy of the shared list for processing (threading issues)
1410 lock (_prims) 2159 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1411 _prims.Add(newPrim); 2160 }
2161
2162 foreach (string jointName in myRequestedJointsToBeDeleted)
2163 {
2164 lock (OdeLock)
2165 {
2166 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
2167 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
2168 {
2169 OdePhysicsJoint joint = null;
2170 if (SOPName_to_activeJoint.ContainsKey(jointName))
2171 {
2172 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
2173 InternalRemoveActiveJoint(joint);
2174 }
2175 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
2176 {
2177 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
2178 InternalRemovePendingJoint(joint);
2179 }
2180
2181 if (joint != null)
2182 {
2183 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
2184 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2185 {
2186 string bodyName = joint.BodyNames[iBodyName];
2187 if (bodyName != "NULL")
2188 {
2189 joints_connecting_actor[bodyName].Remove(joint);
2190 if (joints_connecting_actor[bodyName].Count == 0)
2191 {
2192 joints_connecting_actor.Remove(bodyName);
2193 }
2194 }
2195 }
2196
2197 DoJointDeactivated(joint);
2198 if (joint.jointID != IntPtr.Zero)
2199 {
2200 d.JointDestroy(joint.jointID);
2201 joint.jointID = IntPtr.Zero;
2202 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
2203 }
2204 else
2205 {
2206 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
2207 }
2208 }
2209 else
2210 {
2211 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
2212 }
2213 }
2214 else
2215 {
2216 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
2217 }
2218 }
2219 }
2220
2221 // remove processed joints from the shared list
2222 lock (externalJointRequestsLock)
2223 {
2224 foreach (string jointName in myRequestedJointsToBeDeleted)
2225 {
2226 requestedJointsToBeDeleted.Remove(jointName);
2227 }
1412 } 2228 }
1413 return newPrim;
1414 } 2229 }
1415 2230
1416 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 2231 // for pending joints we don't know if their associated bodies exist yet or not.
1417 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) 2232 // the joint is actually created during processing of the taints
2233 private void CreateRequestedJoints()
1418 { 2234 {
1419 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); 2235 List<PhysicsJoint> myRequestedJointsToBeCreated;
1420 } 2236 lock (externalJointRequestsLock)
2237 {
2238 // make a local copy of the shared list for processing (threading issues)
2239 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
2240 }
2241
2242 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2243 {
2244 lock (OdeLock)
2245 {
2246 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
2247 {
2248 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2249 continue;
2250 }
2251 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
2252 {
2253 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2254 continue;
2255 }
1421 2256
2257 InternalAddPendingJoint(joint as OdePhysicsJoint);
1422 2258
1423 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 2259 if (joint.BodyNames.Count >= 2)
1424 Vector3 size, Quaternion rotation, bool isPhysical, uint localid) 2260 {
2261 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2262 {
2263 string bodyName = joint.BodyNames[iBodyName];
2264 if (bodyName != "NULL")
2265 {
2266 if (!joints_connecting_actor.ContainsKey(bodyName))
2267 {
2268 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
2269 }
2270 joints_connecting_actor[bodyName].Add(joint);
2271 }
2272 }
2273 }
2274 }
2275 }
2276
2277 // remove processed joints from shared list
2278 lock (externalJointRequestsLock)
2279 {
2280 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2281 {
2282 requestedJointsToBeCreated.Remove(joint);
2283 }
2284 }
2285 }
2286
2287 /// <summary>
2288 /// Add a request for joint creation.
2289 /// </summary>
2290 /// <remarks>
2291 /// this joint will just be added to a waiting list that is NOT processed during the main
2292 /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2293 /// </remarks>
2294 /// <param name="objectNameInScene"></param>
2295 /// <param name="jointType"></param>
2296 /// <param name="position"></param>
2297 /// <param name="rotation"></param>
2298 /// <param name="parms"></param>
2299 /// <param name="bodyNames"></param>
2300 /// <param name="trackedBodyName"></param>
2301 /// <param name="localRotation"></param>
2302 /// <returns></returns>
2303 public override PhysicsJoint RequestJointCreation(
2304 string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2305 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
1425 { 2306 {
1426 return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); 2307 OdePhysicsJoint joint = new OdePhysicsJoint();
2308 joint.ObjectNameInScene = objectNameInScene;
2309 joint.Type = jointType;
2310 joint.Position = position;
2311 joint.Rotation = rotation;
2312 joint.RawParams = parms;
2313 joint.BodyNames = new List<string>(bodyNames);
2314 joint.TrackedBodyName = trackedBodyName;
2315 joint.LocalRotation = localRotation;
2316 joint.jointID = IntPtr.Zero;
2317 joint.ErrorMessageCount = 0;
2318
2319 lock (externalJointRequestsLock)
2320 {
2321 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2322 {
2323 requestedJointsToBeCreated.Add(joint);
2324 }
2325 }
2326
2327 return joint;
1427 } 2328 }
1428 2329
1429 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 2330 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
1430 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1431 { 2331 {
2332 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2333 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2334 {
2335 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2336 //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
2337 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2338 {
2339 jointsToRemove.Add(j);
2340 }
2341 foreach (PhysicsJoint j in jointsToRemove)
2342 {
2343 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2344 RequestJointDeletion(j.ObjectNameInScene);
2345 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2346 j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
2347 }
2348 }
2349 }
1432 2350
1433 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); 2351 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2352 {
2353 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2354 lock (OdeLock)
2355 {
2356 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2357 RemoveAllJointsConnectedToActor(actor);
2358 }
1434 } 2359 }
1435 2360
1436 public void remActivePrim(OdePrim deactivatePrim) 2361 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2362 public override Vector3 GetJointAnchor(PhysicsJoint joint)
1437 { 2363 {
1438 lock (_activeprims) 2364 Debug.Assert(joint.IsInPhysicsEngine);
2365 d.Vector3 pos = new d.Vector3();
2366
2367 if (!(joint is OdePhysicsJoint))
1439 { 2368 {
1440 _activeprims.Remove(deactivatePrim); 2369 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
1441 } 2370 }
2371 else
2372 {
2373 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2374 switch (odeJoint.Type)
2375 {
2376 case PhysicsJointType.Ball:
2377 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2378 break;
2379 case PhysicsJointType.Hinge:
2380 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2381 break;
2382 }
2383 }
2384 return new Vector3(pos.X, pos.Y, pos.Z);
1442 } 2385 }
1443 public void remActiveGroup(OdePrim deactivatePrim) 2386
2387 /// <summary>
2388 /// Get joint axis.
2389 /// </summary>
2390 /// <remarks>
2391 /// normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2392 /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2393 /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2394 /// keeping track of the joint's original orientation relative to one of the involved bodies.
2395 /// </remarks>
2396 /// <param name="joint"></param>
2397 /// <returns></returns>
2398 public override Vector3 GetJointAxis(PhysicsJoint joint)
1444 { 2399 {
1445 lock (_activegroups) 2400 Debug.Assert(joint.IsInPhysicsEngine);
2401 d.Vector3 axis = new d.Vector3();
2402
2403 if (!(joint is OdePhysicsJoint))
1446 { 2404 {
1447 _activegroups.Remove(deactivatePrim); 2405 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
1448 } 2406 }
2407 else
2408 {
2409 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2410 switch (odeJoint.Type)
2411 {
2412 case PhysicsJointType.Ball:
2413 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2414 break;
2415 case PhysicsJointType.Hinge:
2416 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2417 break;
2418 }
2419 }
2420 return new Vector3(axis.X, axis.Y, axis.Z);
2421 }
2422
2423 /// <summary>
2424 /// Stop this prim being subject to physics
2425 /// </summary>
2426 /// <param name="prim"></param>
2427 internal void DeactivatePrim(OdePrim prim)
2428 {
2429 _activeprims.Remove(prim);
1449 } 2430 }
1450 2431
1451 public override void RemovePrim(PhysicsActor prim) 2432 public override void RemovePrim(PhysicsActor prim)
@@ -1454,14 +2435,16 @@ namespace OpenSim.Region.Physics.OdePlugin
1454 // removed in the next physics simulate pass. 2435 // removed in the next physics simulate pass.
1455 if (prim is OdePrim) 2436 if (prim is OdePrim)
1456 { 2437 {
1457// lock (OdeLock) 2438 lock (OdeLock)
1458 { 2439 {
1459 2440 OdePrim p = (OdePrim) prim;
1460 OdePrim p = (OdePrim)prim; 2441
1461 p.setPrimForRemoval(); 2442 p.setPrimForRemoval();
2443 AddPhysicsActorTaint(prim);
1462 } 2444 }
1463 } 2445 }
1464 } 2446 }
2447
1465 /// <summary> 2448 /// <summary>
1466 /// This is called from within simulate but outside the locked portion 2449 /// This is called from within simulate but outside the locked portion
1467 /// We need to do our own locking here 2450 /// We need to do our own locking here
@@ -1474,37 +2457,89 @@ namespace OpenSim.Region.Physics.OdePlugin
1474 /// that the space was using. 2457 /// that the space was using.
1475 /// </summary> 2458 /// </summary>
1476 /// <param name="prim"></param> 2459 /// <param name="prim"></param>
1477 public void RemovePrimThreadLocked(OdePrim prim) 2460 internal void RemovePrimThreadLocked(OdePrim prim)
1478 { 2461 {
1479 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); 2462// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
2463
1480 lock (prim) 2464 lock (prim)
1481 { 2465 {
1482// RemoveCollisionEventReporting(prim); 2466 RemoveCollisionEventReporting(prim);
1483 lock (_prims)
1484 _prims.Remove(prim);
1485 }
1486 2467
1487 } 2468 if (prim.prim_geom != IntPtr.Zero)
2469 {
2470 prim.ResetTaints();
1488 2471
1489 public bool havePrim(OdePrim prm) 2472 if (prim.IsPhysical)
1490 { 2473 {
1491 lock (_prims) 2474 prim.disableBody();
1492 return _prims.Contains(prm); 2475 if (prim.childPrim)
1493 } 2476 {
2477 prim.childPrim = false;
2478 prim.Body = IntPtr.Zero;
2479 prim.m_disabled = true;
2480 prim.IsPhysical = false;
2481 }
1494 2482
1495 public bool haveActor(PhysicsActor actor) 2483
1496 { 2484 }
1497 if (actor is OdePrim) 2485 // we don't want to remove the main space
1498 { 2486
1499 lock (_prims) 2487 // If the geometry is in the targetspace, remove it from the target space
1500 return _prims.Contains((OdePrim)actor); 2488 //m_log.Warn(prim.m_targetSpace);
1501 } 2489
1502 else if (actor is OdeCharacter) 2490 //if (prim.m_targetSpace != IntPtr.Zero)
1503 { 2491 //{
1504 lock (_characters) 2492 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
1505 return _characters.Contains((OdeCharacter)actor); 2493 //{
2494
2495 //if (d.GeomIsSpace(prim.m_targetSpace))
2496 //{
2497 //waitForSpaceUnlock(prim.m_targetSpace);
2498 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2499 prim.m_targetSpace = IntPtr.Zero;
2500 //}
2501 //else
2502 //{
2503 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2504 //((OdePrim)prim).m_targetSpace.ToString());
2505 //}
2506
2507 //}
2508 //}
2509 //m_log.Warn(prim.prim_geom);
2510
2511 if (!prim.RemoveGeom())
2512 m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
2513
2514 lock (_prims)
2515 _prims.Remove(prim);
2516
2517 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2518 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2519 //{
2520 //if (prim.m_targetSpace != null)
2521 //{
2522 //if (d.GeomIsSpace(prim.m_targetSpace))
2523 //{
2524 //waitForSpaceUnlock(prim.m_targetSpace);
2525 //d.SpaceRemove(space, prim.m_targetSpace);
2526 // free up memory used by the space.
2527 //d.SpaceDestroy(prim.m_targetSpace);
2528 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2529 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2530 //}
2531 //else
2532 //{
2533 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2534 //((OdePrim) prim).m_targetSpace.ToString());
2535 //}
2536 //}
2537 //}
2538
2539 if (SupportsNINJAJoints)
2540 RemoveAllJointsConnectedToActorThreadLocked(prim);
2541 }
1506 } 2542 }
1507 return false;
1508 } 2543 }
1509 2544
1510 #endregion 2545 #endregion
@@ -1512,435 +2547,682 @@ namespace OpenSim.Region.Physics.OdePlugin
1512 #region Space Separation Calculation 2547 #region Space Separation Calculation
1513 2548
1514 /// <summary> 2549 /// <summary>
1515 /// Called when a static prim moves or becomes static 2550 /// Takes a space pointer and zeros out the array we're using to hold the spaces
1516 /// Places the prim in a space one the static sub-spaces grid 2551 /// </summary>
2552 /// <param name="pSpace"></param>
2553 private void resetSpaceArrayItemToZero(IntPtr pSpace)
2554 {
2555 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2556 {
2557 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2558 {
2559 if (staticPrimspace[x, y] == pSpace)
2560 staticPrimspace[x, y] = IntPtr.Zero;
2561 }
2562 }
2563 }
2564
2565// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2566// {
2567// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2568// }
2569
2570 /// <summary>
2571 /// Called when a static prim moves. Allocates a space for the prim based on its position
1517 /// </summary> 2572 /// </summary>
1518 /// <param name="geom">the pointer to the geom that moved</param> 2573 /// <param name="geom">the pointer to the geom that moved</param>
1519 /// <param name="pos">the position that the geom moved to</param> 2574 /// <param name="pos">the position that the geom moved to</param>
1520 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> 2575 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1521 /// <returns>a pointer to the new space it's in</returns> 2576 /// <returns>a pointer to the new space it's in</returns>
1522 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) 2577 internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
1523 { 2578 {
1524 // moves a prim into another static sub-space or from another space into a static sub-space 2579 // Called from setting the Position and Size of an ODEPrim so
1525
1526 // Called ODEPrim so
1527 // it's already in locked space. 2580 // it's already in locked space.
1528 2581
1529 if (geom == IntPtr.Zero) // shouldn't happen 2582 // we don't want to remove the main space
1530 return IntPtr.Zero; 2583 // we don't need to test physical here because this function should
2584 // never be called if the prim is physical(active)
1531 2585
1532 // get the static sub-space for current position 2586 // All physical prim end up in the root space
1533 IntPtr newspace = calculateSpaceForGeom(pos); 2587 //Thread.Sleep(20);
1534 2588 if (currentspace != space)
1535 if (newspace == currentspace) // if we are there all done
1536 return newspace;
1537
1538 // else remove it from its current space
1539 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1540 { 2589 {
1541 if (d.GeomIsSpace(currentspace)) 2590 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2591 //if (currentspace == IntPtr.Zero)
2592 //{
2593 //int adfadf = 0;
2594 //}
2595 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
1542 { 2596 {
1543 waitForSpaceUnlock(currentspace); 2597 if (d.GeomIsSpace(currentspace))
1544 d.SpaceRemove(currentspace, geom);
1545
1546 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1547 { 2598 {
1548 d.SpaceDestroy(currentspace); 2599// waitForSpaceUnlock(currentspace);
2600 d.SpaceRemove(currentspace, geom);
2601 }
2602 else
2603 {
2604 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2605 " Geom:" + geom);
1549 } 2606 }
1550 } 2607 }
1551 else 2608 else
1552 { 2609 {
1553 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + 2610 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
1554 " Geom:" + geom); 2611 if (sGeomIsIn != IntPtr.Zero)
2612 {
2613 if (d.GeomIsSpace(currentspace))
2614 {
2615// waitForSpaceUnlock(sGeomIsIn);
2616 d.SpaceRemove(sGeomIsIn, geom);
2617 }
2618 else
2619 {
2620 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2621 sGeomIsIn + " Geom:" + geom);
2622 }
2623 }
2624 }
2625
2626 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2627 if (d.SpaceGetNumGeoms(currentspace) == 0)
2628 {
2629 if (currentspace != IntPtr.Zero)
2630 {
2631 if (d.GeomIsSpace(currentspace))
2632 {
2633// waitForSpaceUnlock(currentspace);
2634// waitForSpaceUnlock(space);
2635 d.SpaceRemove(space, currentspace);
2636 // free up memory used by the space.
2637
2638 //d.SpaceDestroy(currentspace);
2639 resetSpaceArrayItemToZero(currentspace);
2640 }
2641 else
2642 {
2643 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2644 currentspace + " Geom:" + geom);
2645 }
2646 }
1555 } 2647 }
1556 } 2648 }
1557 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space 2649 else
1558 { 2650 {
1559 currentspace = d.GeomGetSpace(geom); 2651 // this is a physical object that got disabled. ;.;
1560 if (currentspace != IntPtr.Zero) 2652 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
1561 { 2653 {
1562 if (d.GeomIsSpace(currentspace)) 2654 if (d.SpaceQuery(currentspace, geom))
1563 { 2655 {
1564 waitForSpaceUnlock(currentspace); 2656 if (d.GeomIsSpace(currentspace))
1565 d.SpaceRemove(currentspace, geom);
1566
1567 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1568 { 2657 {
1569 d.SpaceDestroy(currentspace); 2658// waitForSpaceUnlock(currentspace);
2659 d.SpaceRemove(currentspace, geom);
2660 }
2661 else
2662 {
2663 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2664 currentspace + " Geom:" + geom);
2665 }
2666 }
2667 else
2668 {
2669 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2670 if (sGeomIsIn != IntPtr.Zero)
2671 {
2672 if (d.GeomIsSpace(sGeomIsIn))
2673 {
2674// waitForSpaceUnlock(sGeomIsIn);
2675 d.SpaceRemove(sGeomIsIn, geom);
2676 }
2677 else
2678 {
2679 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2680 sGeomIsIn + " Geom:" + geom);
2681 }
1570 } 2682 }
1571
1572 } 2683 }
1573 } 2684 }
1574 } 2685 }
1575 2686
1576 // put the geom in the newspace 2687 // The routines in the Position and Size sections do the 'inserting' into the space,
1577 waitForSpaceUnlock(newspace); 2688 // so all we have to do is make sure that the space that we're putting the prim into
1578 d.SpaceAdd(newspace, geom); 2689 // is in the 'main' space.
2690 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2691 IntPtr newspace = calculateSpaceForGeom(pos);
2692
2693 if (newspace == IntPtr.Zero)
2694 {
2695 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2696 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2697 }
1579 2698
1580 // let caller know this newspace
1581 return newspace; 2699 return newspace;
1582 } 2700 }
1583 2701
1584 /// <summary> 2702 /// <summary>
2703 /// Creates a new space at X Y
2704 /// </summary>
2705 /// <param name="iprimspaceArrItemX"></param>
2706 /// <param name="iprimspaceArrItemY"></param>
2707 /// <returns>A pointer to the created space</returns>
2708 internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2709 {
2710 // creating a new space for prim and inserting it into main space.
2711 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2712 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2713// waitForSpaceUnlock(space);
2714 d.SpaceSetSublevel(space, 1);
2715 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2716
2717 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2718 }
2719
2720 /// <summary>
1585 /// Calculates the space the prim should be in by its position 2721 /// Calculates the space the prim should be in by its position
1586 /// </summary> 2722 /// </summary>
1587 /// <param name="pos"></param> 2723 /// <param name="pos"></param>
1588 /// <returns>a pointer to the space. This could be a new space or reused space.</returns> 2724 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1589 public IntPtr calculateSpaceForGeom(Vector3 pos) 2725 internal IntPtr calculateSpaceForGeom(Vector3 pos)
2726 {
2727 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2728 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2729 return staticPrimspace[xyspace[0], xyspace[1]];
2730 }
2731
2732 /// <summary>
2733 /// Holds the space allocation logic
2734 /// </summary>
2735 /// <param name="pos"></param>
2736 /// <returns>an array item based on the position</returns>
2737 internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
1590 { 2738 {
1591 int x, y; 2739 int[] returnint = new int[2];
1592 2740
1593 if (pos.X < 0) 2741 returnint[0] = (int) (pos.X/metersInSpace);
1594 return staticPrimspaceOffRegion[0];
1595 2742
1596 if (pos.Y < 0) 2743 if (returnint[0] > ((int) (259f/metersInSpace)))
1597 return staticPrimspaceOffRegion[2]; 2744 returnint[0] = ((int) (259f/metersInSpace));
2745 if (returnint[0] < 0)
2746 returnint[0] = 0;
1598 2747
1599 x = (int)(pos.X * spacesPerMeter); 2748 returnint[1] = (int) (pos.Y/metersInSpace);
1600 if (x > spaceGridMaxX) 2749 if (returnint[1] > ((int) (259f/metersInSpace)))
1601 return staticPrimspaceOffRegion[1]; 2750 returnint[1] = ((int) (259f/metersInSpace));
1602 2751 if (returnint[1] < 0)
1603 y = (int)(pos.Y * spacesPerMeter); 2752 returnint[1] = 0;
1604 if (y > spaceGridMaxY)
1605 return staticPrimspaceOffRegion[3];
1606 2753
1607 return staticPrimspace[x, y]; 2754 return returnint;
1608 } 2755 }
1609
1610 #endregion
1611 2756
2757 #endregion
1612 2758
1613 /// <summary> 2759 /// <summary>
1614 /// Called to queue a change to a actor 2760 /// Routine to figure out if we need to mesh this prim with our mesher
1615 /// to use in place of old taint mechanism so changes do have a time sequence
1616 /// </summary> 2761 /// </summary>
1617 2762 /// <param name="pbs"></param>
1618 public void AddChange(PhysicsActor actor, changes what, Object arg) 2763 /// <returns></returns>
2764 internal bool needsMeshing(PrimitiveBaseShape pbs)
1619 { 2765 {
1620 ODEchangeitem item = new ODEchangeitem(); 2766 // most of this is redundant now as the mesher will return null if it cant mesh a prim
1621 item.actor = actor; 2767 // but we still need to check for sculptie meshing being enabled so this is the most
1622 item.what = what; 2768 // convenient place to do it for now...
1623 item.arg = arg;
1624 ChangesQueue.Enqueue(item);
1625 }
1626 2769
1627 /// <summary> 2770 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
1628 /// Called after our prim properties are set Scale, position etc. 2771 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
1629 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex 2772 int iPropertiesNotSupportedDefault = 0;
1630 /// This assures us that we have no race conditions
1631 /// </summary>
1632 /// <param name="prim"></param>
1633 public override void AddPhysicsActorTaint(PhysicsActor prim)
1634 {
1635 }
1636 2773
1637 // does all pending changes generated during region load process 2774 if (pbs.SculptEntry && !meshSculptedPrim)
1638 public override void PrepareSimulation()
1639 {
1640 lock (OdeLock)
1641 { 2775 {
1642 if (world == IntPtr.Zero) 2776#if SPAM
2777 m_log.Warn("NonMesh");
2778#endif
2779 return false;
2780 }
2781
2782 // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
2783 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2784 {
2785 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2786 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2787 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1643 { 2788 {
1644 ChangesQueue.Clear(); 2789
1645 return; 2790 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2791 && pbs.ProfileHollow == 0
2792 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2793 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2794 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2795 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2796 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2797 {
2798#if SPAM
2799 m_log.Warn("NonMesh");
2800#endif
2801 return false;
2802 }
1646 } 2803 }
2804 }
2805
2806 if (pbs.ProfileHollow != 0)
2807 iPropertiesNotSupportedDefault++;
2808
2809 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
2810 iPropertiesNotSupportedDefault++;
2811
2812 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2813 iPropertiesNotSupportedDefault++;
2814
2815 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2816 iPropertiesNotSupportedDefault++;
2817
2818 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2819 iPropertiesNotSupportedDefault++;
2820
2821 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2822 iPropertiesNotSupportedDefault++;
1647 2823
1648 ODEchangeitem item; 2824 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2825 iPropertiesNotSupportedDefault++;
1649 2826
1650 int donechanges = 0; 2827 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
1651 if (ChangesQueue.Count > 0) 2828 iPropertiesNotSupportedDefault++;
2829
2830 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2831 iPropertiesNotSupportedDefault++;
2832
2833 // test for torus
2834 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2835 {
2836 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1652 { 2837 {
1653 m_log.InfoFormat("[ODE] start processing pending actor operations"); 2838 iPropertiesNotSupportedDefault++;
1654 int tstart = Util.EnvironmentTickCount(); 2839 }
2840 }
2841 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2842 {
2843 if (pbs.PathCurve == (byte)Extrusion.Straight)
2844 {
2845 iPropertiesNotSupportedDefault++;
2846 }
1655 2847
1656 while (ChangesQueue.Dequeue(out item)) 2848 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1657 { 2849 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1658 if (item.actor != null) 2850 {
1659 { 2851 iPropertiesNotSupportedDefault++;
1660 try 2852 }
1661 { 2853 }
1662 if (item.actor is OdeCharacter) 2854 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1663 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); 2855 {
1664 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) 2856 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1665 RemovePrimThreadLocked((OdePrim)item.actor); 2857 {
1666 } 2858 iPropertiesNotSupportedDefault++;
1667 catch 2859 }
1668 { 2860 }
1669 m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", 2861 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1670 item.actor.Name, item.what.ToString()); 2862 {
1671 } 2863 if (pbs.PathCurve == (byte)Extrusion.Straight)
1672 } 2864 {
1673 donechanges++; 2865 iPropertiesNotSupportedDefault++;
1674 } 2866 }
1675 int time = Util.EnvironmentTickCountSubtract(tstart); 2867 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1676 m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); 2868 {
2869 iPropertiesNotSupportedDefault++;
2870 }
2871 }
2872
2873 if (pbs.SculptEntry && meshSculptedPrim)
2874 iPropertiesNotSupportedDefault++;
2875
2876 if (iPropertiesNotSupportedDefault == 0)
2877 {
2878#if SPAM
2879 m_log.Warn("NonMesh");
2880#endif
2881 return false;
2882 }
2883#if SPAM
2884 m_log.Debug("Mesh");
2885#endif
2886 return true;
2887 }
2888
2889 /// <summary>
2890 /// Called after our prim properties are set Scale, position etc.
2891 /// </summary>
2892 /// <remarks>
2893 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2894 /// This assures us that we have no race conditions
2895 /// </remarks>
2896 /// <param name="actor"></param>
2897 public override void AddPhysicsActorTaint(PhysicsActor actor)
2898 {
2899 if (actor is OdePrim)
2900 {
2901 OdePrim taintedprim = ((OdePrim)actor);
2902 lock (_taintedPrims)
2903 _taintedPrims.Add(taintedprim);
2904 }
2905 else if (actor is OdeCharacter)
2906 {
2907 OdeCharacter taintedchar = ((OdeCharacter)actor);
2908 lock (_taintedActors)
2909 {
2910 _taintedActors.Add(taintedchar);
2911 if (taintedchar.bad)
2912 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
1677 } 2913 }
1678 } 2914 }
1679 } 2915 }
1680 2916
1681 /// <summary> 2917 /// <summary>
1682 /// This is our main simulate loop 2918 /// This is our main simulate loop
2919 /// </summary>
2920 /// <remarks>
1683 /// It's thread locked by a Mutex in the scene. 2921 /// It's thread locked by a Mutex in the scene.
1684 /// It holds Collisions, it instructs ODE to step through the physical reactions 2922 /// It holds Collisions, it instructs ODE to step through the physical reactions
1685 /// It moves the objects around in memory 2923 /// It moves the objects around in memory
1686 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) 2924 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1687 /// </summary> 2925 /// </remarks>
1688 /// <param name="timeStep"></param> 2926 /// <param name="timeStep"></param>
1689 /// <returns></returns> 2927 /// <returns>The number of frames simulated over that period.</returns>
1690 public override float Simulate(float timeStep) 2928 public override float Simulate(float timeStep)
1691 { 2929 {
2930 if (!_worldInitialized) return 11f;
1692 2931
1693 DateTime now = DateTime.UtcNow; 2932 int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
1694 TimeSpan timedif = now - m_lastframe; 2933 int tempTick = 0, tempTick2 = 0;
1695 m_lastframe = now;
1696 timeStep = (float)timedif.TotalSeconds;
1697
1698 // acumulate time so we can reduce error
1699 step_time += timeStep;
1700
1701 if (step_time < HalfOdeStep)
1702 return 0;
1703 2934
1704 if (framecount < 0) 2935 if (framecount >= int.MaxValue)
1705 framecount = 0; 2936 framecount = 0;
1706 2937
1707 framecount++; 2938 framecount++;
1708 2939
1709 int curphysiteractions; 2940 float fps = 0;
1710 2941
1711 // if in trouble reduce step resolution 2942 float timeLeft = timeStep;
1712 if (step_time >= m_SkipFramesAtms)
1713 curphysiteractions = m_physicsiterations / 2;
1714 else
1715 curphysiteractions = m_physicsiterations;
1716
1717 int nodeframes = 0;
1718
1719// checkThread();
1720 2943
1721 lock (SimulationLock) 2944 //m_log.Info(timeStep.ToString());
1722 lock(OdeLock) 2945// step_time += timeSte
2946//
2947// // If We're loaded down by something else,
2948// // or debugging with the Visual Studio project on pause
2949// // skip a few frames to catch up gracefully.
2950// // without shooting the physicsactors all over the place
2951//
2952// if (step_time >= m_SkipFramesAtms)
2953// {
2954// // Instead of trying to catch up, it'll do 5 physics frames only
2955// step_time = ODE_STEPSIZE;
2956// m_physicsiterations = 5;
2957// }
2958// else
2959// {
2960// m_physicsiterations = 10;
2961// }
2962
2963 // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
2964 // deadlock if the collision event tries to lock something else later on which is already locked by a
2965 // caller that is adding or removing the collision event.
2966 lock (m_collisionEventActorsChanges)
1723 { 2967 {
1724 if (world == IntPtr.Zero) 2968 foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
1725 { 2969 {
1726 ChangesQueue.Clear(); 2970 if (kvp.Value == null)
1727 return 0; 2971 m_collisionEventActors.Remove(kvp.Key);
2972 else
2973 m_collisionEventActors[kvp.Key] = kvp.Value;
1728 } 2974 }
1729 2975
1730 ODEchangeitem item; 2976 m_collisionEventActorsChanges.Clear();
1731 2977 }
1732 if (ChangesQueue.Count > 0)
1733 {
1734 int ttmpstart = Util.EnvironmentTickCount();
1735 int ttmp;
1736 2978
1737 while (ChangesQueue.Dequeue(out item)) 2979 if (SupportsNINJAJoints)
1738 { 2980 {
1739 if (item.actor != null) 2981 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
1740 { 2982 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
1741 try 2983 }
1742 {
1743 if (item.actor is OdeCharacter)
1744 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1745 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1746 RemovePrimThreadLocked((OdePrim)item.actor);
1747 }
1748 catch
1749 {
1750 m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
1751 item.actor.Name, item.what.ToString());
1752 }
1753 }
1754 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1755 if (ttmp > 20)
1756 break;
1757 }
1758 }
1759
1760 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1761 2984
1762 while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever 2985 lock (OdeLock)
2986 {
2987 // Process 10 frames if the sim is running normal..
2988 // process 5 frames if the sim is running slow
2989 //try
2990 //{
2991 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2992 //}
2993 //catch (StackOverflowException)
2994 //{
2995 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2996 // ode.drelease(world);
2997 //base.TriggerPhysicsBasedRestart();
2998 //}
2999
3000 // Figure out the Frames Per Second we're going at.
3001 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
3002
3003 fps = (timeStep / ODE_STEPSIZE) * 1000;
3004 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
3005 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
3006
3007 while (timeLeft > 0.0f)
1763 { 3008 {
1764 try 3009 try
1765 { 3010 {
1766 // clear pointer/counter to contacts to pass into joints 3011 if (CollectStats)
1767 m_global_contactcount = 0; 3012 tempTick = Util.EnvironmentTickCount();
1768 3013
3014 lock (_taintedActors)
3015 {
3016 foreach (OdeCharacter character in _taintedActors)
3017 character.ProcessTaints();
1769 3018
1770 // Move characters 3019 _taintedActors.Clear();
1771 lock (_characters) 3020 }
3021
3022 if (CollectStats)
1772 { 3023 {
1773 List<OdeCharacter> defects = new List<OdeCharacter>(); 3024 tempTick2 = Util.EnvironmentTickCount();
1774 foreach (OdeCharacter actor in _characters) 3025 m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
1775 { 3026 tempTick = tempTick2;
1776 if (actor != null) 3027 }
1777 actor.Move(ODE_STEPSIZE, defects); 3028
1778 } 3029 lock (_taintedPrims)
1779 if (defects.Count != 0) 3030 {
3031 foreach (OdePrim prim in _taintedPrims)
1780 { 3032 {
1781 foreach (OdeCharacter defect in defects) 3033 if (prim.m_taintremove)
3034 {
3035// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name);
3036 RemovePrimThreadLocked(prim);
3037 }
3038 else
1782 { 3039 {
1783 RemoveCharacter(defect); 3040// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name);
3041 prim.ProcessTaints();
1784 } 3042 }
1785 defects.Clear(); 3043
3044 prim.m_collisionscore = 0;
3045
3046 // This loop can block up the Heartbeat for a very long time on large regions.
3047 // We need to let the Watchdog know that the Heartbeat is not dead
3048 // NOTE: This is currently commented out, but if things like OAR loading are
3049 // timing the heartbeat out we will need to uncomment it
3050 //Watchdog.UpdateThread();
1786 } 3051 }
3052
3053 if (SupportsNINJAJoints)
3054 SimulatePendingNINJAJoints();
3055
3056 _taintedPrims.Clear();
1787 } 3057 }
1788 3058
1789 // Move other active objects 3059 if (CollectStats)
1790 lock (_activegroups)
1791 { 3060 {
1792 foreach (OdePrim aprim in _activegroups) 3061 tempTick2 = Util.EnvironmentTickCount();
3062 m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3063 tempTick = tempTick2;
3064 }
3065
3066 // Move characters
3067 foreach (OdeCharacter actor in _characters)
3068 actor.Move(defects);
3069
3070 if (defects.Count != 0)
3071 {
3072 foreach (OdeCharacter actor in defects)
1793 { 3073 {
1794 aprim.Move(); 3074 m_log.ErrorFormat(
3075 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving",
3076 actor.Name, actor.LocalID, Name);
3077
3078 RemoveCharacter(actor);
3079 actor.DestroyOdeStructures();
1795 } 3080 }
3081
3082 defects.Clear();
3083 }
3084
3085 if (CollectStats)
3086 {
3087 tempTick2 = Util.EnvironmentTickCount();
3088 m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3089 tempTick = tempTick2;
3090 }
3091
3092 // Move other active objects
3093 foreach (OdePrim prim in _activeprims)
3094 {
3095 prim.m_collisionscore = 0;
3096 prim.Move(timeStep);
3097 }
3098
3099 if (CollectStats)
3100 {
3101 tempTick2 = Util.EnvironmentTickCount();
3102 m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3103 tempTick = tempTick2;
1796 } 3104 }
1797 3105
1798 //if ((framecount % m_randomizeWater) == 0) 3106 //if ((framecount % m_randomizeWater) == 0)
1799 // randomizeWater(waterlevel); 3107 // randomizeWater(waterlevel);
1800 3108
3109 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
1801 m_rayCastManager.ProcessQueuedRequests(); 3110 m_rayCastManager.ProcessQueuedRequests();
1802 3111
3112 if (CollectStats)
3113 {
3114 tempTick2 = Util.EnvironmentTickCount();
3115 m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3116 tempTick = tempTick2;
3117 }
3118
1803 collision_optimized(); 3119 collision_optimized();
1804 3120
1805 foreach (PhysicsActor obj in _collisionEventPrim) 3121 if (CollectStats)
3122 {
3123 tempTick2 = Util.EnvironmentTickCount();
3124 m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3125 tempTick = tempTick2;
3126 }
3127
3128 foreach (PhysicsActor obj in m_collisionEventActors.Values)
1806 { 3129 {
1807 if (obj == null) 3130// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
1808 continue;
1809 3131
1810 switch ((ActorTypes)obj.PhysicsActorType) 3132 switch ((ActorTypes)obj.PhysicsActorType)
1811 { 3133 {
1812 case ActorTypes.Agent: 3134 case ActorTypes.Agent:
1813 OdeCharacter cobj = (OdeCharacter)obj; 3135 OdeCharacter cobj = (OdeCharacter)obj;
1814 cobj.AddCollisionFrameTime((int)(odetimestepMS)); 3136 cobj.AddCollisionFrameTime(100);
1815 cobj.SendCollisions(); 3137 cobj.SendCollisions();
1816 break; 3138 break;
1817 3139
1818 case ActorTypes.Prim: 3140 case ActorTypes.Prim:
1819 OdePrim pobj = (OdePrim)obj; 3141 OdePrim pobj = (OdePrim)obj;
1820 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) 3142 pobj.SendCollisions();
1821 if (!pobj.m_outbounds)
1822 {
1823 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1824 pobj.SendCollisions();
1825 }
1826 break; 3143 break;
1827 } 3144 }
1828 } 3145 }
1829 3146
1830 foreach (PhysicsActor obj in _collisionEventPrimRemove) 3147// if (m_global_contactcount > 0)
1831 _collisionEventPrim.Remove(obj); 3148// m_log.DebugFormat(
1832 3149// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
1833 _collisionEventPrimRemove.Clear();
1834 3150
1835 // do a ode simulation step 3151 m_global_contactcount = 0;
1836 d.WorldQuickStep(world, ODE_STEPSIZE);
1837 d.JointGroupEmpty(contactgroup);
1838 3152
1839 // update managed ideia of physical data and do updates to core 3153 if (CollectStats)
1840 /*
1841 lock (_characters)
1842 { 3154 {
1843 foreach (OdeCharacter actor in _characters) 3155 tempTick2 = Util.EnvironmentTickCount();
1844 { 3156 m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
1845 if (actor != null) 3157 tempTick = tempTick2;
1846 {
1847 if (actor.bad)
1848 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1849
1850 actor.UpdatePositionAndVelocity();
1851 }
1852 }
1853 } 3158 }
1854 */
1855 3159
1856 lock (_activegroups) 3160 d.WorldQuickStep(world, ODE_STEPSIZE);
1857 { 3161
1858 { 3162 if (CollectStats)
1859 foreach (OdePrim actor in _activegroups) 3163 m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
1860 { 3164
1861 if (actor.IsPhysical) 3165 d.JointGroupEmpty(contactgroup);
1862 {
1863 actor.UpdatePositionAndVelocity();
1864 }
1865 }
1866 }
1867 }
1868 } 3166 }
1869 catch (Exception e) 3167 catch (Exception e)
1870 { 3168 {
1871 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); 3169 m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1872// ode.dunlock(world);
1873 } 3170 }
1874 3171
3172 timeLeft -= ODE_STEPSIZE;
3173 }
1875 3174
1876 step_time -= ODE_STEPSIZE; 3175 if (CollectStats)
1877 nodeframes++; 3176 tempTick = Util.EnvironmentTickCount();
3177
3178 foreach (OdeCharacter actor in _characters)
3179 {
3180 if (actor.bad)
3181 m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3182
3183 actor.UpdatePositionAndVelocity(defects);
1878 } 3184 }
1879 3185
1880 lock (_badCharacter) 3186 if (defects.Count != 0)
1881 { 3187 {
1882 if (_badCharacter.Count > 0) 3188 foreach (OdeCharacter actor in defects)
1883 { 3189 {
1884 foreach (OdeCharacter chr in _badCharacter) 3190 m_log.ErrorFormat(
1885 { 3191 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity",
1886 RemoveCharacter(chr); 3192 actor.Name, actor.LocalID, Name);
1887 }
1888 3193
1889 _badCharacter.Clear(); 3194 RemoveCharacter(actor);
3195 actor.DestroyOdeStructures();
1890 } 3196 }
1891 }
1892 3197
1893 timedif = now - m_lastMeshExpire; 3198 defects.Clear();
3199 }
1894 3200
1895 if (timedif.Seconds > 10) 3201 if (CollectStats)
1896 { 3202 {
1897 mesher.ExpireReleaseMeshs(); 3203 tempTick2 = Util.EnvironmentTickCount();
1898 m_lastMeshExpire = now; 3204 m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3205 tempTick = tempTick2;
1899 } 3206 }
1900 3207
1901// information block running in debug only 3208 //if (timeStep < 0.2f)
1902/*
1903 int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1904 int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1905 int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
1906
1907 int nactivegeoms = 0;
1908 int nactivespaces = 0;
1909 3209
1910 int nstaticgeoms = 0; 3210 foreach (OdePrim prim in _activeprims)
1911 int nstaticspaces = 0;
1912 IntPtr sp;
1913
1914 for (int i = 0; i < ntopactivegeoms; i++)
1915 { 3211 {
1916 sp = d.SpaceGetGeom(ActiveSpace, i); 3212 if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag))
1917 if (d.GeomIsSpace(sp))
1918 { 3213 {
1919 nactivespaces++; 3214 prim.UpdatePositionAndVelocity();
1920 nactivegeoms += d.SpaceGetNumGeoms(sp);
1921 }
1922 else
1923 nactivegeoms++;
1924 }
1925 3215
1926 for (int i = 0; i < ntopstaticgeoms; i++) 3216 if (SupportsNINJAJoints)
1927 { 3217 SimulateActorPendingJoints(prim);
1928 sp = d.SpaceGetGeom(StaticSpace, i);
1929 if (d.GeomIsSpace(sp))
1930 {
1931 nstaticspaces++;
1932 nstaticgeoms += d.SpaceGetNumGeoms(sp);
1933 } 3218 }
1934 else
1935 nstaticgeoms++;
1936 } 3219 }
1937 3220
1938 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); 3221 if (CollectStats)
3222 m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
3223
3224 //DumpJointInfo();
1939 3225
1940 int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
1941 int nbodies = d.NTotalBodies;
1942 int ngeoms = d.NTotalGeoms;
1943*/
1944 // Finished with all sim stepping. If requested, dump world state to file for debugging. 3226 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1945 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? 3227 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1946 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? 3228 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
@@ -1959,26 +3241,256 @@ namespace OpenSim.Region.Physics.OdePlugin
1959 3241
1960 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); 3242 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1961 } 3243 }
1962 3244
1963 // think time dilation as to do with dinamic step size that we dont' have 3245 latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
1964 // even so tell something to world 3246
1965 if (nodeframes < 10) // we did the requested loops 3247 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics
3248 // has a max of 100 ms to run theoretically.
3249 // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
3250 // If Physics stalls, it takes longer which makes the tick count ms larger.
3251
3252 if (latertickcount < 100)
3253 {
1966 m_timeDilation = 1.0f; 3254 m_timeDilation = 1.0f;
1967 else if (step_time > 0) 3255 }
3256 else
1968 { 3257 {
1969 m_timeDilation = timeStep / step_time; 3258 m_timeDilation = 100f / latertickcount;
1970 if (m_timeDilation > 1) 3259 //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f);
1971 m_timeDilation = 1;
1972 if (step_time > m_SkipFramesAtms)
1973 step_time = 0;
1974 } 3260 }
3261
3262 tickCountFrameRun = Util.EnvironmentTickCount();
3263
3264 if (CollectStats)
3265 m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
1975 } 3266 }
1976 3267
1977// return nodeframes * ODE_STEPSIZE; // return real simulated time 3268 return fps;
1978 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
1979 } 3269 }
1980 3270
1981 /// <summary> 3271 /// <summary>
3272 /// Simulate pending NINJA joints.
3273 /// </summary>
3274 /// <remarks>
3275 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
3276 /// </remarks>
3277 private void SimulatePendingNINJAJoints()
3278 {
3279 // Create pending joints, if possible
3280
3281 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
3282 // a joint requires specifying the body id of both involved bodies
3283 if (pendingJoints.Count > 0)
3284 {
3285 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
3286 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
3287 foreach (PhysicsJoint joint in pendingJoints)
3288 {
3289 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
3290 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
3291 List<IntPtr> jointBodies = new List<IntPtr>();
3292 bool allJointBodiesAreReady = true;
3293 foreach (string jointParam in jointParams)
3294 {
3295 if (jointParam == "NULL")
3296 {
3297 //DoJointErrorMessage(joint, "attaching NULL joint to world");
3298 jointBodies.Add(IntPtr.Zero);
3299 }
3300 else
3301 {
3302 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
3303 bool foundPrim = false;
3304 lock (_prims)
3305 {
3306 foreach (OdePrim prim in _prims) // FIXME: inefficient
3307 {
3308 if (prim.SOPName == jointParam)
3309 {
3310 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
3311 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
3312 {
3313 jointBodies.Add(prim.Body);
3314 foundPrim = true;
3315 break;
3316 }
3317 else
3318 {
3319 DoJointErrorMessage(joint, "prim name " + jointParam +
3320 " exists but is not (yet) physical; deferring joint creation. " +
3321 "IsPhysical property is " + prim.IsPhysical +
3322 " and body is " + prim.Body);
3323 foundPrim = false;
3324 break;
3325 }
3326 }
3327 }
3328 }
3329 if (foundPrim)
3330 {
3331 // all is fine
3332 }
3333 else
3334 {
3335 allJointBodiesAreReady = false;
3336 break;
3337 }
3338 }
3339 }
3340
3341 if (allJointBodiesAreReady)
3342 {
3343 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3344 if (jointBodies[0] == jointBodies[1])
3345 {
3346 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
3347 }
3348 else
3349 {
3350 switch (joint.Type)
3351 {
3352 case PhysicsJointType.Ball:
3353 {
3354 IntPtr odeJoint;
3355 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3356 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3357 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3358 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3359 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3360 d.JointSetBallAnchor(odeJoint,
3361 joint.Position.X,
3362 joint.Position.Y,
3363 joint.Position.Z);
3364 //DoJointErrorMessage(joint, "ODE joint setting OK");
3365 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3366 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3367 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3368 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3369
3370 if (joint is OdePhysicsJoint)
3371 {
3372 ((OdePhysicsJoint)joint).jointID = odeJoint;
3373 }
3374 else
3375 {
3376 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3377 }
3378 }
3379 break;
3380 case PhysicsJointType.Hinge:
3381 {
3382 IntPtr odeJoint;
3383 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3384 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3385 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3386 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3387 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3388 d.JointSetHingeAnchor(odeJoint,
3389 joint.Position.X,
3390 joint.Position.Y,
3391 joint.Position.Z);
3392 // We use the orientation of the x-axis of the joint's coordinate frame
3393 // as the axis for the hinge.
3394
3395 // Therefore, we must get the joint's coordinate frame based on the
3396 // joint.Rotation field, which originates from the orientation of the
3397 // joint's proxy object in the scene.
3398
3399 // The joint's coordinate frame is defined as the transformation matrix
3400 // that converts a vector from joint-local coordinates into world coordinates.
3401 // World coordinates are defined as the XYZ coordinate system of the sim,
3402 // as shown in the top status-bar of the viewer.
3403
3404 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3405 // and use that as the hinge axis.
3406
3407 //joint.Rotation.Normalize();
3408 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3409
3410 // Now extract the X axis of the joint's coordinate frame.
3411
3412 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3413 // tar pit of transposed, inverted, and generally messed-up orientations.
3414 // (In other words, Matrix4.AtAxis() is borked.)
3415 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3416
3417 // Instead, compute the X axis of the coordinate frame by transforming
3418 // the (1,0,0) vector. At least that works.
3419
3420 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3421 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3422 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3423 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3424 d.JointSetHingeAxis(odeJoint,
3425 jointAxis.X,
3426 jointAxis.Y,
3427 jointAxis.Z);
3428 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3429 if (joint is OdePhysicsJoint)
3430 {
3431 ((OdePhysicsJoint)joint).jointID = odeJoint;
3432 }
3433 else
3434 {
3435 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3436 }
3437 }
3438 break;
3439 }
3440 successfullyProcessedPendingJoints.Add(joint);
3441 }
3442 }
3443 else
3444 {
3445 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3446 }
3447 }
3448
3449 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
3450 {
3451 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
3452 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
3453 InternalRemovePendingJoint(successfullyProcessedJoint);
3454 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
3455 InternalAddActiveJoint(successfullyProcessedJoint);
3456 //DoJointErrorMessage(successfullyProcessedJoint, "done");
3457 }
3458 }
3459 }
3460
3461 /// <summary>
3462 /// Simulate the joint proxies of a NINJA actor.
3463 /// </summary>
3464 /// <remarks>
3465 /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
3466 /// </remarks>
3467 /// <param name="actor"></param>
3468 private void SimulateActorPendingJoints(OdePrim actor)
3469 {
3470 // If an actor moved, move its joint proxy objects as well.
3471 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3472 // for this purpose but it is never called! So we just do the joint
3473 // movement code here.
3474
3475 if (actor.SOPName != null &&
3476 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3477 joints_connecting_actor[actor.SOPName] != null &&
3478 joints_connecting_actor[actor.SOPName].Count > 0)
3479 {
3480 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3481 {
3482 if (affectedJoint.IsInPhysicsEngine)
3483 {
3484 DoJointMoved(affectedJoint);
3485 }
3486 else
3487 {
3488 DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
3489 }
3490 }
3491 }
3492 }
3493
1982 public override void GetResults() 3494 public override void GetResults()
1983 { 3495 {
1984 } 3496 }
@@ -1986,141 +3498,275 @@ namespace OpenSim.Region.Physics.OdePlugin
1986 public override bool IsThreaded 3498 public override bool IsThreaded
1987 { 3499 {
1988 // for now we won't be multithreaded 3500 // for now we won't be multithreaded
1989 get { return (false); } 3501 get { return false; }
1990 } 3502 }
1991 3503
1992 public float GetTerrainHeightAtXY(float x, float y) 3504 #region ODE Specific Terrain Fixes
3505 private float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
1993 { 3506 {
3507 float[] returnarr = new float[262144];
3508 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
1994 3509
1995 3510 // Filling out the array into its multi-dimensional components
1996 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; 3511 for (int y = 0; y < WorldExtents.Y; y++)
1997 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1998
1999
2000 IntPtr heightFieldGeom = IntPtr.Zero;
2001
2002 // get region map
2003 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
2004 return 0f;
2005
2006 if (heightFieldGeom == IntPtr.Zero)
2007 return 0f;
2008
2009 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
2010 return 0f;
2011
2012 // TerrainHeightField for ODE as offset 1m
2013 x += 1f - offsetX;
2014 y += 1f - offsetY;
2015
2016 // make position fit into array
2017 if (x < 0)
2018 x = 0;
2019 if (y < 0)
2020 y = 0;
2021
2022 // integer indexs
2023 int ix;
2024 int iy;
2025 // interpolators offset
2026 float dx;
2027 float dy;
2028
2029 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
2030
2031 if (OdeUbitLib)
2032 { 3512 {
2033 if (x < regsize - 1) 3513 for (int x = 0; x < WorldExtents.X; x++)
2034 {
2035 ix = (int)x;
2036 dx = x - (float)ix;
2037 }
2038 else // out world use external height
2039 {
2040 ix = regsize - 2;
2041 dx = 0;
2042 }
2043 if (y < regsize - 1)
2044 {
2045 iy = (int)y;
2046 dy = y - (float)iy;
2047 }
2048 else
2049 { 3514 {
2050 iy = regsize - 2; 3515 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
2051 dy = 0;
2052 } 3516 }
2053 } 3517 }
2054 3518
2055 else 3519 // Resize using Nearest Neighbour
3520
3521 // This particular way is quick but it only works on a multiple of the original
3522
3523 // The idea behind this method can be described with the following diagrams
3524 // second pass and third pass happen in the same loop really.. just separated
3525 // them to show what this does.
3526
3527 // First Pass
3528 // ResultArr:
3529 // 1,1,1,1,1,1
3530 // 1,1,1,1,1,1
3531 // 1,1,1,1,1,1
3532 // 1,1,1,1,1,1
3533 // 1,1,1,1,1,1
3534 // 1,1,1,1,1,1
3535
3536 // Second Pass
3537 // ResultArr2:
3538 // 1,,1,,1,,1,,1,,1,
3539 // ,,,,,,,,,,
3540 // 1,,1,,1,,1,,1,,1,
3541 // ,,,,,,,,,,
3542 // 1,,1,,1,,1,,1,,1,
3543 // ,,,,,,,,,,
3544 // 1,,1,,1,,1,,1,,1,
3545 // ,,,,,,,,,,
3546 // 1,,1,,1,,1,,1,,1,
3547 // ,,,,,,,,,,
3548 // 1,,1,,1,,1,,1,,1,
3549
3550 // Third pass fills in the blanks
3551 // ResultArr2:
3552 // 1,1,1,1,1,1,1,1,1,1,1,1
3553 // 1,1,1,1,1,1,1,1,1,1,1,1
3554 // 1,1,1,1,1,1,1,1,1,1,1,1
3555 // 1,1,1,1,1,1,1,1,1,1,1,1
3556 // 1,1,1,1,1,1,1,1,1,1,1,1
3557 // 1,1,1,1,1,1,1,1,1,1,1,1
3558 // 1,1,1,1,1,1,1,1,1,1,1,1
3559 // 1,1,1,1,1,1,1,1,1,1,1,1
3560 // 1,1,1,1,1,1,1,1,1,1,1,1
3561 // 1,1,1,1,1,1,1,1,1,1,1,1
3562 // 1,1,1,1,1,1,1,1,1,1,1,1
3563
3564 // X,Y = .
3565 // X+1,y = ^
3566 // X,Y+1 = *
3567 // X+1,Y+1 = #
3568
3569 // Filling in like this;
3570 // .*
3571 // ^#
3572 // 1st .
3573 // 2nd *
3574 // 3rd ^
3575 // 4th #
3576 // on single loop.
3577
3578 float[,] resultarr2 = new float[512, 512];
3579 for (int y = 0; y < WorldExtents.Y; y++)
2056 { 3580 {
2057 // we still have square fixed size regions 3581 for (int x = 0; x < WorldExtents.X; x++)
2058 // also flip x and y because of how map is done for ODE fliped axis
2059 // so ix,iy,dx and dy are inter exchanged
2060 if (x < regsize - 1)
2061 {
2062 iy = (int)x;
2063 dy = x - (float)iy;
2064 }
2065 else // out world use external height
2066 { 3582 {
2067 iy = regsize - 2; 3583 resultarr2[y * 2, x * 2] = resultarr[y, x];
2068 dy = 0; 3584
3585 if (y < WorldExtents.Y)
3586 {
3587 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3588 }
3589 if (x < WorldExtents.X)
3590 {
3591 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3592 }
3593 if (x < WorldExtents.X && y < WorldExtents.Y)
3594 {
3595 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3596 }
2069 } 3597 }
2070 if (y < regsize - 1) 3598 }
3599
3600 //Flatten out the array
3601 int i = 0;
3602 for (int y = 0; y < 512; y++)
3603 {
3604 for (int x = 0; x < 512; x++)
2071 { 3605 {
2072 ix = (int)y; 3606 if (resultarr2[y, x] <= 0)
2073 dx = y - (float)ix; 3607 returnarr[i] = 0.0000001f;
3608 else
3609 returnarr[i] = resultarr2[y, x];
3610
3611 i++;
2074 } 3612 }
2075 else 3613 }
3614
3615 return returnarr;
3616 }
3617
3618 private float[] ResizeTerrain512Interpolation(float[] heightMap)
3619 {
3620 float[] returnarr = new float[262144];
3621 float[,] resultarr = new float[512,512];
3622
3623 // Filling out the array into its multi-dimensional components
3624 for (int y = 0; y < 256; y++)
3625 {
3626 for (int x = 0; x < 256; x++)
2076 { 3627 {
2077 ix = regsize - 2; 3628 resultarr[y, x] = heightMap[y * 256 + x];
2078 dx = 0;
2079 } 3629 }
2080 } 3630 }
2081 3631
2082 float h0; 3632 // Resize using interpolation
2083 float h1; 3633
2084 float h2; 3634 // This particular way is quick but it only works on a multiple of the original
2085 3635
2086 iy *= regsize; 3636 // The idea behind this method can be described with the following diagrams
2087 iy += ix; // all indexes have iy + ix 3637 // second pass and third pass happen in the same loop really.. just separated
3638 // them to show what this does.
3639
3640 // First Pass
3641 // ResultArr:
3642 // 1,1,1,1,1,1
3643 // 1,1,1,1,1,1
3644 // 1,1,1,1,1,1
3645 // 1,1,1,1,1,1
3646 // 1,1,1,1,1,1
3647 // 1,1,1,1,1,1
3648
3649 // Second Pass
3650 // ResultArr2:
3651 // 1,,1,,1,,1,,1,,1,
3652 // ,,,,,,,,,,
3653 // 1,,1,,1,,1,,1,,1,
3654 // ,,,,,,,,,,
3655 // 1,,1,,1,,1,,1,,1,
3656 // ,,,,,,,,,,
3657 // 1,,1,,1,,1,,1,,1,
3658 // ,,,,,,,,,,
3659 // 1,,1,,1,,1,,1,,1,
3660 // ,,,,,,,,,,
3661 // 1,,1,,1,,1,,1,,1,
3662
3663 // Third pass fills in the blanks
3664 // ResultArr2:
3665 // 1,1,1,1,1,1,1,1,1,1,1,1
3666 // 1,1,1,1,1,1,1,1,1,1,1,1
3667 // 1,1,1,1,1,1,1,1,1,1,1,1
3668 // 1,1,1,1,1,1,1,1,1,1,1,1
3669 // 1,1,1,1,1,1,1,1,1,1,1,1
3670 // 1,1,1,1,1,1,1,1,1,1,1,1
3671 // 1,1,1,1,1,1,1,1,1,1,1,1
3672 // 1,1,1,1,1,1,1,1,1,1,1,1
3673 // 1,1,1,1,1,1,1,1,1,1,1,1
3674 // 1,1,1,1,1,1,1,1,1,1,1,1
3675 // 1,1,1,1,1,1,1,1,1,1,1,1
3676
3677 // X,Y = .
3678 // X+1,y = ^
3679 // X,Y+1 = *
3680 // X+1,Y+1 = #
3681
3682 // Filling in like this;
3683 // .*
3684 // ^#
3685 // 1st .
3686 // 2nd *
3687 // 3rd ^
3688 // 4th #
3689 // on single loop.
3690
3691 float[,] resultarr2 = new float[512,512];
3692 for (int y = 0; y < (int)Constants.RegionSize; y++)
3693 {
3694 for (int x = 0; x < (int)Constants.RegionSize; x++)
3695 {
3696 resultarr2[y*2, x*2] = resultarr[y, x];
2088 3697
2089 float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; 3698 if (y < (int)Constants.RegionSize)
2090 /* 3699 {
2091 if ((dx + dy) <= 1.0f) 3700 if (y + 1 < (int)Constants.RegionSize)
2092 { 3701 {
2093 h0 = ((float)heights[iy]); // 0,0 vertice 3702 if (x + 1 < (int)Constants.RegionSize)
2094 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 3703 {
2095 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 3704 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3705 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3706 }
3707 else
3708 {
3709 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3710 }
2096 } 3711 }
2097 else 3712 else
2098 { 3713 {
2099 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice 3714 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
2100 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2101 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2102 } 3715 }
2103 */ 3716 }
2104 h0 = ((float)heights[iy]); // 0,0 vertice 3717 if (x < (int)Constants.RegionSize)
2105 3718 {
2106 if ((dy > dx)) 3719 if (x + 1 < (int)Constants.RegionSize)
2107 { 3720 {
2108 iy += regsize; 3721 if (y + 1 < (int)Constants.RegionSize)
2109 h2 = (float)heights[iy]; // 0,1 vertice 3722 {
2110 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 3723 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
2111 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 3724 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3725 }
3726 else
3727 {
3728 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3729 }
3730 }
3731 else
3732 {
3733 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3734 }
3735 }
3736 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3737 {
3738 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3739 {
3740 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3741 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3742 }
3743 else
3744 {
3745 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3746 }
3747 }
3748 }
2112 } 3749 }
2113 else 3750 //Flatten out the array
3751 int i = 0;
3752 for (int y = 0; y < 512; y++)
2114 { 3753 {
2115 iy++; 3754 for (int x = 0; x < 512; x++)
2116 h2 = (float)heights[iy]; // vertice 1,0 3755 {
2117 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 3756 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
2118 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 3757 {
3758 m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0");
3759 resultarr2[y, x] = 0;
3760 }
3761 returnarr[i] = resultarr2[y, x];
3762 i++;
3763 }
2119 } 3764 }
2120 3765
2121 return h0 + h1 + h2; 3766 return returnarr;
2122 } 3767 }
2123 3768
3769 #endregion
2124 3770
2125 public override void SetTerrain(float[] heightMap) 3771 public override void SetTerrain(float[] heightMap)
2126 { 3772 {
@@ -2137,75 +3783,78 @@ namespace OpenSim.Region.Physics.OdePlugin
2137 } 3783 }
2138 } 3784 }
2139 3785
2140 public override void CombineTerrain(float[] heightMap, Vector3 pOffset) 3786 private void SetTerrain(float[] heightMap, Vector3 pOffset)
2141 { 3787 {
2142 SetTerrain(heightMap, pOffset); 3788 int startTime = Util.EnvironmentTickCount();
2143 } 3789 m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset);
2144 3790
2145 public void SetTerrain(float[] heightMap, Vector3 pOffset) 3791 // this._heightmap[i] = (double)heightMap[i];
2146 { 3792 // dbm (danx0r) -- creating a buffer zone of one extra sample all around
2147 if (OdeUbitLib) 3793 //_origheightmap = heightMap;
2148 UbitSetTerrain(heightMap, pOffset); 3794
2149 else 3795 float[] _heightmap;
2150 OriSetTerrain(heightMap, pOffset);
2151 }
2152 3796
2153 public void OriSetTerrain(float[] heightMap, Vector3 pOffset) 3797 // zero out a heightmap array float array (single dimension [flattened]))
2154 { 3798 //if ((int)Constants.RegionSize == 256)
2155 // assumes 1m size grid and constante size square regions 3799 // _heightmap = new float[514 * 514];
2156 // needs to know about sims around in future 3800 //else
2157 3801
2158 float[] _heightmap; 3802 _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
3803
3804 uint heightmapWidth = Constants.RegionSize + 1;
3805 uint heightmapHeight = Constants.RegionSize + 1;
2159 3806
2160 uint heightmapWidth = Constants.RegionSize + 2; 3807 uint heightmapWidthSamples;
2161 uint heightmapHeight = Constants.RegionSize + 2;
2162 3808
2163 uint heightmapWidthSamples = heightmapWidth + 1; 3809 uint heightmapHeightSamples;
2164 uint heightmapHeightSamples = heightmapHeight + 1;
2165 3810
2166 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; 3811 //if (((int)Constants.RegionSize) == 256)
3812 //{
3813 // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
3814 // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
3815 // heightmapWidth++;
3816 // heightmapHeight++;
3817 //}
3818 //else
3819 //{
3820
3821 heightmapWidthSamples = (uint)Constants.RegionSize + 1;
3822 heightmapHeightSamples = (uint)Constants.RegionSize + 1;
3823 //}
2167 3824
2168 const float scale = 1.0f; 3825 const float scale = 1.0f;
2169 const float offset = 0.0f; 3826 const float offset = 0.0f;
2170 const float thickness = 10f; 3827 const float thickness = 0.2f;
2171 const int wrap = 0; 3828 const int wrap = 0;
2172 3829
2173 uint regionsize = Constants.RegionSize; 3830 int regionsize = (int) Constants.RegionSize + 2;
2174 3831 //Double resolution
2175 float hfmin = float.MaxValue; 3832 //if (((int)Constants.RegionSize) == 256)
2176 float hfmax = float.MinValue; 3833 // heightMap = ResizeTerrain512Interpolation(heightMap);
2177 float val;
2178 uint xx;
2179 uint yy;
2180 3834
2181 uint maxXXYY = regionsize - 1;
2182 // flipping map adding one margin all around so things don't fall in edges
2183 3835
2184 uint xt = 0; 3836 // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
2185 xx = 0; 3837 // regionsize = 512;
2186 3838
2187 for (uint x = 0; x < heightmapWidthSamples; x++) 3839 float hfmin = 2000;
3840 float hfmax = -2000;
3841
3842 for (int x = 0; x < heightmapWidthSamples; x++)
2188 { 3843 {
2189 if (x > 1 && xx < maxXXYY) 3844 for (int y = 0; y < heightmapHeightSamples; y++)
2190 xx++;
2191 yy = 0;
2192 for (uint y = 0; y < heightmapHeightSamples; y++)
2193 { 3845 {
2194 if (y > 1 && y < maxXXYY) 3846 int xx = Util.Clip(x - 1, 0, regionsize - 1);
2195 yy += regionsize; 3847 int yy = Util.Clip(y - 1, 0, regionsize - 1);
2196 3848
2197 val = heightMap[yy + xx]; 3849
2198 if (val < 0.0f) 3850 float val= heightMap[yy * (int)Constants.RegionSize + xx];
2199 val = 0.0f; // no neg terrain as in chode 3851 _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
2200 _heightmap[xt + y] = val; 3852
2201 3853 hfmin = (val < hfmin) ? val : hfmin;
2202 if (hfmin > val) 3854 hfmax = (val > hfmax) ? val : hfmax;
2203 hfmin = val;
2204 if (hfmax < val)
2205 hfmax = val;
2206 } 3855 }
2207 xt += heightmapHeightSamples;
2208 } 3856 }
3857
2209 lock (OdeLock) 3858 lock (OdeLock)
2210 { 3859 {
2211 IntPtr GroundGeom = IntPtr.Zero; 3860 IntPtr GroundGeom = IntPtr.Zero;
@@ -2214,177 +3863,62 @@ namespace OpenSim.Region.Physics.OdePlugin
2214 RegionTerrain.Remove(pOffset); 3863 RegionTerrain.Remove(pOffset);
2215 if (GroundGeom != IntPtr.Zero) 3864 if (GroundGeom != IntPtr.Zero)
2216 { 3865 {
2217 actor_name_map.Remove(GroundGeom);
2218 d.GeomDestroy(GroundGeom);
2219
2220 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) 3866 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2221 { 3867 {
2222 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2223 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2224 TerrainHeightFieldHeights.Remove(GroundGeom); 3868 TerrainHeightFieldHeights.Remove(GroundGeom);
2225 } 3869 }
3870 d.SpaceRemove(space, GroundGeom);
3871 d.GeomDestroy(GroundGeom);
2226 } 3872 }
3873
2227 } 3874 }
2228 IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); 3875 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2229 3876 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1,
2230 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); 3877 (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale,
2231
2232 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2233 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2234 offset, thickness, wrap); 3878 offset, thickness, wrap);
2235
2236 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); 3879 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2237 3880 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
2238 GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
2239
2240 if (GroundGeom != IntPtr.Zero) 3881 if (GroundGeom != IntPtr.Zero)
2241 { 3882 {
2242 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); 3883 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2243 d.GeomSetCollideBits(GroundGeom, 0); 3884 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2244
2245 PhysicsActor pa = new NullPhysicsActor();
2246 pa.Name = "Terrain";
2247 pa.PhysicsActorType = (int)ActorTypes.Ground;
2248 actor_name_map[GroundGeom] = pa;
2249
2250// geom_name_map[GroundGeom] = "Terrain";
2251
2252 d.Matrix3 R = new d.Matrix3();
2253
2254 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2255 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2256 3885
2257
2258 q1 = q1 * q2;
2259
2260 Vector3 v3;
2261 float angle;
2262 q1.GetAxisAngle(out v3, out angle);
2263
2264 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2265 d.GeomSetRotation(GroundGeom, ref R);
2266 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2267 RegionTerrain.Add(pOffset, GroundGeom);
2268 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2269 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2270 } 3886 }
2271 } 3887 geom_name_map[GroundGeom] = "Terrain";
2272 }
2273
2274 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2275 {
2276 // assumes 1m size grid and constante size square regions
2277 // needs to know about sims around in future
2278
2279 float[] _heightmap;
2280
2281 uint heightmapWidth = Constants.RegionSize + 2;
2282 uint heightmapHeight = Constants.RegionSize + 2;
2283
2284 uint heightmapWidthSamples = heightmapWidth + 1;
2285 uint heightmapHeightSamples = heightmapHeight + 1;
2286
2287 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2288
2289
2290 uint regionsize = Constants.RegionSize;
2291
2292 float hfmin = float.MaxValue;
2293// float hfmax = float.MinValue;
2294 float val;
2295
2296
2297 uint maxXXYY = regionsize - 1;
2298 // adding one margin all around so things don't fall in edges
2299 3888
2300 uint xx; 3889 d.Matrix3 R = new d.Matrix3();
2301 uint yy = 0;
2302 uint yt = 0;
2303 3890
2304 for (uint y = 0; y < heightmapHeightSamples; y++) 3891 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2305 { 3892 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2306 if (y > 1 && y < maxXXYY) 3893 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
2307 yy += regionsize;
2308 xx = 0;
2309 for (uint x = 0; x < heightmapWidthSamples; x++)
2310 {
2311 if (x > 1 && x < maxXXYY)
2312 xx++;
2313 3894
2314 val = heightMap[yy + xx]; 3895 q1 = q1 * q2;
2315 if (val < 0.0f) 3896 //q1 = q1 * q3;
2316 val = 0.0f; // no neg terrain as in chode 3897 Vector3 v3;
2317 _heightmap[yt + x] = val; 3898 float angle;
3899 q1.GetAxisAngle(out v3, out angle);
2318 3900
2319 if (hfmin > val) 3901 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2320 hfmin = val; 3902 d.GeomSetRotation(GroundGeom, ref R);
2321// if (hfmax < val) 3903 d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0);
2322// hfmax = val; 3904 IntPtr testGround = IntPtr.Zero;
2323 } 3905 if (RegionTerrain.TryGetValue(pOffset, out testGround))
2324 yt += heightmapWidthSamples;
2325 }
2326 lock (OdeLock)
2327 {
2328 IntPtr GroundGeom = IntPtr.Zero;
2329 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2330 { 3906 {
2331 RegionTerrain.Remove(pOffset); 3907 RegionTerrain.Remove(pOffset);
2332 if (GroundGeom != IntPtr.Zero)
2333 {
2334 actor_name_map.Remove(GroundGeom);
2335 d.GeomDestroy(GroundGeom);
2336
2337 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2338 {
2339 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2340 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2341 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2342 TerrainHeightFieldHeights.Remove(GroundGeom);
2343 }
2344 }
2345 }
2346 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2347
2348 const int wrap = 0;
2349 float thickness = hfmin;
2350 if (thickness < 0)
2351 thickness = 1;
2352
2353 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2354
2355 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2356 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2357 thickness, wrap);
2358
2359// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2360 GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1);
2361 if (GroundGeom != IntPtr.Zero)
2362 {
2363 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2364 d.GeomSetCollideBits(GroundGeom, 0);
2365
2366
2367 PhysicsActor pa = new NullPhysicsActor();
2368 pa.Name = "Terrain";
2369 pa.PhysicsActorType = (int)ActorTypes.Ground;
2370 actor_name_map[GroundGeom] = pa;
2371
2372// geom_name_map[GroundGeom] = "Terrain";
2373
2374 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2375 RegionTerrain.Add(pOffset, GroundGeom);
2376 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2377 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2378 } 3908 }
3909 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3910 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
2379 } 3911 }
2380 }
2381 3912
3913 m_log.DebugFormat(
3914 "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime));
3915 }
2382 3916
2383 public override void DeleteTerrain() 3917 public override void DeleteTerrain()
2384 { 3918 {
2385 } 3919 }
2386 3920
2387 public float GetWaterLevel() 3921 internal float GetWaterLevel()
2388 { 3922 {
2389 return waterlevel; 3923 return waterlevel;
2390 } 3924 }
@@ -2393,252 +3927,169 @@ namespace OpenSim.Region.Physics.OdePlugin
2393 { 3927 {
2394 return true; 3928 return true;
2395 } 3929 }
2396/*
2397 public override void UnCombine(PhysicsScene pScene)
2398 {
2399 IntPtr localGround = IntPtr.Zero;
2400// float[] localHeightfield;
2401 bool proceed = false;
2402 List<IntPtr> geomDestroyList = new List<IntPtr>();
2403
2404 lock (OdeLock)
2405 {
2406 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2407 {
2408 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2409 {
2410 if (geom == localGround)
2411 {
2412// localHeightfield = TerrainHeightFieldHeights[geom];
2413 proceed = true;
2414 }
2415 else
2416 {
2417 geomDestroyList.Add(geom);
2418 }
2419 }
2420
2421 if (proceed)
2422 {
2423 m_worldOffset = Vector3.Zero;
2424 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2425 m_parentScene = null;
2426
2427 foreach (IntPtr g in geomDestroyList)
2428 {
2429 // removingHeightField needs to be done or the garbage collector will
2430 // collect the terrain data before we tell ODE to destroy it causing
2431 // memory corruption
2432 if (TerrainHeightFieldHeights.ContainsKey(g))
2433 {
2434// float[] removingHeightField = TerrainHeightFieldHeights[g];
2435 TerrainHeightFieldHeights.Remove(g);
2436
2437 if (RegionTerrain.ContainsKey(g))
2438 {
2439 RegionTerrain.Remove(g);
2440 }
2441 3930
2442 d.GeomDestroy(g); 3931// public override void UnCombine(PhysicsScene pScene)
2443 //removingHeightField = new float[0]; 3932// {
2444 } 3933// IntPtr localGround = IntPtr.Zero;
2445 } 3934//// float[] localHeightfield;
3935// bool proceed = false;
3936// List<IntPtr> geomDestroyList = new List<IntPtr>();
3937//
3938// lock (OdeLock)
3939// {
3940// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3941// {
3942// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3943// {
3944// if (geom == localGround)
3945// {
3946//// localHeightfield = TerrainHeightFieldHeights[geom];
3947// proceed = true;
3948// }
3949// else
3950// {
3951// geomDestroyList.Add(geom);
3952// }
3953// }
3954//
3955// if (proceed)
3956// {
3957// m_worldOffset = Vector3.Zero;
3958// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3959// m_parentScene = null;
3960//
3961// foreach (IntPtr g in geomDestroyList)
3962// {
3963// // removingHeightField needs to be done or the garbage collector will
3964// // collect the terrain data before we tell ODE to destroy it causing
3965// // memory corruption
3966// if (TerrainHeightFieldHeights.ContainsKey(g))
3967// {
3968//// float[] removingHeightField = TerrainHeightFieldHeights[g];
3969// TerrainHeightFieldHeights.Remove(g);
3970//
3971// if (RegionTerrain.ContainsKey(g))
3972// {
3973// RegionTerrain.Remove(g);
3974// }
3975//
3976// d.GeomDestroy(g);
3977// //removingHeightField = new float[0];
3978// }
3979// }
3980//
3981// }
3982// else
3983// {
3984// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3985// }
3986// }
3987// }
3988// }
2446 3989
2447 }
2448 else
2449 {
2450 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2451 }
2452 }
2453 }
2454 }
2455*/
2456 public override void SetWaterLevel(float baseheight) 3990 public override void SetWaterLevel(float baseheight)
2457 { 3991 {
2458 waterlevel = baseheight; 3992 waterlevel = baseheight;
2459// randomizeWater(waterlevel); 3993 randomizeWater(waterlevel);
2460 } 3994 }
2461/*
2462 public void randomizeWater(float baseheight)
2463 {
2464 const uint heightmapWidth = Constants.RegionSize + 2;
2465 const uint heightmapHeight = Constants.RegionSize + 2;
2466 const uint heightmapWidthSamples = heightmapWidth + 1;
2467 const uint heightmapHeightSamples = heightmapHeight + 1;
2468 3995
3996 private void randomizeWater(float baseheight)
3997 {
3998 const uint heightmapWidth = m_regionWidth + 2;
3999 const uint heightmapHeight = m_regionHeight + 2;
4000 const uint heightmapWidthSamples = m_regionWidth + 2;
4001 const uint heightmapHeightSamples = m_regionHeight + 2;
2469 const float scale = 1.0f; 4002 const float scale = 1.0f;
2470 const float offset = 0.0f; 4003 const float offset = 0.0f;
4004 const float thickness = 2.9f;
2471 const int wrap = 0; 4005 const int wrap = 0;
2472 4006
2473 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; 4007 for (int i = 0; i < (258 * 258); i++)
2474
2475 float maxheigh = float.MinValue;
2476 float minheigh = float.MaxValue;
2477 float val;
2478 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
2479 { 4008 {
2480 4009 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
2481 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); 4010 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
2482 _watermap[i] = val;
2483 if (maxheigh < val)
2484 maxheigh = val;
2485 if (minheigh > val)
2486 minheigh = val;
2487 } 4011 }
2488 4012
2489 float thickness = minheigh;
2490
2491 lock (OdeLock) 4013 lock (OdeLock)
2492 { 4014 {
2493 if (WaterGeom != IntPtr.Zero) 4015 if (WaterGeom != IntPtr.Zero)
2494 { 4016 {
2495 actor_name_map.Remove(WaterGeom); 4017 d.SpaceRemove(space, WaterGeom);
2496 d.GeomDestroy(WaterGeom);
2497 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2498 WaterGeom = IntPtr.Zero;
2499 WaterHeightmapData = IntPtr.Zero;
2500 if(WaterMapHandler.IsAllocated)
2501 WaterMapHandler.Free();
2502 } 4018 }
2503 4019 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2504 WaterHeightmapData = d.GeomHeightfieldDataCreate(); 4020 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
2505
2506 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2507
2508 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
2509 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, 4021 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2510 offset, thickness, wrap); 4022 offset, thickness, wrap);
2511 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); 4023 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
2512 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); 4024 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
2513 if (WaterGeom != IntPtr.Zero) 4025 if (WaterGeom != IntPtr.Zero)
2514 { 4026 {
2515 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); 4027 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
2516 d.GeomSetCollideBits(WaterGeom, 0); 4028 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
2517 4029 }
2518
2519 PhysicsActor pa = new NullPhysicsActor();
2520 pa.Name = "Water";
2521 pa.PhysicsActorType = (int)ActorTypes.Water;
2522 4030
2523 actor_name_map[WaterGeom] = pa; 4031 geom_name_map[WaterGeom] = "Water";
2524// geom_name_map[WaterGeom] = "Water";
2525 4032
2526 d.Matrix3 R = new d.Matrix3(); 4033 d.Matrix3 R = new d.Matrix3();
2527 4034
2528 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); 4035 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2529 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); 4036 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
4037 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
2530 4038
2531 q1 = q1 * q2; 4039 q1 = q1 * q2;
2532 Vector3 v3; 4040 //q1 = q1 * q3;
2533 float angle; 4041 Vector3 v3;
2534 q1.GetAxisAngle(out v3, out angle); 4042 float angle;
4043 q1.GetAxisAngle(out v3, out angle);
2535 4044
2536 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); 4045 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2537 d.GeomSetRotation(WaterGeom, ref R); 4046 d.GeomSetRotation(WaterGeom, ref R);
2538 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); 4047 d.GeomSetPosition(WaterGeom, 128, 128, 0);
2539 }
2540 } 4048 }
2541 } 4049 }
2542*/ 4050
2543 public override void Dispose() 4051 public override void Dispose()
2544 { 4052 {
2545 if (m_meshWorker != null) 4053 _worldInitialized = false;
2546 m_meshWorker.Stop(); 4054
4055 m_rayCastManager.Dispose();
4056 m_rayCastManager = null;
2547 4057
2548 lock (OdeLock) 4058 lock (OdeLock)
2549 { 4059 {
2550 m_rayCastManager.Dispose();
2551 m_rayCastManager = null;
2552
2553 lock (_prims) 4060 lock (_prims)
2554 { 4061 {
2555 ChangesQueue.Clear();
2556 foreach (OdePrim prm in _prims) 4062 foreach (OdePrim prm in _prims)
2557 { 4063 {
2558 prm.DoAChange(changes.Remove, null); 4064 RemovePrim(prm);
2559 _collisionEventPrim.Remove(prm);
2560 } 4065 }
2561 _prims.Clear();
2562 }
2563
2564 OdeCharacter[] chtorem;
2565 lock (_characters)
2566 {
2567 chtorem = new OdeCharacter[_characters.Count];
2568 _characters.CopyTo(chtorem);
2569 } 4066 }
2570 4067
2571 ChangesQueue.Clear(); 4068 //foreach (OdeCharacter act in _characters)
2572 foreach (OdeCharacter ch in chtorem) 4069 //{
2573 ch.DoAChange(changes.Remove, null); 4070 //RemoveAvatar(act);
2574 4071 //}
2575
2576 foreach (IntPtr GroundGeom in RegionTerrain.Values)
2577 {
2578 if (GroundGeom != IntPtr.Zero)
2579 d.GeomDestroy(GroundGeom);
2580 }
2581
2582
2583 RegionTerrain.Clear();
2584
2585 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2586 {
2587 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2588 {
2589 if (gch.IsAllocated)
2590 gch.Free();
2591 }
2592 }
2593
2594 TerrainHeightFieldHeightsHandlers.Clear();
2595 TerrainHeightFieldHeights.Clear();
2596/*
2597 if (WaterGeom != IntPtr.Zero)
2598 {
2599 d.GeomDestroy(WaterGeom);
2600 WaterGeom = IntPtr.Zero;
2601 if (WaterHeightmapData != IntPtr.Zero)
2602 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2603 WaterHeightmapData = IntPtr.Zero;
2604
2605 if (WaterMapHandler.IsAllocated)
2606 WaterMapHandler.Free();
2607 }
2608*/
2609 if (ContactgeomsArray != IntPtr.Zero)
2610 Marshal.FreeHGlobal(ContactgeomsArray);
2611 if (GlobalContactsArray != IntPtr.Zero)
2612 Marshal.FreeHGlobal(GlobalContactsArray);
2613
2614
2615 d.WorldDestroy(world); 4072 d.WorldDestroy(world);
2616 world = IntPtr.Zero;
2617 //d.CloseODE(); 4073 //d.CloseODE();
2618 } 4074 }
4075
2619 } 4076 }
2620 4077
2621 public override Dictionary<uint, float> GetTopColliders() 4078 public override Dictionary<uint, float> GetTopColliders()
2622 { 4079 {
2623 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>(); 4080 Dictionary<uint, float> topColliders;
2624 int cnt = 0; 4081
2625 lock (_prims) 4082 lock (_prims)
2626 { 4083 {
2627 foreach (OdePrim prm in _prims) 4084 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
2628 { 4085 orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25);
2629 if (prm.CollisionScore > 0) 4086 topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore);
2630 { 4087
2631 returncolliders.Add(prm.LocalID, prm.CollisionScore); 4088 foreach (OdePrim p in _prims)
2632 cnt++; 4089 p.CollisionScore = 0;
2633 prm.CollisionScore = 0f;
2634 if (cnt > 25)
2635 {
2636 break;
2637 }
2638 }
2639 }
2640 } 4090 }
2641 return returncolliders; 4091
4092 return topColliders;
2642 } 4093 }
2643 4094
2644 public override bool SupportsRayCast() 4095 public override bool SupportsRayCast()
@@ -2662,7 +4113,6 @@ namespace OpenSim.Region.Physics.OdePlugin
2662 } 4113 }
2663 } 4114 }
2664 4115
2665 // don't like this
2666 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) 4116 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2667 { 4117 {
2668 ContactResult[] ourResults = null; 4118 ContactResult[] ourResults = null;
@@ -2679,107 +4129,182 @@ namespace OpenSim.Region.Physics.OdePlugin
2679 waitTime++; 4129 waitTime++;
2680 } 4130 }
2681 if (ourResults == null) 4131 if (ourResults == null)
2682 return new List<ContactResult>(); 4132 return new List<ContactResult> ();
2683 return new List<ContactResult>(ourResults); 4133 return new List<ContactResult>(ourResults);
2684 } 4134 }
2685 4135
2686 public override bool SuportsRaycastWorldFiltered() 4136#if USE_DRAWSTUFF
4137 // Keyboard callback
4138 public void command(int cmd)
2687 { 4139 {
2688 return true; 4140 IntPtr geom;
4141 d.Mass mass;
4142 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
4143
4144
4145
4146 Char ch = Char.ToLower((Char)cmd);
4147 switch ((Char)ch)
4148 {
4149 case 'w':
4150 try
4151 {
4152 Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
4153
4154 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
4155 ds.SetViewpoint(ref xyz, ref hpr);
4156 }
4157 catch (ArgumentException)
4158 { hpr.X = 0; }
4159 break;
4160
4161 case 'a':
4162 hpr.X++;
4163 ds.SetViewpoint(ref xyz, ref hpr);
4164 break;
4165
4166 case 's':
4167 try
4168 {
4169 Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
4170
4171 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
4172 ds.SetViewpoint(ref xyz, ref hpr);
4173 }
4174 catch (ArgumentException)
4175 { hpr.X = 0; }
4176 break;
4177 case 'd':
4178 hpr.X--;
4179 ds.SetViewpoint(ref xyz, ref hpr);
4180 break;
4181 case 'r':
4182 xyz.Z++;
4183 ds.SetViewpoint(ref xyz, ref hpr);
4184 break;
4185 case 'f':
4186 xyz.Z--;
4187 ds.SetViewpoint(ref xyz, ref hpr);
4188 break;
4189 case 'e':
4190 xyz.Y++;
4191 ds.SetViewpoint(ref xyz, ref hpr);
4192 break;
4193 case 'q':
4194 xyz.Y--;
4195 ds.SetViewpoint(ref xyz, ref hpr);
4196 break;
4197 }
2689 } 4198 }
2690 4199
2691 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) 4200 public void step(int pause)
2692 { 4201 {
2693 object SyncObject = new object(); 4202
2694 List<ContactResult> ourresults = new List<ContactResult>(); 4203 ds.SetColor(1.0f, 1.0f, 0.0f);
2695 4204 ds.SetTexture(ds.Texture.Wood);
2696 RayCallback retMethod = delegate(List<ContactResult> results) 4205 lock (_prims)
2697 { 4206 {
2698 lock (SyncObject) 4207 foreach (OdePrim prm in _prims)
2699 { 4208 {
2700 ourresults = results; 4209 //IntPtr body = d.GeomGetBody(prm.prim_geom);
2701 Monitor.PulseAll(SyncObject); 4210 if (prm.prim_geom != IntPtr.Zero)
4211 {
4212 d.Vector3 pos;
4213 d.GeomCopyPosition(prm.prim_geom, out pos);
4214 //d.BodyCopyPosition(body, out pos);
4215
4216 d.Matrix3 R;
4217 d.GeomCopyRotation(prm.prim_geom, out R);
4218 //d.BodyCopyRotation(body, out R);
4219
4220
4221 d.Vector3 sides = new d.Vector3();
4222 sides.X = prm.Size.X;
4223 sides.Y = prm.Size.Y;
4224 sides.Z = prm.Size.Z;
4225
4226 ds.DrawBox(ref pos, ref R, ref sides);
4227 }
2702 } 4228 }
2703 }; 4229 }
4230 ds.SetColor(1.0f, 0.0f, 0.0f);
2704 4231
2705 lock (SyncObject) 4232 foreach (OdeCharacter chr in _characters)
2706 { 4233 {
2707 m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); 4234 if (chr.Shell != IntPtr.Zero)
2708 if (!Monitor.Wait(SyncObject, 500)) 4235 {
2709 return null; 4236 IntPtr body = d.GeomGetBody(chr.Shell);
2710 else 4237
2711 return ourresults; 4238 d.Vector3 pos;
4239 d.GeomCopyPosition(chr.Shell, out pos);
4240 //d.BodyCopyPosition(body, out pos);
4241
4242 d.Matrix3 R;
4243 d.GeomCopyRotation(chr.Shell, out R);
4244 //d.BodyCopyRotation(body, out R);
4245
4246 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
4247 d.Vector3 sides = new d.Vector3();
4248 sides.X = 0.5f;
4249 sides.Y = 0.5f;
4250 sides.Z = 0.5f;
4251
4252 ds.DrawBox(ref pos, ref R, ref sides);
4253 }
2712 } 4254 }
2713 } 4255 }
2714 4256
2715 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) 4257 public void start(int unused)
2716 { 4258 {
2717 if (retMethod != null && actor !=null) 4259 ds.SetViewpoint(ref xyz, ref hpr);
2718 {
2719 IntPtr geom;
2720 if (actor is OdePrim)
2721 geom = ((OdePrim)actor).prim_geom;
2722 else if (actor is OdeCharacter)
2723 geom = ((OdePrim)actor).prim_geom;
2724 else
2725 return;
2726 if (geom == IntPtr.Zero)
2727 return;
2728 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2729 }
2730 } 4260 }
4261#endif
2731 4262
2732 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) 4263 public override Dictionary<string, float> GetStats()
2733 { 4264 {
2734 if (retMethod != null && actor != null) 4265 if (!CollectStats)
4266 return null;
4267
4268 Dictionary<string, float> returnStats;
4269
4270 lock (OdeLock)
2735 { 4271 {
2736 IntPtr geom; 4272 returnStats = new Dictionary<string, float>(m_stats);
2737 if (actor is OdePrim) 4273
2738 geom = ((OdePrim)actor).prim_geom; 4274 // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
2739 else if (actor is OdeCharacter) 4275 // 3 from the SimStatsReporter.
2740 geom = ((OdePrim)actor).prim_geom; 4276 returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
2741 else 4277 returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
2742 return; 4278 returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
2743 if (geom == IntPtr.Zero)
2744 return;
2745 4279
2746 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); 4280 InitializeExtraStats();
2747 } 4281 }
4282
4283 returnStats[ODEOtherCollisionFrameMsStatName]
4284 = returnStats[ODEOtherCollisionFrameMsStatName]
4285 - returnStats[ODENativeSpaceCollisionFrameMsStatName]
4286 - returnStats[ODENativeGeomCollisionFrameMsStatName];
4287
4288 return returnStats;
2748 } 4289 }
2749 4290
2750 // don't like this 4291 private void InitializeExtraStats()
2751 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2752 { 4292 {
2753 if (actor != null) 4293 m_stats[ODETotalFrameMsStatName] = 0;
2754 { 4294 m_stats[ODEAvatarTaintMsStatName] = 0;
2755 IntPtr geom; 4295 m_stats[ODEPrimTaintMsStatName] = 0;
2756 if (actor is OdePrim) 4296 m_stats[ODEAvatarForcesFrameMsStatName] = 0;
2757 geom = ((OdePrim)actor).prim_geom; 4297 m_stats[ODEPrimForcesFrameMsStatName] = 0;
2758 else if (actor is OdeCharacter) 4298 m_stats[ODERaycastingFrameMsStatName] = 0;
2759 geom = ((OdePrim)actor).prim_geom; 4299 m_stats[ODENativeStepFrameMsStatName] = 0;
2760 else 4300 m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
2761 return new List<ContactResult>(); 4301 m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
2762 if (geom == IntPtr.Zero) 4302 m_stats[ODEOtherCollisionFrameMsStatName] = 0;
2763 return new List<ContactResult>(); 4303 m_stats[ODECollisionNotificationFrameMsStatName] = 0;
2764 4304 m_stats[ODEAvatarContactsStatsName] = 0;
2765 ContactResult[] ourResults = null; 4305 m_stats[ODEPrimContactsStatName] = 0;
2766 RayCallback retMethod = delegate(List<ContactResult> results) 4306 m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
2767 { 4307 m_stats[ODEPrimUpdateFrameMsStatName] = 0;
2768 ourResults = new ContactResult[results.Count];
2769 results.CopyTo(ourResults, 0);
2770 };
2771 int waitTime = 0;
2772 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2773 while (ourResults == null && waitTime < 1000)
2774 {
2775 Thread.Sleep(1);
2776 waitTime++;
2777 }
2778 if (ourResults == null)
2779 return new List<ContactResult>();
2780 return new List<ContactResult>(ourResults);
2781 }
2782 return new List<ContactResult>();
2783 } 4308 }
2784 } 4309 }
2785} 4310} \ No newline at end of file
generated by cgit v1.1 at 2024-10-24 11:30:16 +0000