aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs1467
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs4091
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs384
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs48
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs3887
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs353
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs122
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs98
9 files changed, 10508 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d65929a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System.Reflection;
29using System.Runtime.InteropServices;
30
31// Information about this assembly is defined by the following
32// attributes.
33//
34// change them to the information which is associated with the assembly
35// you compile.
36
37[assembly : AssemblyTitle("OdePlugin")]
38[assembly : AssemblyDescription("")]
39[assembly : AssemblyConfiguration("")]
40[assembly : AssemblyCompany("http://opensimulator.org")]
41[assembly : AssemblyProduct("OdePlugin")]
42[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
43[assembly : AssemblyTrademark("")]
44[assembly : AssemblyCulture("")]
45
46// This sets the default COM visibility of types in the assembly to invisible.
47// If you need to expose a type to COM, use [ComVisible(true)] on that type.
48
49[assembly : ComVisible(false)]
50
51// The assembly version has following format :
52//
53// Major.Minor.Build.Revision
54//
55// You can specify all values by your own or you can build default build and revision
56// numbers with the '*' character (the default):
57
58[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..ec717d7
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1467 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using OpenMetaverse;
32using Ode.NET;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using log4net;
36
37namespace OpenSim.Region.Physics.OdePlugin
38{
39 /// <summary>
40 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
41 /// </summary>
42
43 public enum dParam : int
44 {
45 LowStop = 0,
46 HiStop = 1,
47 Vel = 2,
48 FMax = 3,
49 FudgeFactor = 4,
50 Bounce = 5,
51 CFM = 6,
52 StopERP = 7,
53 StopCFM = 8,
54 LoStop2 = 256,
55 HiStop2 = 257,
56 Vel2 = 258,
57 FMax2 = 259,
58 StopERP2 = 7 + 256,
59 StopCFM2 = 8 + 256,
60 LoStop3 = 512,
61 HiStop3 = 513,
62 Vel3 = 514,
63 FMax3 = 515,
64 StopERP3 = 7 + 512,
65 StopCFM3 = 8 + 512
66 }
67 public class OdeCharacter : PhysicsActor
68 {
69 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
70
71 private Vector3 _position;
72 private d.Vector3 _zeroPosition;
73 // private d.Matrix3 m_StandUpRotation;
74 private bool _zeroFlag = false;
75 private bool m_lastUpdateSent = false;
76 private Vector3 _velocity;
77 private Vector3 _target_velocity;
78 private Vector3 _acceleration;
79 private Vector3 m_rotationalVelocity;
80 private float m_mass = 80f;
81 public float m_density = 60f;
82 private bool m_pidControllerActive = true;
83 public float PID_D = 800.0f;
84 public float PID_P = 900.0f;
85 //private static float POSTURE_SERVO = 10000.0f;
86 public float CAPSULE_RADIUS = 0.37f;
87 public float CAPSULE_LENGTH = 2.140599f;
88 public float m_tensor = 3800000f;
89 public float heightFudgeFactor = 0.52f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool jumping = false; // add for jumping
94 private bool m_iscolliding = false;
95 private bool m_iscollidingGround = false;
96 private bool m_wascolliding = false;
97 private bool m_wascollidingGround = false;
98 private bool m_iscollidingObj = false;
99 private bool m_alwaysRun = false;
100 private bool m_hackSentFall = false;
101 private bool m_hackSentFly = false;
102 private int m_requestedUpdateFrequency = 0;
103 private Vector3 m_taintPosition = Vector3.Zero;
104 public uint m_localID = 0;
105 public bool m_returnCollisions = false;
106 // taints and their non-tainted counterparts
107 public bool m_isPhysical = false; // the current physical status
108 public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
109 public float MinimumGroundFlightOffset = 3f;
110
111 private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
112 private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
113
114
115 private float m_buoyancy = 0f;
116
117 // private CollisionLocker ode;
118
119 private string m_name = String.Empty;
120
121 private bool[] m_colliderarr = new bool[11];
122 private bool[] m_colliderGroundarr = new bool[11];
123
124 // Default we're a Character
125 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
126
127 // Default, Collide with Other Geometries, spaces, bodies and characters.
128 private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
129 | CollisionCategories.Space
130 | CollisionCategories.Body
131 | CollisionCategories.Character
132 | CollisionCategories.Land);
133 public IntPtr Body = IntPtr.Zero;
134 private OdeScene _parent_scene;
135 public IntPtr Shell = IntPtr.Zero;
136 public IntPtr Amotor = IntPtr.Zero;
137 public d.Mass ShellMass;
138 public bool collidelock = false;
139
140 public int m_eventsubscription = 0;
141 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
142
143 private Vector3 m_taintMomentum = Vector3.Zero;
144 private bool m_haveTaintMomentum = false;
145
146
147 // unique UUID of this character object
148 public UUID m_uuid;
149 public bool bad = false;
150 private Object m_syncRoot = new Object();
151
152 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
153 {
154 m_uuid = UUID.Random();
155
156 if (pos.IsFinite())
157 {
158 if (pos.Z > 9999999f)
159 {
160 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
161 }
162 if (pos.Z < -90000f)
163 {
164 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
165 }
166 _position = pos;
167 m_taintPosition.X = pos.X;
168 m_taintPosition.Y = pos.Y;
169 m_taintPosition.Z = pos.Z;
170 }
171 else
172 {
173 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
174 m_taintPosition.X = _position.X;
175 m_taintPosition.Y = _position.Y;
176 m_taintPosition.Z = _position.Z;
177 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
178 }
179
180 _parent_scene = parent_scene;
181
182 PID_D = pid_d;
183 PID_P = pid_p;
184 CAPSULE_RADIUS = capsule_radius;
185 m_tensor = tensor;
186 m_density = density;
187 heightFudgeFactor = height_fudge_factor;
188 walkDivisor = walk_divisor;
189 runDivisor = rundivisor;
190
191 // m_StandUpRotation =
192 // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
193 // 0.5f);
194
195 for (int i = 0; i < 11; i++)
196 {
197 m_colliderarr[i] = false;
198 }
199 CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
200 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
201 m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
202
203 m_isPhysical = false; // current status: no ODE information exists
204 m_tainted_isPhysical = true; // new tainted status: need to create ODE information
205
206 _parent_scene.AddPhysicsActorTaint(this);
207
208 m_name = avName;
209 }
210
211 public override int PhysicsActorType
212 {
213 get { return (int) ActorTypes.Agent; }
214 set { return; }
215 }
216
217 /// <summary>
218 /// If this is set, the avatar will move faster
219 /// </summary>
220 public override bool SetAlwaysRun
221 {
222 get { return m_alwaysRun; }
223 set { m_alwaysRun = value; }
224 }
225
226 public override uint LocalID
227 {
228 set { m_localID = value; }
229 }
230
231 public override bool Grabbed
232 {
233 set { return; }
234 }
235
236 public override bool Selected
237 {
238// set { return; }
239 set { jumping = value; } // add for jumping flag
240 }
241
242 public override float Buoyancy
243 {
244 get { return m_buoyancy; }
245 set { m_buoyancy = value; }
246 }
247
248 public override bool FloatOnWater
249 {
250 set { return; }
251 }
252
253 public override bool IsPhysical
254 {
255 get { return false; }
256 set { return; }
257 }
258
259 public override bool ThrottleUpdates
260 {
261 get { return false; }
262 set { return; }
263 }
264
265 public override bool Flying
266 {
267 get { return flying; }
268 set { flying = value; }
269 }
270
271 /// <summary>
272 /// Returns if the avatar is colliding in general.
273 /// This includes the ground and objects and avatar.
274 /// </summary>
275 public override bool IsColliding
276 {
277//#@ get { return m_iscolliding; }
278 get { //##
279//Console.WriteLine(">>>>>>>>>>>> IC get = {0}", m_iscolliding); //##
280 return m_iscolliding; } //##
281 set
282 {
283 int i;
284 int truecount = 0;
285 int falsecount = 0;
286
287 if (m_colliderarr.Length >= 10)
288 {
289 for (i = 0; i < 10; i++)
290 {
291 m_colliderarr[i] = m_colliderarr[i + 1];
292 }
293 }
294 m_colliderarr[10] = value;
295
296 for (i = 0; i < 11; i++)
297 {
298 if (m_colliderarr[i])
299 {
300 truecount++;
301 }
302 else
303 {
304 falsecount++;
305 }
306 }
307
308 // Equal truecounts and false counts means we're colliding with something.
309
310 if (falsecount > 1.2*truecount)
311 {
312 m_iscolliding = false;
313 }
314 else
315 {
316 m_iscolliding = true;
317 }
318// ## Console.WriteLine("IC SET = {0} t{1} f{2} i {3}", value, truecount, falsecount, m_iscolliding);
319 if (m_wascolliding != m_iscolliding)
320 {
321 //base.SendCollisionUpdate(new CollisionEventUpdate());
322 }
323 m_wascolliding = m_iscolliding;
324 }
325 }
326
327 /// <summary>
328 /// Returns if an avatar is colliding with the ground
329 /// </summary>
330 public override bool CollidingGround
331 {
332 get { return m_iscollidingGround; }
333 set
334 {
335 // Collisions against the ground are not really reliable
336 // So, to get a consistant value we have to average the current result over time
337 // Currently we use 1 second = 10 calls to this.
338 int i;
339 int truecount = 0;
340 int falsecount = 0;
341
342 if (m_colliderGroundarr.Length >= 10)
343 {
344 for (i = 0; i < 10; i++)
345 {
346 m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
347 }
348 }
349 m_colliderGroundarr[10] = value;
350
351 for (i = 0; i < 11; i++)
352 {
353 if (m_colliderGroundarr[i])
354 {
355 truecount++;
356 }
357 else
358 {
359 falsecount++;
360 }
361 }
362
363 // Equal truecounts and false counts means we're colliding with something.
364
365 if (falsecount > 1.2*truecount)
366 {
367 m_iscollidingGround = false;
368 }
369 else
370 {
371 m_iscollidingGround = true;
372 }
373 if (m_wascollidingGround != m_iscollidingGround)
374 {
375 //base.SendCollisionUpdate(new CollisionEventUpdate());
376 }
377 m_wascollidingGround = m_iscollidingGround;
378 }
379 }
380
381 /// <summary>
382 /// Returns if the avatar is colliding with an object
383 /// </summary>
384 public override bool CollidingObj
385 {
386 get { return m_iscollidingObj; }
387 set
388 {
389 m_iscollidingObj = value;
390 if (value)
391 m_pidControllerActive = false;
392 else
393 m_pidControllerActive = true;
394 }
395 }
396
397 /// <summary>
398 /// turn the PID controller on or off.
399 /// The PID Controller will turn on all by itself in many situations
400 /// </summary>
401 /// <param name="status"></param>
402 public void SetPidStatus(bool status)
403 {
404 m_pidControllerActive = status;
405 }
406
407 public override bool Stopped
408 {
409 get { return _zeroFlag; }
410 }
411
412 /// <summary>
413 /// This 'puts' an avatar somewhere in the physics space.
414 /// Not really a good choice unless you 'know' it's a good
415 /// spot otherwise you're likely to orbit the avatar.
416 /// </summary>
417 public override Vector3 Position
418 {
419 get { return _position; }
420 set
421 {
422 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
423 {
424 if (value.IsFinite())
425 {
426 if (value.Z > 9999999f)
427 {
428 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
429 }
430 if (value.Z < -90000f)
431 {
432 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
433 }
434
435 _position.X = value.X;
436 _position.Y = value.Y;
437 _position.Z = value.Z;
438
439 m_taintPosition.X = value.X;
440 m_taintPosition.Y = value.Y;
441 m_taintPosition.Z = value.Z;
442 _parent_scene.AddPhysicsActorTaint(this);
443 }
444 else
445 {
446 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
447 }
448 }
449 }
450 }
451
452 public override Vector3 RotationalVelocity
453 {
454 get { return m_rotationalVelocity; }
455 set { m_rotationalVelocity = value; }
456 }
457
458 /// <summary>
459 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
460 /// and use it to offset landings properly
461 /// </summary>
462 public override Vector3 Size
463 {
464 get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
465 set
466 {
467 if (value.IsFinite())
468 {
469 m_pidControllerActive = true;
470
471 Vector3 SetSize = value;
472 m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
473 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
474
475 Velocity = Vector3.Zero;
476 m_taintPosition = _position; // update the stale taint position
477 _parent_scene.AddPhysicsActorTaint(this);
478 }
479 else
480 {
481 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
482 }
483 }
484 }
485
486 private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
487 {
488 movementVector.Z = 0f;
489 float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
490 if (magnitude < 0.1f) return;
491
492 // normalize the velocity vector
493 float invMagnitude = 1.0f / magnitude;
494 movementVector.X *= invMagnitude;
495 movementVector.Y *= invMagnitude;
496
497 // if we change the capsule heading too often, the capsule can fall down
498 // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
499 // meaning only 4 possible capsule tilt orientations
500 if (movementVector.X > 0)
501 {
502 // east
503 if (movementVector.Y > 0)
504 {
505 // northeast
506 movementVector.X = (float)Math.Sqrt(2.0);
507 movementVector.Y = (float)Math.Sqrt(2.0);
508 }
509 else
510 {
511 // southeast
512 movementVector.X = (float)Math.Sqrt(2.0);
513 movementVector.Y = -(float)Math.Sqrt(2.0);
514 }
515 }
516 else
517 {
518 // west
519 if (movementVector.Y > 0)
520 {
521 // northwest
522 movementVector.X = -(float)Math.Sqrt(2.0);
523 movementVector.Y = (float)Math.Sqrt(2.0);
524 }
525 else
526 {
527 // southwest
528 movementVector.X = -(float)Math.Sqrt(2.0);
529 movementVector.Y = -(float)Math.Sqrt(2.0);
530 }
531 }
532
533
534 // movementVector.Z is zero
535
536 // calculate tilt components based on desired amount of tilt and current (snapped) heading.
537 // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
538 float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
539 float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
540
541 //m_log.Debug("[PHYSICS] changing avatar tilt");
542 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
543 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
544 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
545 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
546 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
547 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
548 }
549
550 /// <summary>
551 /// This creates the Avatar's physical Surrogate at the position supplied
552 /// </summary>
553 /// <param name="npositionX"></param>
554 /// <param name="npositionY"></param>
555 /// <param name="npositionZ"></param>
556
557 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
558 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
559 // place that is safe to call this routine AvatarGeomAndBodyCreation.
560 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
561 {
562 //CAPSULE_LENGTH = -5;
563 //CAPSULE_RADIUS = -5;
564 int dAMotorEuler = 1;
565 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
566 if (CAPSULE_LENGTH <= 0)
567 {
568 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
569 CAPSULE_LENGTH = 0.01f;
570
571 }
572
573 if (CAPSULE_RADIUS <= 0)
574 {
575 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
576 CAPSULE_RADIUS = 0.01f;
577
578 }
579
580 if(Shell != IntPtr.Zero)
581 {
582 try
583 {
584 d.GeomDestroy(Shell);
585 }
586 catch (System.AccessViolationException)
587 {
588 m_log.Error("[PHYSICS]: PrimGeom dead");
589 }
590 // Remove any old entries
591//string tShell;
592//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
593//Console.WriteLine("**** Remove {0}", tShell);
594 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
595 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
596 }
597
598 Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
599 _parent_scene.geom_name_map[Shell] = m_name;
600 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
601//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS);
602
603 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
604 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
605
606 d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
607 Body = d.BodyCreate(_parent_scene.world);
608 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
609
610 _position.X = npositionX;
611 _position.Y = npositionY;
612 _position.Z = npositionZ;
613
614
615 m_taintPosition.X = npositionX;
616 m_taintPosition.Y = npositionY;
617 m_taintPosition.Z = npositionZ;
618
619 d.BodySetMass(Body, ref ShellMass);
620 d.Matrix3 m_caprot;
621 // 90 Stand up on the cap of the capped cyllinder
622 if (_parent_scene.IsAvCapsuleTilted)
623 {
624 d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
625 }
626 else
627 {
628 d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
629 }
630
631
632 d.GeomSetRotation(Shell, ref m_caprot);
633 d.BodySetRotation(Body, ref m_caprot);
634
635 d.GeomSetBody(Shell, Body);
636
637
638 // The purpose of the AMotor here is to keep the avatar's physical
639 // surrogate from rotating while moving
640 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
641 d.JointAttach(Amotor, Body, IntPtr.Zero);
642 d.JointSetAMotorMode(Amotor, dAMotorEuler);
643 d.JointSetAMotorNumAxes(Amotor, 3);
644 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
645 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
646 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
647 d.JointSetAMotorAngle(Amotor, 0, 0);
648 d.JointSetAMotorAngle(Amotor, 1, 0);
649 d.JointSetAMotorAngle(Amotor, 2, 0);
650
651 // These lowstops and high stops are effectively (no wiggle room)
652 if (_parent_scene.IsAvCapsuleTilted)
653 {
654 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
655 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
656 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
657 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
658 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
659 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
660 }
661 else
662 {
663 #region Documentation of capsule motor LowStop and HighStop parameters
664 // Intentionally introduce some tilt into the capsule by setting
665 // the motor stops to small epsilon values. This small tilt prevents
666 // the capsule from falling into the terrain; a straight-up capsule
667 // (with -0..0 motor stops) falls into the terrain for reasons yet
668 // to be comprehended in their entirety.
669 #endregion
670 AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
671 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
672 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
673 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
674 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
675 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
676 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
677 }
678
679 // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the
680 // capped cyllinder will fall over
681 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
682 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
683
684 //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
685 //d.QfromR(
686 //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
687 //
688 //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
689 //standupStraight();
690 }
691
692 //
693 /// <summary>
694 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
695 /// This may be used in calculations in the scene/scenepresence
696 /// </summary>
697 public override float Mass
698 {
699 get
700 {
701 float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
702 return m_density*AVvolume;
703 }
704 }
705 public override void link(PhysicsActor obj)
706 {
707
708 }
709
710 public override void delink()
711 {
712
713 }
714
715 public override void LockAngularMotion(Vector3 axis)
716 {
717
718 }
719
720// This code is very useful. Written by DanX0r. We're just not using it right now.
721// Commented out to prevent a warning.
722//
723// private void standupStraight()
724// {
725// // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
726// // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
727// // change appearance and when you enter the simulator
728// // After this routine is done, the amotor stabilizes much quicker
729// d.Vector3 feet;
730// d.Vector3 head;
731// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
732// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
733// float posture = head.Z - feet.Z;
734
735// // restoring force proportional to lack of posture:
736// float servo = (2.5f - posture) * POSTURE_SERVO;
737// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
738// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
739// //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
740// //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
741// }
742
743 public override Vector3 Force
744 {
745 get { return _target_velocity; }
746 set { return; }
747 }
748
749 public override int VehicleType
750 {
751 get { return 0; }
752 set { return; }
753 }
754
755 public override void VehicleFloatParam(int param, float value)
756 {
757
758 }
759
760 public override void VehicleVectorParam(int param, Vector3 value)
761 {
762
763 }
764
765 public override void VehicleRotationParam(int param, Quaternion rotation)
766 {
767
768 }
769
770 public override void VehicleFlags(int flags, bool remove)
771 {
772 }
773
774 public override void SetVolumeDetect(int param)
775 {
776
777 }
778
779 public override Vector3 CenterOfMass
780 {
781 get { return Vector3.Zero; }
782 }
783
784 public override Vector3 GeometricCenter
785 {
786 get { return Vector3.Zero; }
787 }
788
789 public override PrimitiveBaseShape Shape
790 {
791 set { return; }
792 }
793
794 public override Vector3 Velocity
795 {
796 get {
797 // There's a problem with Vector3.Zero! Don't Use it Here!
798 if (_zeroFlag)
799 return Vector3.Zero;
800 m_lastUpdateSent = false;
801 return _velocity;
802 }
803 set
804 {
805 if (value.IsFinite())
806 {
807 _target_velocity = value;
808 m_pidControllerActive = true;
809 }
810 else
811 {
812 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
813 }
814 }
815 }
816
817 public override Vector3 Torque
818 {
819 get { return Vector3.Zero; }
820 set { return; }
821 }
822
823 public override float CollisionScore
824 {
825 get { return 0f; }
826 set { }
827 }
828
829 public override bool Kinematic
830 {
831 get { return false; }
832 set { }
833 }
834
835 public override Quaternion Orientation
836 {
837 get { return Quaternion.Identity; }
838 set {
839 //Matrix3 or = Orientation.ToRotationMatrix();
840 //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
841 //d.BodySetRotation(Body, ref ord);
842 }
843 }
844
845 public override Vector3 Acceleration
846 {
847 get { return _acceleration; }
848 set { _acceleration = value; }
849 }
850
851 public void SetAcceleration(Vector3 accel)
852 {
853 m_pidControllerActive = true;
854 _acceleration = accel;
855 }
856
857 /// <summary>
858 /// Adds the force supplied to the Target Velocity
859 /// The PID controller takes this target velocity and tries to make it a reality
860 /// </summary>
861 /// <param name="force"></param>
862 public override void AddForce(Vector3 force, bool pushforce)
863 {
864 if (force.IsFinite())
865 {
866 if (pushforce)
867 {
868 m_pidControllerActive = false;
869 force *= 100f;
870//Console.WriteLine("DF 1"); // ##
871 if (!force.ApproxEquals(Vector3.Zero, 0.01f))
872 doForce(force);
873 // If uncommented, things get pushed off world
874 //
875 // m_log.Debug("Push!");
876 // _target_velocity.X += force.X;
877 // _target_velocity.Y += force.Y;
878 // _target_velocity.Z += force.Z;
879 }
880 else
881 {
882 m_pidControllerActive = true;
883 _target_velocity.X += force.X;
884 _target_velocity.Y += force.Y;
885 _target_velocity.Z += force.Z;
886 }
887 }
888 else
889 {
890 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
891 }
892 //m_lastUpdateSent = false;
893 }
894
895 public override void AddAngularForce(Vector3 force, bool pushforce)
896 {
897
898 }
899
900 /// <summary>
901 /// After all of the forces add up with 'add force' we apply them with doForce
902 /// </summary>
903 /// <param name="force"></param>
904 public void doForce(Vector3 force)
905 {
906 if (!collidelock)
907 {
908 d.BodyAddForce(Body, force.X, force.Y, force.Z);
909 //d.BodySetRotation(Body, ref m_StandUpRotation);
910 //standupStraight();
911 d.Vector3 vel = d.BodyGetLinearVel(Body); //##
912//Console.WriteLine("AvVel <{0},{1},{2}>", vel.X, vel.Y, vel.Z); //##
913 }
914 }
915
916 public override void SetMomentum(Vector3 momentum)
917 {
918 if (momentum.IsFinite())
919 {
920 m_taintMomentum = momentum;
921 m_haveTaintMomentum = true;
922 _parent_scene.AddPhysicsActorTaint(this);
923 }
924 else
925 m_log.Warn("[PHYSICS] !isFinite momentum");
926 }
927
928
929 /// <summary>
930 /// Called from Simulate
931 /// This is the avatar's movement control + PID Controller
932 /// </summary>
933 /// <param name="timeStep"></param>
934 public void Move(float timeStep, List<OdeCharacter> defects)
935 {
936 // no lock; for now it's only called from within Simulate()
937
938 // If the PID Controller isn't active then we set our force
939 // calculating base velocity to the current position
940
941 if (Body == IntPtr.Zero)
942 return;
943
944 if (m_pidControllerActive == false)
945 {
946 _zeroPosition = d.BodyGetPosition(Body);
947 }
948 //PidStatus = true;
949
950 d.Vector3 localpos = d.BodyGetPosition(Body);
951 Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
952
953 if (!localPos.IsFinite())
954 {
955
956 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
957 defects.Add(this);
958 // _parent_scene.RemoveCharacter(this);
959
960 // destroy avatar capsule and related ODE data
961 if (Amotor != IntPtr.Zero)
962 {
963 // Kill the Amotor
964 d.JointDestroy(Amotor);
965 Amotor = IntPtr.Zero;
966 }
967
968 //kill the Geometry
969 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
970
971 if (Body != IntPtr.Zero)
972 {
973 //kill the body
974 d.BodyDestroy(Body);
975
976 Body = IntPtr.Zero;
977 }
978
979 if(Shell != IntPtr.Zero)
980 {
981 try
982 {
983 d.GeomDestroy(Shell);
984 }
985 catch (System.AccessViolationException)
986 {
987 m_log.Error("[PHYSICS]: PrimGeom dead");
988 }
989 // Remove any old entries
990//string tShell;
991//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
992//Console.WriteLine("**** Remove {0}", tShell);
993
994 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
995 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
996 Shell = IntPtr.Zero;
997 }
998
999 return;
1000 }
1001
1002 Vector3 vec = Vector3.Zero;
1003 d.Vector3 vel = d.BodyGetLinearVel(Body);
1004
1005 float movementdivisor = 1f;
1006
1007 if (!m_alwaysRun)
1008 {
1009 movementdivisor = walkDivisor;
1010 }
1011 else
1012 {
1013 movementdivisor = runDivisor;
1014 }
1015
1016 // if velocity is zero, use position control; otherwise, velocity control
1017 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
1018 {
1019 // keep track of where we stopped. No more slippin' & slidin'
1020 if (!_zeroFlag)
1021 {
1022 _zeroFlag = true;
1023 _zeroPosition = d.BodyGetPosition(Body);
1024 }
1025 if (m_pidControllerActive)
1026 {
1027 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1028 // react to the physics scene by moving it's position.
1029 // Avatar to Avatar collisions
1030 // Prim to avatar collisions
1031
1032 d.Vector3 pos = d.BodyGetPosition(Body);
1033 float errX = _zeroPosition.X - pos.X;
1034 float errY = _zeroPosition.Y - pos.Y;
1035 if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) )
1036 {
1037 vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2);
1038 vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2);
1039 }
1040 else
1041 { // close, jump to lateral destination
1042 d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z);
1043 }
1044// if (flying)
1045 if (flying || jumping) // add for jumping
1046 {
1047 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1048 }
1049 }
1050 //PidStatus = true;
1051 }
1052 else
1053 {
1054 m_pidControllerActive = true;
1055 _zeroFlag = false;
1056 if (m_iscolliding && !flying)
1057 {
1058 // We're standing on something
1059 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
1060 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
1061 }
1062 else if (m_iscolliding && flying)
1063 {
1064 // We're flying and colliding with something
1065 vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
1066 vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
1067 }
1068 else if (!m_iscolliding && flying)
1069 {
1070 // we're in mid air suspended
1071 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
1072 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
1073 }
1074
1075 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
1076 {
1077 // We're colliding with something and we're not flying but we're moving
1078 // This means we're walking or running.
1079 d.Vector3 pos = d.BodyGetPosition(Body);
1080 vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
1081 if (_target_velocity.X > 0)
1082 {
1083 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1084 }
1085 if (_target_velocity.Y > 0)
1086 {
1087 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1088 }
1089 }
1090 else if (!m_iscolliding && !flying)
1091 {
1092 // we're not colliding and we're not flying so that means we're falling!
1093 // m_iscolliding includes collisions with the ground.
1094
1095 // d.Vector3 pos = d.BodyGetPosition(Body);
1096 if (Math.Abs(_target_velocity.X) > 0)
1097 {
1098 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1099 }
1100 if (Math.Abs(_target_velocity.Y) > 0)
1101 {
1102 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1103 }
1104 }
1105
1106 if (flying)
1107 {
1108 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
1109 }
1110 }
1111 if (flying)
1112 {
1113 vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
1114
1115 //Added for auto fly height. Kitto Flora
1116 //d.Vector3 pos = d.BodyGetPosition(Body);
1117 float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
1118
1119 if (_position.Z < target_altitude)
1120 {
1121 vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
1122 }
1123 // end add Kitto Flora
1124 }
1125
1126 if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50ms apply breaks
1127 {
1128 float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall
1129 vec.X -= breakfactor * vel.X;
1130 vec.Y -= breakfactor * vel.Y;
1131 vec.Z -= breakfactor * vel.Z;
1132 }
1133
1134 if (vec.IsFinite())
1135 {
1136 if (vec.LengthSquared() > 0.0004f) // 0.01 allows 0.002 !!
1137 {
1138//Console.WriteLine("DF 2"); // ##
1139
1140 doForce(vec);
1141 if (!_zeroFlag)
1142 {
1143// AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
1144 }
1145 }
1146 }
1147 else
1148 {
1149 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1150 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1151 defects.Add(this);
1152 // _parent_scene.RemoveCharacter(this);
1153 // destroy avatar capsule and related ODE data
1154 if (Amotor != IntPtr.Zero)
1155 {
1156 // Kill the Amotor
1157 d.JointDestroy(Amotor);
1158 Amotor = IntPtr.Zero;
1159 }
1160 //kill the Geometry
1161 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1162
1163 if (Body != IntPtr.Zero)
1164 {
1165 //kill the body
1166 d.BodyDestroy(Body);
1167
1168 Body = IntPtr.Zero;
1169 }
1170
1171 if(Shell != IntPtr.Zero)
1172 {
1173 try
1174 {
1175 d.GeomDestroy(Shell);
1176 }
1177 catch (System.AccessViolationException)
1178 {
1179 m_log.Error("[PHYSICS]: PrimGeom dead");
1180 }
1181 // Remove any old entries
1182//string tShell;
1183//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1184//Console.WriteLine("**** Remove {0}", tShell);
1185
1186 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1187 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1188 Shell = IntPtr.Zero;
1189 }
1190 }
1191 }
1192
1193 /// <summary>
1194 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1195 /// </summary>
1196 public void UpdatePositionAndVelocity()
1197 {
1198 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1199 d.Vector3 vec;
1200 try
1201 {
1202 vec = d.BodyGetPosition(Body);
1203 }
1204 catch (NullReferenceException)
1205 {
1206 bad = true;
1207 _parent_scene.BadCharacter(this);
1208 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1209 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1210 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1211 }
1212
1213
1214 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
1215 if (vec.X < 0.0f) vec.X = 0.0f;
1216 if (vec.Y < 0.0f) vec.Y = 0.0f;
1217 if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
1218 if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
1219
1220 _position.X = vec.X;
1221 _position.Y = vec.Y;
1222 _position.Z = vec.Z;
1223
1224 // Did we move last? = zeroflag
1225 // This helps keep us from sliding all over
1226
1227 if (_zeroFlag)
1228 {
1229 _velocity.X = 0.0f;
1230 _velocity.Y = 0.0f;
1231 _velocity.Z = 0.0f;
1232
1233 // Did we send out the 'stopped' message?
1234 if (!m_lastUpdateSent)
1235 {
1236 m_lastUpdateSent = true;
1237 //base.RequestPhysicsterseUpdate();
1238
1239 }
1240 }
1241 else
1242 {
1243 m_lastUpdateSent = false;
1244 try
1245 {
1246 vec = d.BodyGetLinearVel(Body);
1247 }
1248 catch (NullReferenceException)
1249 {
1250 vec.X = _velocity.X;
1251 vec.Y = _velocity.Y;
1252 vec.Z = _velocity.Z;
1253 }
1254 _velocity.X = (vec.X);
1255 _velocity.Y = (vec.Y);
1256
1257 _velocity.Z = (vec.Z);
1258
1259 if (_velocity.Z < -6 && !m_hackSentFall)
1260 {
1261 m_hackSentFall = true;
1262 m_pidControllerActive = false;
1263 }
1264 else if (flying && !m_hackSentFly)
1265 {
1266 //m_hackSentFly = true;
1267 //base.SendCollisionUpdate(new CollisionEventUpdate());
1268 }
1269 else
1270 {
1271 m_hackSentFly = false;
1272 m_hackSentFall = false;
1273 }
1274 }
1275 }
1276
1277 /// <summary>
1278 /// Cleanup the things we use in the scene.
1279 /// </summary>
1280 public void Destroy()
1281 {
1282 m_tainted_isPhysical = false;
1283 _parent_scene.AddPhysicsActorTaint(this);
1284 }
1285
1286 public override void CrossingFailure()
1287 {
1288 }
1289
1290 public override Vector3 PIDTarget { set { return; } }
1291 public override bool PIDActive { set { return; } }
1292 public override float PIDTau { set { return; } }
1293
1294 public override float PIDHoverHeight { set { return; } }
1295 public override bool PIDHoverActive { set { return; } }
1296 public override PIDHoverType PIDHoverType { set { return; } }
1297 public override float PIDHoverTau { set { return; } }
1298
1299 public override Quaternion APIDTarget{ set { return; } }
1300
1301 public override bool APIDActive{ set { return; } }
1302
1303 public override float APIDStrength{ set { return; } }
1304
1305 public override float APIDDamping{ set { return; } }
1306
1307
1308 public override void SubscribeEvents(int ms)
1309 {
1310 m_requestedUpdateFrequency = ms;
1311 m_eventsubscription = ms;
1312 _parent_scene.addCollisionEventReporting(this);
1313 }
1314 public override void UnSubscribeEvents()
1315 {
1316 _parent_scene.remCollisionEventReporting(this);
1317 m_requestedUpdateFrequency = 0;
1318 m_eventsubscription = 0;
1319 }
1320 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1321 {
1322 if (m_eventsubscription > 0)
1323 {
1324 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1325 }
1326 }
1327
1328 public void SendCollisions()
1329 {
1330 if (m_eventsubscription > m_requestedUpdateFrequency)
1331 {
1332 if (CollisionEventsThisFrame != null)
1333 {
1334 base.SendCollisionUpdate(CollisionEventsThisFrame);
1335 }
1336 CollisionEventsThisFrame = new CollisionEventUpdate();
1337 m_eventsubscription = 0;
1338 }
1339 }
1340 public override bool SubscribedEvents()
1341 {
1342 if (m_eventsubscription > 0)
1343 return true;
1344 return false;
1345 }
1346
1347 public void ProcessTaints(float timestep)
1348 {
1349 lock (m_syncRoot)
1350 {
1351 if (m_tainted_isPhysical != m_isPhysical)
1352 {
1353 if (m_tainted_isPhysical)
1354 {
1355 // Create avatar capsule and related ODE data
1356 if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
1357 {
1358 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1359 + (Shell!=IntPtr.Zero ? "Shell ":"")
1360 + (Body!=IntPtr.Zero ? "Body ":"")
1361 + (Amotor!=IntPtr.Zero ? "Amotor ":""));
1362 }
1363 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
1364 _parent_scene.AddCharacter(this);
1365 }
1366 else
1367 {
1368 _parent_scene.RemoveCharacter(this);
1369 // destroy avatar capsule and related ODE data
1370 if (Amotor != IntPtr.Zero)
1371 {
1372 // Kill the Amotor
1373 d.JointDestroy(Amotor);
1374 Amotor = IntPtr.Zero;
1375 }
1376 //kill the Geometry
1377 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1378
1379 if (Body != IntPtr.Zero)
1380 {
1381 //kill the body
1382 d.BodyDestroy(Body);
1383 Body = IntPtr.Zero;
1384 }
1385
1386 if(Shell != IntPtr.Zero)
1387 {
1388 try
1389 {
1390 d.GeomDestroy(Shell);
1391 }
1392 catch (Exception e)
1393 {
1394 m_log.ErrorFormat("[PHYSICS]: Failed to destroy character shell {0}",e.Message);
1395 }
1396 // Remove any old entries
1397 //string tShell;
1398 //_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1399 //Console.WriteLine("**** Remove {0}", tShell);
1400
1401 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1402 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1403 Shell = IntPtr.Zero;
1404 }
1405 }
1406
1407 m_isPhysical = m_tainted_isPhysical;
1408 }
1409
1410 if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
1411 {
1412 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1413 {
1414
1415 m_pidControllerActive = true;
1416 // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
1417 d.JointDestroy(Amotor);
1418 float prevCapsule = CAPSULE_LENGTH;
1419 CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
1420 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
1421 d.BodyDestroy(Body);
1422 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1423 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
1424 Velocity = Vector3.Zero;
1425 }
1426 else
1427 {
1428 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1429 + (Shell==IntPtr.Zero ? "Shell ":"")
1430 + (Body==IntPtr.Zero ? "Body ":"")
1431 + (Amotor==IntPtr.Zero ? "Amotor ":""));
1432 }
1433 }
1434
1435 if (!m_taintPosition.ApproxEquals(_position, 0.05f))
1436 {
1437 if (Body != IntPtr.Zero)
1438 {
1439 d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
1440
1441 }
1442 _position.X = m_taintPosition.X;
1443 _position.Y = m_taintPosition.Y;
1444 _position.Z = m_taintPosition.Z;
1445 }
1446
1447 if (m_haveTaintMomentum)
1448 {
1449 m_haveTaintMomentum = false;
1450 _velocity = m_taintMomentum;
1451 _target_velocity = m_taintMomentum;
1452 m_pidControllerActive = true;
1453 if (Body != IntPtr.Zero)
1454 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1455 }
1456 }
1457 }
1458
1459 internal void AddCollisionFrameTime(int p)
1460 {
1461 // protect it from overflow crashing
1462 if (m_eventsubscription + p >= int.MaxValue)
1463 m_eventsubscription = 0;
1464 m_eventsubscription += p;
1465 }
1466 }
1467}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..77ea2af
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,4091 @@
1/* Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders.
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright
6 * notice, this list of conditions and the following disclaimer.
7 * * Redistributions in binary form must reproduce the above copyright
8 * notice, this list of conditions and the following disclaimer in the
9 * documentation and/or other materials provided with the distribution.
10 * * Neither the name of the OpenSimulator Project nor the
11 * names of its contributors may be used to endorse or promote products
12 * derived from this software without specific prior written permission.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
15 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
16 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
18 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
19 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
21 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 *
25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * Ubit 2012
27 * rolled into ODEPrim.cs
28 */
29
30using System;
31using System.IO;
32using System.Collections.Generic;
33using System.Reflection;
34using System.Runtime.InteropServices;
35using System.Threading;
36using log4net;
37using OpenMetaverse;
38using Ode.NET;
39using OpenSim.Framework;
40using OpenSim.Region.Physics.Manager;
41
42namespace OpenSim.Region.Physics.OdePlugin
43{
44 /// <summary>
45 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
46 /// </summary>
47
48 public class OdePrim : PhysicsActor
49 {
50 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
51
52 public class SerialControl
53 {
54 public object alock = new object();
55 public byte[] data = new byte[0];
56 }
57 private Vector3 _position;
58 private Vector3 _velocity;
59 private Vector3 _torque;
60 private Vector3 m_lastVelocity;
61 private Vector3 m_lastposition;
62 private Quaternion m_lastorientation = new Quaternion();
63 private Vector3 m_rotationalVelocity;
64 private Vector3 _size;
65 private Vector3 _acceleration;
66 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
67 private Quaternion _orientation;
68 private Vector3 m_taintposition;
69 private Vector3 m_taintsize;
70 private Vector3 m_taintVelocity;
71 private Vector3 m_taintTorque;
72 private Quaternion m_taintrot;
73 private Vector3 m_rotateEnable = Vector3.One; // Current setting
74 private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL
75 private bool m_rotateEnableUpdate = false;
76 private Vector3 m_lockX;
77 private Vector3 m_lockY;
78 private Vector3 m_lockZ;
79 private IntPtr Amotor = IntPtr.Zero;
80 private IntPtr AmotorX = IntPtr.Zero;
81 private IntPtr AmotorY = IntPtr.Zero;
82 private IntPtr AmotorZ = IntPtr.Zero;
83
84 private Vector3 m_PIDTarget;
85 private float m_PIDTau;
86 private float PID_D = 35f;
87 private float PID_G = 25f;
88 private bool m_usePID = false;
89
90 private Quaternion m_APIDTarget = new Quaternion();
91 private float m_APIDStrength = 0.5f;
92 private float m_APIDDamping = 0.5f;
93 private bool m_useAPID = false;
94 private float m_APIDdamper = 1.0f;
95
96 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
97 // do not confuse with VEHICLE HOVER
98
99 private float m_PIDHoverHeight;
100 private float m_PIDHoverTau;
101 private bool m_useHoverPID;
102 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
103 private float m_targetHoverHeight;
104 private float m_groundHeight;
105 private float m_waterHeight;
106 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
107
108 // private float m_tensor = 5f;
109 private int body_autodisable_frames = 20;
110
111
112 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
113 | CollisionCategories.Space
114 | CollisionCategories.Body
115 | CollisionCategories.Character
116 );
117 private bool m_taintshape;
118 private bool m_taintPhysics;
119 private bool m_collidesLand = true;
120 private bool m_collidesWater;
121 // public bool m_returnCollisions;
122
123 // Default we're a Geometry
124 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
125
126 // Default, Collide with Other Geometries, spaces and Bodies
127 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
128
129 public bool m_taintremove;
130 public bool m_taintdisable;
131 public bool m_disabled;
132 public bool m_taintadd;
133 public bool m_taintselected;
134 public bool m_taintphantom;
135 public bool m_taintCollidesWater;
136
137 public uint m_localID;
138
139 //public GCHandle gc;
140 private CollisionLocker ode;
141
142 private bool m_meshfailed = false;
143 private bool m_taintforce = false;
144 private bool m_taintaddangularforce = false;
145 private Vector3 m_force;
146 private List<Vector3> m_forcelist = new List<Vector3>();
147 private List<Vector3> m_angularforcelist = new List<Vector3>();
148
149 private IMesh _mesh;
150 private PrimitiveBaseShape _pbs;
151 private OdeScene _parent_scene;
152 public IntPtr m_targetSpace = IntPtr.Zero;
153 public IntPtr prim_geom;
154 // public IntPtr prev_geom;
155 public IntPtr _triMeshData;
156
157 private IntPtr _linkJointGroup = IntPtr.Zero;
158 private PhysicsActor _parent;
159 private PhysicsActor m_taintparent;
160
161 private List<OdePrim> childrenPrim = new List<OdePrim>();
162
163 private bool iscolliding;
164 private bool m_isphysical;
165 private bool m_isphantom;
166 private bool m_isSelected;
167
168 private bool m_NoColide; // for now only for internal use for bad meshs
169
170 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
171
172 private bool m_throttleUpdates;
173 private int throttleCounter;
174 public int m_interpenetrationcount;
175 public float m_collisionscore;
176 // public int m_roundsUnderMotionThreshold;
177 // private int m_crossingfailures;
178
179 public bool m_outofBounds;
180 private float m_density = 10.000006836f; // Aluminum g/cm3;
181
182 private float m_primMass = 10.000006836f; // Aluminum g/cm3;
183
184 private byte m_shapetype;
185 private byte m_taintshapetype;
186
187 public bool _zeroFlag; // if body has been stopped
188 private bool m_lastUpdateSent;
189
190 public IntPtr Body = IntPtr.Zero;
191 public String m_primName;
192 private Vector3 _target_velocity;
193 public d.Mass pMass;
194
195 public int m_eventsubscription;
196 private CollisionEventUpdate CollisionEventsThisFrame;
197
198 private IntPtr m_linkJoint = IntPtr.Zero;
199
200 public volatile bool childPrim;
201
202 internal int m_material = (int)Material.Wood;
203
204 private IntPtr m_body = IntPtr.Zero;
205
206 // Vehicle properties ============================================================================================
207 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
208 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
209 private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings:
210 // HOVER_TERRAIN_ONLY
211 // HOVER_GLOBAL_HEIGHT
212 // NO_DEFLECTION_UP
213 // HOVER_WATER_ONLY
214 // HOVER_UP_ONLY
215 // LIMIT_MOTOR_UP
216 // LIMIT_ROLL_ONLY
217
218 // Linear properties
219 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
220 //requested by LSL
221 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
222 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
223 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
224
225 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
226 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
227 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
228
229 //Angular properties
230 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
231
232 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
233 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
234 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
235
236 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
237 // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
238 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
239
240 //Deflection properties
241 // private float m_angularDeflectionEfficiency = 0;
242 // private float m_angularDeflectionTimescale = 0;
243 // private float m_linearDeflectionEfficiency = 0;
244 // private float m_linearDeflectionTimescale = 0;
245
246 //Banking properties
247 // private float m_bankingEfficiency = 0;
248 // private float m_bankingMix = 0;
249 // private float m_bankingTimescale = 0;
250
251 //Hover and Buoyancy properties
252 private float m_VhoverHeight = 0f;
253 // private float m_VhoverEfficiency = 0f;
254 private float m_VhoverTimescale = 0f;
255 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
256 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
257 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
258 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
259 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
260
261 //Attractor properties
262 private float m_verticalAttractionEfficiency = 1.0f; // damped
263 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
264
265// SerialControl m_taintserial = null;
266 object m_taintvehicledata = null;
267
268 public void DoSetVehicle()
269 {
270 VehicleData vd = (VehicleData)m_taintvehicledata;
271
272 m_type = vd.m_type;
273 m_flags = vd.m_flags;
274
275 // Linear properties
276 m_linearMotorDirection = vd.m_linearMotorDirection;
277 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
278 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
279 m_linearMotorTimescale = vd.m_linearMotorTimescale;
280// m_linearMotorOffset = vd.m_linearMotorOffset;
281
282 //Angular properties
283 m_angularMotorDirection = vd.m_angularMotorDirection;
284 m_angularMotorTimescale = vd.m_angularMotorTimescale;
285 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
286 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
287
288 //Deflection properties
289// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
290// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
291// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
292// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
293
294 //Banking properties
295// m_bankingEfficiency = vd.m_bankingEfficiency;
296// m_bankingMix = vd.m_bankingMix;
297// m_bankingTimescale = vd.m_bankingTimescale;
298
299 //Hover and Buoyancy properties
300 m_VhoverHeight = vd.m_VhoverHeight;
301// m_VhoverEfficiency = vd.m_VhoverEfficiency;
302 m_VhoverTimescale = vd.m_VhoverTimescale;
303 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
304
305 //Attractor properties
306 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
307 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
308
309 // Axis
310// m_referenceFrame = vd.m_referenceFrame;
311
312
313 m_taintvehicledata = null;
314 }
315
316 public override void SetVehicle(object vdata)
317 {
318 m_taintvehicledata = vdata;
319 _parent_scene.AddPhysicsActorTaint(this);
320 }
321
322 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
323 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical,
324 bool pisPhantom,byte shapetype, CollisionLocker dode, uint localid)
325 {
326 m_localID = localid;
327 ode = dode;
328 if (!pos.IsFinite())
329 {
330 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
331 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
332 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
333 }
334
335 _position = pos;
336 m_taintposition = pos;
337 PID_D = parent_scene.bodyPIDD;
338 PID_G = parent_scene.bodyPIDG;
339 m_density = parent_scene.geomDefaultDensity;
340 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
341 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
342
343 prim_geom = IntPtr.Zero;
344 // prev_geom = IntPtr.Zero;
345
346 if (!pos.IsFinite())
347 {
348 size = new Vector3(0.5f, 0.5f, 0.5f);
349 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
350 }
351
352 if (size.X <= 0) size.X = 0.01f;
353 if (size.Y <= 0) size.Y = 0.01f;
354 if (size.Z <= 0) size.Z = 0.01f;
355
356 _size = size;
357 m_taintsize = _size;
358
359 if (!QuaternionIsFinite(rotation))
360 {
361 rotation = Quaternion.Identity;
362 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
363 }
364
365 _orientation = rotation;
366 m_taintrot = _orientation;
367 _mesh = mesh;
368 _pbs = pbs;
369 m_shapetype = shapetype;
370 m_taintshapetype = shapetype;
371
372 _parent_scene = parent_scene;
373 m_targetSpace = (IntPtr)0;
374
375 // if (pos.Z < 0)
376 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
377 m_isphysical = false;
378 else
379 {
380 m_isphysical = pisPhysical;
381 // If we're physical, we need to be in the master space for now.
382 // linksets *should* be in a space together.. but are not currently
383 if (m_isphysical)
384 m_targetSpace = _parent_scene.space;
385 }
386
387 m_isphantom = pisPhantom;
388 m_taintphantom = pisPhantom;
389
390 _triMeshData = IntPtr.Zero;
391 m_NoColide = false;
392
393// m_taintserial = null;
394 m_primName = primName;
395 m_taintadd = true;
396 _parent_scene.AddPhysicsActorTaint(this);
397 // don't do .add() here; old geoms get recycled with the same hash
398 }
399
400 public override int PhysicsActorType
401 {
402 get { return (int)ActorTypes.Prim; }
403 set { return; }
404 }
405
406 public override bool SetAlwaysRun
407 {
408 get { return false; }
409 set { return; }
410 }
411
412 public override uint LocalID
413 {
414 set
415 {
416 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
417 m_localID = value;
418 }
419 }
420
421 public override bool Grabbed
422 {
423 set { return; }
424 }
425
426 public override bool Selected
427 {
428 set
429 {
430 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
431 // This only makes the object not collidable if the object
432 // is physical or the object is modified somehow *IN THE FUTURE*
433 // without this, if an avatar selects prim, they can walk right
434 // through it while it's selected
435 m_collisionscore = 0;
436 if ((m_isphysical && !_zeroFlag) || !value)
437 {
438 m_taintselected = value;
439 _parent_scene.AddPhysicsActorTaint(this);
440 }
441 else
442 {
443 m_taintselected = value;
444 m_isSelected = value;
445 }
446 if (m_isSelected) disableBodySoft();
447 }
448 }
449
450 public override bool IsPhysical
451 {
452 get { return m_isphysical; }
453 set
454 {
455 m_isphysical = value;
456 if (!m_isphysical)
457 { // Zero the remembered last velocity
458 m_lastVelocity = Vector3.Zero;
459 if (m_type != Vehicle.TYPE_NONE) Halt();
460 }
461 }
462 }
463
464 public override bool Phantom
465 {
466 get { return m_isphantom; }
467 set
468 {
469 m_isphantom = value;
470 }
471 }
472
473 public void setPrimForRemoval()
474 {
475 m_taintremove = true;
476 }
477
478 public override bool Flying
479 {
480 // no flying prims for you
481 get { return false; }
482 set { }
483 }
484
485 public override bool IsColliding
486 {
487 get { return iscolliding; }
488 set { iscolliding = value; }
489 }
490
491 public override bool CollidingGround
492 {
493 get { return false; }
494 set { return; }
495 }
496
497 public override bool CollidingObj
498 {
499 get { return false; }
500 set { return; }
501 }
502
503 public override bool ThrottleUpdates
504 {
505 get { return m_throttleUpdates; }
506 set { m_throttleUpdates = value; }
507 }
508
509 public override bool Stopped
510 {
511 get { return _zeroFlag; }
512 }
513
514 public override Vector3 Position
515 {
516 get { return _position; }
517
518 set
519 {
520 _position = value;
521 //m_log.Info("[PHYSICS]: " + _position.ToString());
522 }
523 }
524
525 public override Vector3 Size
526 {
527 get { return _size; }
528 set
529 {
530 if (value.IsFinite())
531 {
532 _size = value;
533 }
534 else
535 {
536 m_log.Warn("[PHYSICS]: Got NaN Size on object");
537 }
538 }
539 }
540
541 public override float Mass
542 {
543 get
544 {
545 CalculateMass();
546 return m_primMass;
547 }
548 }
549
550 public override Vector3 Force
551 {
552 //get { return Vector3.Zero; }
553 get { return m_force; }
554 set
555 {
556 if (value.IsFinite())
557 {
558 m_force = value;
559 }
560 else
561 {
562 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
563 }
564 }
565 }
566
567 public override int VehicleType
568 {
569 get { return (int)m_type; }
570 set { ProcessTypeChange((Vehicle)value); }
571 }
572
573 public override void VehicleFloatParam(int param, float value)
574 {
575 ProcessFloatVehicleParam((Vehicle)param, value);
576 }
577
578 public override void VehicleVectorParam(int param, Vector3 value)
579 {
580 ProcessVectorVehicleParam((Vehicle)param, value);
581 }
582
583 public override void VehicleRotationParam(int param, Quaternion rotation)
584 {
585 ProcessRotationVehicleParam((Vehicle)param, rotation);
586 }
587
588 public override void VehicleFlags(int param, bool remove)
589 {
590 ProcessVehicleFlags(param, remove);
591 }
592
593 public override void SetVolumeDetect(int param)
594 {
595 lock (_parent_scene.OdeLock)
596 {
597 m_isVolumeDetect = (param != 0);
598 }
599 }
600
601 public override Vector3 CenterOfMass
602 {
603 get { return Vector3.Zero; }
604 }
605
606 public override Vector3 GeometricCenter
607 {
608 get { return Vector3.Zero; }
609 }
610
611 public override PrimitiveBaseShape Shape
612 {
613 set
614 {
615 _pbs = value;
616 m_taintshape = true;
617 }
618 }
619
620 public override byte PhysicsShapeType
621 {
622 get
623 {
624 return m_shapetype;
625 }
626 set
627 {
628 m_taintshapetype = value;
629 _parent_scene.AddPhysicsActorTaint(this);
630 }
631 }
632
633 public override Vector3 Velocity
634 {
635 get
636 {
637 // Averate previous velocity with the new one so
638 // client object interpolation works a 'little' better
639 if (_zeroFlag)
640 return Vector3.Zero;
641
642 Vector3 returnVelocity = Vector3.Zero;
643 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
644 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
645 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
646 return returnVelocity;
647 }
648 set
649 {
650 if (value.IsFinite())
651 {
652 _velocity = value;
653 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
654 _acceleration = Vector3.Zero;
655
656 m_taintVelocity = value;
657 _parent_scene.AddPhysicsActorTaint(this);
658 }
659 else
660 {
661 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
662 }
663
664 }
665 }
666
667 public override Vector3 Torque
668 {
669 get
670 {
671 if (!m_isphysical || Body == IntPtr.Zero)
672 return Vector3.Zero;
673
674 return _torque;
675 }
676
677 set
678 {
679 if (value.IsFinite())
680 {
681 m_taintTorque = value;
682 _parent_scene.AddPhysicsActorTaint(this);
683 }
684 else
685 {
686 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
687 }
688 }
689 }
690
691 public override float CollisionScore
692 {
693 get { return m_collisionscore; }
694 set { m_collisionscore = value; }
695 }
696
697 public override bool Kinematic
698 {
699 get { return false; }
700 set { }
701 }
702
703 public override Quaternion Orientation
704 {
705 get { return _orientation; }
706 set
707 {
708 if (QuaternionIsFinite(value))
709 {
710 _orientation = value;
711 }
712 else
713 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
714
715 }
716 }
717
718 public override bool FloatOnWater
719 {
720 set
721 {
722 m_taintCollidesWater = value;
723 _parent_scene.AddPhysicsActorTaint(this);
724 }
725 }
726
727 public override void SetMomentum(Vector3 momentum)
728 {
729 }
730
731 public override Vector3 PIDTarget
732 {
733 set
734 {
735 if (value.IsFinite())
736 {
737 m_PIDTarget = value;
738 }
739 else
740 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
741 }
742 }
743 public override bool PIDActive { set { m_usePID = value; } }
744 public override float PIDTau { set { m_PIDTau = value; } }
745
746 // For RotLookAt
747 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
748 public override bool APIDActive { set { m_useAPID = value; } }
749 public override float APIDStrength { set { m_APIDStrength = value; } }
750 public override float APIDDamping { set { m_APIDDamping = value; } }
751
752 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
753 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
754 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
755 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
756
757 internal static bool QuaternionIsFinite(Quaternion q)
758 {
759 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
760 return false;
761 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
762 return false;
763 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
764 return false;
765 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
766 return false;
767 return true;
768 }
769
770 public override Vector3 Acceleration // client updates read data via here
771 {
772 get
773 {
774 if (_zeroFlag)
775 {
776 return Vector3.Zero;
777 }
778 return _acceleration;
779 }
780 set { _acceleration = value; }
781 }
782
783
784 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
785 {
786 _acceleration = accel;
787 }
788
789 public override void AddForce(Vector3 force, bool pushforce)
790 {
791 if (force.IsFinite())
792 {
793 lock (m_forcelist)
794 m_forcelist.Add(force);
795
796 m_taintforce = true;
797 }
798 else
799 {
800 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
801 }
802 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
803 }
804
805 public override void AddAngularForce(Vector3 force, bool pushforce)
806 {
807 if (force.IsFinite())
808 {
809 m_angularforcelist.Add(force);
810 m_taintaddangularforce = true;
811 }
812 else
813 {
814 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
815 }
816 }
817
818 public override Vector3 RotationalVelocity
819 {
820 get
821 {
822 return m_rotationalVelocity;
823 }
824 set
825 {
826 if (value.IsFinite())
827 {
828 m_rotationalVelocity = value;
829 }
830 else
831 {
832 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
833 }
834 }
835 }
836
837 public override void CrossingFailure()
838 {
839 if (m_outofBounds)
840 {
841 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
842 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
843 _position.Z = Util.Clip(_position.Z, -100f, 50000f);
844 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
845
846 m_lastposition = _position;
847
848 _velocity = Vector3.Zero;
849 m_lastVelocity = _velocity;
850
851
852 if (m_type != Vehicle.TYPE_NONE)
853 Halt();
854
855 d.BodySetLinearVel(Body, 0, 0, 0);
856 base.RequestPhysicsterseUpdate();
857 m_outofBounds = false;
858 }
859 /*
860 int tmp = Interlocked.Increment(ref m_crossingfailures);
861 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
862 {
863 base.RaiseOutOfBounds(_position);
864 return;
865 }
866 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
867 {
868 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
869 }
870 */
871 }
872
873 public override float Buoyancy
874 {
875 get { return m_buoyancy; }
876 set { m_buoyancy = value; }
877 }
878
879 public override void link(PhysicsActor obj)
880 {
881 m_taintparent = obj;
882 }
883
884 public override void delink()
885 {
886 m_taintparent = null;
887 }
888
889 public override void LockAngularMotion(Vector3 axis)
890 {
891 // This is actually ROTATION ENABLE, not a lock.
892 // default is <1,1,1> which is all enabled.
893 // The lock value is updated inside Move(), no point in using the taint system.
894 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
895 if (axis.IsFinite())
896 {
897 axis.X = (axis.X > 0) ? 1f : 0f;
898 axis.Y = (axis.Y > 0) ? 1f : 0f;
899 axis.Z = (axis.Z > 0) ? 1f : 0f;
900 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
901 m_rotateEnableRequest = axis;
902 m_rotateEnableUpdate = true;
903 }
904 else
905 {
906 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
907 }
908 }
909
910 public void SetGeom(IntPtr geom)
911 {
912 if (prim_geom != IntPtr.Zero)
913 {
914 // Remove any old entries
915 //string tPA;
916 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
917 //Console.WriteLine("**** Remove {0}", tPA);
918 if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
919 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
920 d.GeomDestroy(prim_geom);
921 }
922
923 prim_geom = geom;
924 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
925 if (prim_geom != IntPtr.Zero)
926 {
927 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
928 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
929 //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
930 if (m_NoColide)
931 {
932 d.GeomSetCategoryBits(prim_geom, 0);
933 if (m_isphysical && !m_isVolumeDetect)
934 {
935 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
936 }
937 else
938 {
939 d.GeomSetCollideBits(prim_geom, 0);
940 d.GeomDisable(prim_geom);
941 }
942 }
943 else
944 {
945 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
946 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
947 }
948 }
949
950 if (childPrim)
951 {
952 if (_parent != null && _parent is OdePrim)
953 {
954 OdePrim parent = (OdePrim)_parent;
955 //Console.WriteLine("SetGeom calls ChildSetGeom");
956 parent.ChildSetGeom(this);
957 }
958 }
959 //m_log.Warn("Setting Geom to: " + prim_geom);
960 }
961
962 public void enableBodySoft()
963 {
964 if (!childPrim)
965 {
966 if (m_isphysical && Body != IntPtr.Zero)
967 {
968 d.BodyEnable(Body);
969 if (m_type != Vehicle.TYPE_NONE)
970 Enable(Body, _parent_scene);
971 }
972
973 m_disabled = false;
974 }
975 }
976
977 public void disableBodySoft()
978 {
979 m_disabled = true;
980
981 if (m_isphysical && Body != IntPtr.Zero)
982 {
983 d.BodyDisable(Body);
984 Halt();
985 }
986 }
987
988 public void enableBody()
989 {
990 // Don't enable this body if we're a child prim
991 // this should be taken care of in the parent function not here
992 if (!childPrim)
993 {
994 // Sets the geom to a body
995 Body = d.BodyCreate(_parent_scene.world);
996
997 setMass();
998 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
999 d.Quaternion myrot = new d.Quaternion();
1000 myrot.X = _orientation.X;
1001 myrot.Y = _orientation.Y;
1002 myrot.Z = _orientation.Z;
1003 myrot.W = _orientation.W;
1004 d.BodySetQuaternion(Body, ref myrot);
1005 d.GeomSetBody(prim_geom, Body);
1006
1007 m_collisionCategories |= CollisionCategories.Body;
1008 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1009
1010 if (m_NoColide)
1011 {
1012 d.GeomSetCategoryBits(prim_geom, 0);
1013 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1014 }
1015 else
1016 {
1017 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1018 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1019 }
1020
1021 d.BodySetAutoDisableFlag(Body, true);
1022 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1023
1024 // disconnect from world gravity so we can apply buoyancy
1025 d.BodySetGravityMode(Body, false);
1026
1027 m_interpenetrationcount = 0;
1028 m_collisionscore = 0;
1029 m_disabled = false;
1030
1031 if (m_type != Vehicle.TYPE_NONE)
1032 {
1033 Enable(Body, _parent_scene);
1034 }
1035
1036 _parent_scene.addActivePrim(this);
1037 }
1038 }
1039
1040 #region Mass Calculation
1041
1042 private float CalculateMass()
1043 {
1044 float volume = _size.X * _size.Y * _size.Z; // default
1045 float tmp;
1046
1047 float returnMass = 0;
1048 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1049 float hollowVolume = hollowAmount * hollowAmount;
1050
1051 switch (_pbs.ProfileShape)
1052 {
1053 case ProfileShape.Square:
1054 // default box
1055
1056 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1057 {
1058 if (hollowAmount > 0.0)
1059 {
1060 switch (_pbs.HollowShape)
1061 {
1062 case HollowShape.Square:
1063 case HollowShape.Same:
1064 break;
1065
1066 case HollowShape.Circle:
1067
1068 hollowVolume *= 0.78539816339f;
1069 break;
1070
1071 case HollowShape.Triangle:
1072
1073 hollowVolume *= (0.5f * .5f);
1074 break;
1075
1076 default:
1077 hollowVolume = 0;
1078 break;
1079 }
1080 volume *= (1.0f - hollowVolume);
1081 }
1082 }
1083
1084 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1085 {
1086 //a tube
1087
1088 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1089 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1090 volume -= volume * tmp * tmp;
1091
1092 if (hollowAmount > 0.0)
1093 {
1094 hollowVolume *= hollowAmount;
1095
1096 switch (_pbs.HollowShape)
1097 {
1098 case HollowShape.Square:
1099 case HollowShape.Same:
1100 break;
1101
1102 case HollowShape.Circle:
1103 hollowVolume *= 0.78539816339f; ;
1104 break;
1105
1106 case HollowShape.Triangle:
1107 hollowVolume *= 0.5f * 0.5f;
1108 break;
1109 default:
1110 hollowVolume = 0;
1111 break;
1112 }
1113 volume *= (1.0f - hollowVolume);
1114 }
1115 }
1116
1117 break;
1118
1119 case ProfileShape.Circle:
1120
1121 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1122 {
1123 volume *= 0.78539816339f; // elipse base
1124
1125 if (hollowAmount > 0.0)
1126 {
1127 switch (_pbs.HollowShape)
1128 {
1129 case HollowShape.Same:
1130 case HollowShape.Circle:
1131 break;
1132
1133 case HollowShape.Square:
1134 hollowVolume *= 0.5f * 2.5984480504799f;
1135 break;
1136
1137 case HollowShape.Triangle:
1138 hollowVolume *= .5f * 1.27323954473516f;
1139 break;
1140
1141 default:
1142 hollowVolume = 0;
1143 break;
1144 }
1145 volume *= (1.0f - hollowVolume);
1146 }
1147 }
1148
1149 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1150 {
1151 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1152 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1153 volume *= (1.0f - tmp * tmp);
1154
1155 if (hollowAmount > 0.0)
1156 {
1157
1158 // calculate the hollow volume by it's shape compared to the prim shape
1159 hollowVolume *= hollowAmount;
1160
1161 switch (_pbs.HollowShape)
1162 {
1163 case HollowShape.Same:
1164 case HollowShape.Circle:
1165 break;
1166
1167 case HollowShape.Square:
1168 hollowVolume *= 0.5f * 2.5984480504799f;
1169 break;
1170
1171 case HollowShape.Triangle:
1172 hollowVolume *= .5f * 1.27323954473516f;
1173 break;
1174
1175 default:
1176 hollowVolume = 0;
1177 break;
1178 }
1179 volume *= (1.0f - hollowVolume);
1180 }
1181 }
1182 break;
1183
1184 case ProfileShape.HalfCircle:
1185 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1186 {
1187 volume *= 0.52359877559829887307710723054658f;
1188 }
1189 break;
1190
1191 case ProfileShape.EquilateralTriangle:
1192
1193 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1194 {
1195 volume *= 0.32475953f;
1196
1197 if (hollowAmount > 0.0)
1198 {
1199
1200 // calculate the hollow volume by it's shape compared to the prim shape
1201 switch (_pbs.HollowShape)
1202 {
1203 case HollowShape.Same:
1204 case HollowShape.Triangle:
1205 hollowVolume *= .25f;
1206 break;
1207
1208 case HollowShape.Square:
1209 hollowVolume *= 0.499849f * 3.07920140172638f;
1210 break;
1211
1212 case HollowShape.Circle:
1213 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1214 // Cyllinder hollow volume calculation
1215
1216 hollowVolume *= 0.1963495f * 3.07920140172638f;
1217 break;
1218
1219 default:
1220 hollowVolume = 0;
1221 break;
1222 }
1223 volume *= (1.0f - hollowVolume);
1224 }
1225 }
1226 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1227 {
1228 volume *= 0.32475953f;
1229 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1230 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1231 volume *= (1.0f - tmp * tmp);
1232
1233 if (hollowAmount > 0.0)
1234 {
1235
1236 hollowVolume *= hollowAmount;
1237
1238 switch (_pbs.HollowShape)
1239 {
1240 case HollowShape.Same:
1241 case HollowShape.Triangle:
1242 hollowVolume *= .25f;
1243 break;
1244
1245 case HollowShape.Square:
1246 hollowVolume *= 0.499849f * 3.07920140172638f;
1247 break;
1248
1249 case HollowShape.Circle:
1250
1251 hollowVolume *= 0.1963495f * 3.07920140172638f;
1252 break;
1253
1254 default:
1255 hollowVolume = 0;
1256 break;
1257 }
1258 volume *= (1.0f - hollowVolume);
1259 }
1260 }
1261 break;
1262
1263 default:
1264 break;
1265 }
1266
1267
1268
1269 float taperX1;
1270 float taperY1;
1271 float taperX;
1272 float taperY;
1273 float pathBegin;
1274 float pathEnd;
1275 float profileBegin;
1276 float profileEnd;
1277
1278 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1279 {
1280 taperX1 = _pbs.PathScaleX * 0.01f;
1281 if (taperX1 > 1.0f)
1282 taperX1 = 2.0f - taperX1;
1283 taperX = 1.0f - taperX1;
1284
1285 taperY1 = _pbs.PathScaleY * 0.01f;
1286 if (taperY1 > 1.0f)
1287 taperY1 = 2.0f - taperY1;
1288 taperY = 1.0f - taperY1;
1289 }
1290 else
1291 {
1292 taperX = _pbs.PathTaperX * 0.01f;
1293 if (taperX < 0.0f)
1294 taperX = -taperX;
1295 taperX1 = 1.0f - taperX;
1296
1297 taperY = _pbs.PathTaperY * 0.01f;
1298 if (taperY < 0.0f)
1299 taperY = -taperY;
1300 taperY1 = 1.0f - taperY;
1301
1302 }
1303
1304
1305 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1306
1307 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1308 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1309 volume *= (pathEnd - pathBegin);
1310
1311 // this is crude aproximation
1312 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1313 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1314 volume *= (profileEnd - profileBegin);
1315
1316 returnMass = m_density * volume;
1317
1318 if (returnMass <= 0)
1319 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1320 // else if (returnMass > _parent_scene.maximumMassObject)
1321 // returnMass = _parent_scene.maximumMassObject;
1322
1323
1324
1325 m_primMass = returnMass;
1326 if (m_primMass > _parent_scene.maximumMassObject)
1327 m_primMass = _parent_scene.maximumMassObject;
1328
1329 // Recursively calculate mass
1330 bool HasChildPrim = false;
1331 lock (childrenPrim)
1332 {
1333 if (childrenPrim.Count > 0)
1334 {
1335 HasChildPrim = true;
1336 }
1337
1338 }
1339 if (HasChildPrim)
1340 {
1341 OdePrim[] childPrimArr = new OdePrim[0];
1342
1343 lock (childrenPrim)
1344 childPrimArr = childrenPrim.ToArray();
1345
1346 for (int i = 0; i < childPrimArr.Length; i++)
1347 {
1348 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1349 returnMass += childPrimArr[i].CalculateMass();
1350 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1351 if (i > 256)
1352 break;
1353 }
1354 }
1355 if (returnMass > _parent_scene.maximumMassObject)
1356 returnMass = _parent_scene.maximumMassObject;
1357 return returnMass;
1358 }// end CalculateMass
1359
1360 #endregion
1361
1362 public void setMass()
1363 {
1364 if (Body != (IntPtr)0)
1365 {
1366 float newmass = CalculateMass();
1367
1368 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1369
1370 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1371 d.BodySetMass(Body, ref pMass);
1372 }
1373 }
1374
1375 public void disableBody()
1376 {
1377 //this kills the body so things like 'mesh' can re-create it.
1378 lock (this)
1379 {
1380 if (!childPrim)
1381 {
1382 if (Body != IntPtr.Zero)
1383 {
1384 _parent_scene.remActivePrim(this);
1385 m_collisionCategories &= ~CollisionCategories.Body;
1386 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1387
1388 if (prim_geom != IntPtr.Zero)
1389 {
1390 if (m_NoColide)
1391 {
1392 d.GeomSetCategoryBits(prim_geom, 0);
1393 d.GeomSetCollideBits(prim_geom, 0);
1394 d.GeomDisable(prim_geom);
1395 }
1396 else
1397 {
1398 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1399 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1400 }
1401 }
1402
1403 d.BodyDestroy(Body);
1404 lock (childrenPrim)
1405 {
1406 if (childrenPrim.Count > 0)
1407 {
1408 foreach (OdePrim prm in childrenPrim)
1409 {
1410 if (prm.m_NoColide && prm.prim_geom != IntPtr.Zero)
1411 {
1412 d.GeomSetCategoryBits(prm.prim_geom, 0);
1413 d.GeomSetCollideBits(prm.prim_geom, 0);
1414 d.GeomDisable(prm.prim_geom);
1415 }
1416
1417 _parent_scene.remActivePrim(prm);
1418 prm.Body = IntPtr.Zero;
1419 }
1420 }
1421 }
1422 Body = IntPtr.Zero;
1423 }
1424 }
1425 else
1426 {
1427 _parent_scene.remActivePrim(this);
1428
1429 m_collisionCategories &= ~CollisionCategories.Body;
1430 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1431
1432 if (prim_geom != IntPtr.Zero)
1433 {
1434 if (m_NoColide)
1435 {
1436 d.GeomSetCategoryBits(prim_geom, 0);
1437 d.GeomSetCollideBits(prim_geom, 0);
1438 d.GeomDisable(prim_geom);
1439 }
1440 else
1441 {
1442 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1443 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1444 }
1445 }
1446
1447 Body = IntPtr.Zero;
1448 }
1449 }
1450 m_disabled = true;
1451 m_collisionscore = 0;
1452 }
1453
1454// private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1455
1456 public bool setMesh(OdeScene parent_scene, IMesh mesh)
1457 {
1458 //Kill Body so that mesh can re-make the geom
1459 if (IsPhysical && Body != IntPtr.Zero)
1460 {
1461 if (childPrim)
1462 {
1463 if (_parent != null)
1464 {
1465 OdePrim parent = (OdePrim)_parent;
1466 parent.ChildDelink(this);
1467 }
1468 }
1469 else
1470 {
1471 disableBody();
1472 }
1473 }
1474
1475 IntPtr vertices, indices;
1476 int vertexCount, indexCount;
1477 int vertexStride, triStride;
1478 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1479 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1480
1481 // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
1482 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1483
1484 if (vertexCount == 0 || indexCount == 0)
1485 {
1486 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1487 return false;
1488 }
1489
1490 IntPtr geo = IntPtr.Zero;
1491 try
1492 {
1493 _triMeshData = d.GeomTriMeshDataCreate();
1494 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1495 d.GeomTriMeshDataPreprocess(_triMeshData);
1496
1497 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1498
1499 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1500 }
1501 catch (Exception e)
1502 {
1503 m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);
1504
1505 if (_triMeshData != IntPtr.Zero)
1506 {
1507 d.GeomTriMeshDataDestroy(_triMeshData);
1508 _triMeshData = IntPtr.Zero;
1509 }
1510 return false;
1511 }
1512
1513 SetGeom(geo);
1514
1515 return true;
1516 }
1517
1518 public void ProcessTaints(float timestep) //=============================================================================
1519 {
1520 if (m_taintadd)
1521 {
1522 changeadd(timestep);
1523 }
1524
1525 if (m_taintremove)
1526 return;
1527
1528 if (prim_geom != IntPtr.Zero)
1529 {
1530 if (!_position.ApproxEquals(m_taintposition, 0f))
1531 {
1532 changemove(timestep);
1533 }
1534 if (m_taintrot != _orientation)
1535 {
1536 if (childPrim && IsPhysical) // For physical child prim...
1537 {
1538 rotate(timestep);
1539 // KF: ODE will also rotate the parent prim!
1540 // so rotate the root back to where it was
1541 OdePrim parent = (OdePrim)_parent;
1542 parent.rotate(timestep);
1543 }
1544 else
1545 {
1546 //Just rotate the prim
1547 rotate(timestep);
1548 }
1549 }
1550 //
1551 if (m_taintphantom != m_isphantom )
1552 {
1553 changePhantomStatus();
1554 }//
1555
1556 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1557 {
1558 changePhysicsStatus(timestep);
1559 }//
1560
1561
1562 if (!_size.ApproxEquals(m_taintsize, 0f))
1563 changesize(timestep);
1564 //
1565
1566 if(m_taintshapetype != m_shapetype)
1567 {
1568 m_shapetype = m_taintshapetype;
1569 changeshape(timestep);
1570 }
1571
1572 if (m_taintshape)
1573 changeshape(timestep);
1574 //
1575
1576 if (m_taintforce)
1577 changeAddForce(timestep);
1578
1579 if (m_taintaddangularforce)
1580 changeAddAngularForce(timestep);
1581
1582 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1583 changeSetTorque(timestep);
1584
1585 if (m_taintdisable)
1586 changedisable(timestep);
1587
1588 if (m_taintselected != m_isSelected)
1589 changeSelectedStatus();
1590
1591 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1592 changevelocity(timestep);
1593
1594 if (m_taintparent != _parent)
1595 changelink(timestep);
1596
1597 if (m_taintCollidesWater != m_collidesWater)
1598 changefloatonwater(timestep);
1599
1600 if (m_taintvehicledata != null)
1601 DoSetVehicle();
1602
1603 /* obsolete
1604 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1605 changeAngularLock(timestep);
1606 */
1607 }
1608
1609 else
1610 {
1611 m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom");
1612
1613 // not sure this will not flame...
1614 m_taintremove = true;
1615 _parent_scene.AddPhysicsActorTaint(this);
1616 }
1617
1618 }
1619
1620 private void changelink(float timestep)
1621 {
1622 // If the newly set parent is not null
1623 // create link
1624 if (_parent == null && m_taintparent != null)
1625 {
1626 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1627 {
1628 OdePrim obj = (OdePrim)m_taintparent;
1629 obj.ParentPrim(this);
1630 }
1631 }
1632 // If the newly set parent is null
1633 // destroy link
1634 else if (_parent != null && m_taintparent == null)
1635 {
1636 if (_parent is OdePrim)
1637 {
1638 OdePrim obj = (OdePrim)_parent;
1639 obj.ChildDelink(this);
1640 childPrim = false;
1641 }
1642 }
1643
1644 _parent = m_taintparent;
1645 m_taintPhysics = m_isphysical;
1646 }
1647
1648 // I'm the parent
1649 // prim is the child
1650 public void ParentPrim(OdePrim prim)
1651 {
1652 if (this.m_localID != prim.m_localID)
1653 {
1654 if (Body == IntPtr.Zero)
1655 {
1656 Body = d.BodyCreate(_parent_scene.world);
1657 // disconnect from world gravity so we can apply buoyancy
1658 d.BodySetGravityMode(Body, false);
1659
1660 setMass();
1661 }
1662 if (Body != IntPtr.Zero)
1663 {
1664 lock (childrenPrim)
1665 {
1666 if (!childrenPrim.Contains(prim))
1667 {
1668 childrenPrim.Add(prim);
1669
1670 foreach (OdePrim prm in childrenPrim)
1671 {
1672 d.Mass m2;
1673 d.MassSetZero(out m2);
1674 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1675
1676
1677 d.Quaternion quat = new d.Quaternion();
1678 quat.W = prm._orientation.W;
1679 quat.X = prm._orientation.X;
1680 quat.Y = prm._orientation.Y;
1681 quat.Z = prm._orientation.Z;
1682
1683 d.Matrix3 mat = new d.Matrix3();
1684 d.RfromQ(out mat, ref quat);
1685 d.MassRotate(ref m2, ref mat);
1686 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1687 d.MassAdd(ref pMass, ref m2);
1688 }
1689 foreach (OdePrim prm in childrenPrim)
1690 {
1691 if (m_isphantom && !prm.m_isVolumeDetect)
1692 {
1693 prm.m_collisionCategories = 0;
1694 prm.m_collisionFlags = CollisionCategories.Land;
1695 }
1696 else
1697 {
1698 prm.m_collisionCategories |= CollisionCategories.Body;
1699 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1700 }
1701 if (prm.prim_geom == IntPtr.Zero)
1702 {
1703 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1704 continue;
1705 }
1706
1707 if (prm.m_NoColide)
1708 {
1709 d.GeomSetCategoryBits(prm.prim_geom, 0);
1710 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1711 }
1712 else
1713 {
1714 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1715 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1716 }
1717
1718 d.Quaternion quat = new d.Quaternion();
1719 quat.W = prm._orientation.W;
1720 quat.X = prm._orientation.X;
1721 quat.Y = prm._orientation.Y;
1722 quat.Z = prm._orientation.Z;
1723
1724 d.Matrix3 mat = new d.Matrix3();
1725 d.RfromQ(out mat, ref quat);
1726 if (Body != IntPtr.Zero)
1727 {
1728 d.GeomSetBody(prm.prim_geom, Body);
1729 prm.childPrim = true;
1730 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1731 //d.GeomSetOffsetPosition(prim.prim_geom,
1732 // (Position.X - prm.Position.X) - pMass.c.X,
1733 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1734 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1735 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1736 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1737 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1738 d.BodySetMass(Body, ref pMass);
1739 }
1740 else
1741 {
1742 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1743 }
1744
1745 prm.m_interpenetrationcount = 0;
1746 prm.m_collisionscore = 0;
1747 prm.m_disabled = false;
1748
1749 prm.Body = Body;
1750
1751 _parent_scene.addActivePrim(prm);
1752 }
1753
1754 if (m_isphantom && !m_isVolumeDetect)
1755 {
1756 m_collisionCategories = 0;
1757 m_collisionFlags = CollisionCategories.Land;
1758 }
1759 else
1760 {
1761 m_collisionCategories |= CollisionCategories.Body;
1762 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1763 }
1764
1765 if (m_NoColide)
1766 {
1767 d.GeomSetCategoryBits(prim_geom, 0);
1768 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1769 }
1770 else
1771 {
1772 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1773 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1774 }
1775
1776 d.Quaternion quat2 = new d.Quaternion();
1777 quat2.W = _orientation.W;
1778 quat2.X = _orientation.X;
1779 quat2.Y = _orientation.Y;
1780 quat2.Z = _orientation.Z;
1781
1782 d.Matrix3 mat2 = new d.Matrix3();
1783 d.RfromQ(out mat2, ref quat2);
1784 d.GeomSetBody(prim_geom, Body);
1785 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1786 //d.GeomSetOffsetPosition(prim.prim_geom,
1787 // (Position.X - prm.Position.X) - pMass.c.X,
1788 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1789 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1790 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1791 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1792 d.BodySetMass(Body, ref pMass);
1793
1794 d.BodySetAutoDisableFlag(Body, true);
1795 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1796
1797 m_interpenetrationcount = 0;
1798 m_collisionscore = 0;
1799 m_disabled = false;
1800
1801 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1802 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1803
1804 _parent_scene.addActivePrim(this);
1805 }
1806 }
1807 }
1808 }
1809 }
1810
1811 private void ChildSetGeom(OdePrim odePrim)
1812 {
1813 lock (childrenPrim)
1814 {
1815 foreach (OdePrim prm in childrenPrim)
1816 {
1817 prm.disableBody();
1818 }
1819 }
1820 disableBody();
1821
1822 if (Body != IntPtr.Zero)
1823 {
1824 _parent_scene.remActivePrim(this);
1825 }
1826
1827 lock (childrenPrim)
1828 {
1829 foreach (OdePrim prm in childrenPrim)
1830 {
1831 ParentPrim(prm);
1832 }
1833 }
1834 }
1835
1836 private void ChildDelink(OdePrim odePrim)
1837 {
1838 // Okay, we have a delinked child.. need to rebuild the body.
1839 lock (childrenPrim)
1840 {
1841 foreach (OdePrim prm in childrenPrim)
1842 {
1843 prm.childPrim = true;
1844 prm.disableBody();
1845 }
1846 }
1847 disableBody();
1848
1849 lock (childrenPrim)
1850 {
1851 childrenPrim.Remove(odePrim);
1852 }
1853
1854 if (Body != IntPtr.Zero)
1855 {
1856 _parent_scene.remActivePrim(this);
1857 }
1858
1859 lock (childrenPrim)
1860 {
1861 foreach (OdePrim prm in childrenPrim)
1862 {
1863 ParentPrim(prm);
1864 }
1865 }
1866 }
1867
1868 private void changePhantomStatus()
1869 {
1870 m_taintphantom = m_isphantom;
1871 changeSelectedStatus();
1872 }
1873
1874/* not in use
1875 private void SetCollider()
1876 {
1877 SetCollider(m_isSelected, m_isphysical, m_isphantom, m_isSelected);
1878 }
1879
1880 private void SetCollider(bool sel, bool phys, bool phan, bool vdtc)
1881 {
1882 if (sel)
1883 {
1884 m_collisionCategories = CollisionCategories.Selected;
1885 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1886 }
1887 else
1888 {
1889 if (phan && !vdtc)
1890 {
1891 m_collisionCategories = 0;
1892 if (phys)
1893 m_collisionFlags = CollisionCategories.Land;
1894 else
1895 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
1896 }
1897 else
1898 {
1899 m_collisionCategories = CollisionCategories.Geom;
1900 if (phys)
1901 m_collisionCategories |= CollisionCategories.Body;
1902
1903 m_collisionFlags = m_default_collisionFlags;
1904
1905 if (m_collidesLand)
1906 m_collisionFlags |= CollisionCategories.Land;
1907 if (m_collidesWater)
1908 m_collisionFlags |= CollisionCategories.Water;
1909 }
1910 }
1911
1912 if (prim_geom != IntPtr.Zero)
1913 {
1914 if (m_NoColide)
1915 {
1916 d.GeomSetCategoryBits(prim_geom, 0);
1917 if (phys)
1918 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1919 else
1920 {
1921 d.GeomSetCollideBits(prim_geom, 0);
1922 d.GeomDisable(prim_geom);
1923 }
1924 }
1925 else
1926 {
1927 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1928 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1929 }
1930 }
1931 }
1932*/
1933
1934 private void changeSelectedStatus()
1935 {
1936 if (m_taintselected)
1937 {
1938 m_collisionCategories = CollisionCategories.Selected;
1939 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1940
1941 // We do the body disable soft twice because 'in theory' a collision could have happened
1942 // in between the disabling and the collision properties setting
1943 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1944 // through the ground.
1945
1946 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1947 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1948 // so that causes the selected part to wake up and continue moving.
1949
1950 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1951 // assembly will stop simulating during the selection, because of the lack of atomicity
1952 // of select operations (their processing could be interrupted by a thread switch, causing
1953 // simulation to continue before all of the selected object notifications trickle down to
1954 // the physics engine).
1955
1956 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1957 // selected and disabled. then, due to a thread switch, the selection processing is
1958 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1959 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1960 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1961 // up, start simulating again, which in turn wakes up the last 50.
1962
1963 if (m_isphysical)
1964 {
1965 disableBodySoft();
1966 }
1967
1968 if (prim_geom != IntPtr.Zero)
1969 {
1970 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1971 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1972 if (m_NoColide)
1973 d.GeomDisable(prim_geom);
1974 }
1975
1976 if (m_isphysical)
1977 {
1978 disableBodySoft();
1979 }
1980 if (Body != IntPtr.Zero)
1981 {
1982 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1983 d.BodySetForce(Body, 0f, 0f, 0f);
1984 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1985 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1986 }
1987 }
1988 else
1989 {
1990 if (m_isphantom && !m_isVolumeDetect)
1991 {
1992 m_collisionCategories = 0;
1993 if (m_isphysical)
1994 m_collisionFlags = CollisionCategories.Land;
1995 else
1996 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
1997 }
1998 else
1999 {
2000 m_collisionCategories = CollisionCategories.Geom;
2001 if (m_isphysical)
2002 m_collisionCategories |= CollisionCategories.Body;
2003
2004 m_collisionFlags = m_default_collisionFlags;
2005
2006 if (m_collidesLand)
2007 m_collisionFlags |= CollisionCategories.Land;
2008 if (m_collidesWater)
2009 m_collisionFlags |= CollisionCategories.Water;
2010 }
2011
2012 if (prim_geom != IntPtr.Zero)
2013 {
2014 if (m_NoColide)
2015 {
2016 d.GeomSetCategoryBits(prim_geom, 0);
2017 if (m_isphysical)
2018 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2019 else
2020 {
2021 d.GeomSetCollideBits(prim_geom, 0);
2022 d.GeomDisable(prim_geom);
2023 }
2024 }
2025 else
2026 {
2027 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2028 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2029 }
2030 }
2031 if (Body != IntPtr.Zero)
2032 {
2033 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2034 d.BodySetForce(Body, 0f, 0f, 0f);
2035 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
2036 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
2037 }
2038
2039 if (m_isphysical)
2040 {
2041 if (Body != IntPtr.Zero)
2042 {
2043 enableBodySoft();
2044 }
2045 }
2046 }
2047
2048 resetCollisionAccounting();
2049 m_isSelected = m_taintselected;
2050 }//end changeSelectedStatus
2051
2052 public void ResetTaints()
2053 {
2054 m_taintposition = _position;
2055 m_taintrot = _orientation;
2056 m_taintPhysics = m_isphysical;
2057 m_taintselected = m_isSelected;
2058 m_taintsize = _size;
2059 m_taintshape = false;
2060 m_taintforce = false;
2061 m_taintdisable = false;
2062 m_taintVelocity = Vector3.Zero;
2063 }
2064
2065 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
2066 {
2067 bool gottrimesh = false;
2068
2069 m_NoColide = false; // assume all will go well
2070
2071 if (_triMeshData != IntPtr.Zero)
2072 {
2073 d.GeomTriMeshDataDestroy(_triMeshData);
2074 _triMeshData = IntPtr.Zero;
2075 }
2076
2077 if (_mesh != null)
2078 {
2079 gottrimesh = setMesh(_parent_scene, _mesh);
2080 if (!gottrimesh)
2081 {
2082 // getting a mesh failed,
2083 // lets go on having a basic box or sphere, with prim size but not coliding
2084 // physical colides with land, non with nothing
2085
2086 m_NoColide = true;
2087 }
2088 }
2089
2090 if (!gottrimesh)
2091 { // we will have a basic box or sphere
2092 IntPtr geo = IntPtr.Zero;
2093
2094 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
2095 && _size.X == _size.Y && _size.X == _size.Z)
2096 {
2097 // its a sphere
2098 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2099 try
2100 {
2101 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
2102 }
2103 catch (Exception e)
2104 {
2105 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2106 geo = IntPtr.Zero;
2107 ode.dunlock(_parent_scene.world);
2108 }
2109 }
2110 else // make it a box
2111 {
2112 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2113 try
2114 {
2115 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
2116 }
2117 catch (Exception e)
2118 {
2119 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2120 geo = IntPtr.Zero;
2121 ode.dunlock(_parent_scene.world);
2122 }
2123 }
2124
2125 if (geo == IntPtr.Zero) // if this happens it must be fixed
2126 {
2127 // if it does lets stop what we can
2128 // not sure this will not flame...
2129
2130 m_taintremove = true;
2131 _parent_scene.AddPhysicsActorTaint(this);
2132 return;
2133 }
2134
2135 SetGeom(geo); // this processes the m_NoColide
2136 }
2137 }
2138
2139 public void changeadd(float timestep)
2140 {
2141 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2142 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
2143
2144 if (targetspace == IntPtr.Zero)
2145 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2146
2147 m_targetSpace = targetspace;
2148
2149 if (_mesh == null) // && m_meshfailed == false)
2150 {
2151 if (_parent_scene.needsMeshing(_pbs))
2152 {
2153 bool convex;
2154 if (m_shapetype == 2)
2155 convex = true;
2156 else
2157 convex = false;
2158 try
2159 {
2160 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex);
2161 }
2162 catch
2163 {
2164 //Don't continuously try to mesh prims when meshing has failed
2165 m_meshfailed = true;
2166 _mesh = null;
2167 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2168 }
2169 }
2170 }
2171
2172 lock (_parent_scene.OdeLock)
2173 {
2174 CreateGeom(m_targetSpace, _mesh);
2175
2176 if (prim_geom != IntPtr.Zero)
2177 {
2178 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2179 d.Quaternion myrot = new d.Quaternion();
2180 myrot.X = _orientation.X;
2181 myrot.Y = _orientation.Y;
2182 myrot.Z = _orientation.Z;
2183 myrot.W = _orientation.W;
2184 d.GeomSetQuaternion(prim_geom, ref myrot);
2185 }
2186
2187 if (m_isphysical && Body == IntPtr.Zero)
2188 {
2189 enableBody();
2190 }
2191 }
2192
2193 changeSelectedStatus();
2194
2195 m_taintadd = false;
2196 }
2197
2198 public void changemove(float timestep)
2199 {
2200 if (m_isphysical)
2201 {
2202 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
2203 if (!m_taintremove && !childPrim)
2204 {
2205 if (Body == IntPtr.Zero)
2206 enableBody();
2207 //Prim auto disable after 20 frames,
2208 //if you move it, re-enable the prim manually.
2209 if (_parent != null)
2210 {
2211 if (m_linkJoint != IntPtr.Zero)
2212 {
2213 d.JointDestroy(m_linkJoint);
2214 m_linkJoint = IntPtr.Zero;
2215 }
2216 }
2217 if (Body != IntPtr.Zero)
2218 {
2219 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
2220
2221 if (_parent != null)
2222 {
2223 OdePrim odParent = (OdePrim)_parent;
2224 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
2225 {
2226 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
2227 Console.WriteLine("ODEPrim JointCreateFixed !!!");
2228 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
2229 d.JointAttach(m_linkJoint, Body, odParent.Body);
2230 d.JointSetFixed(m_linkJoint);
2231 }
2232 }
2233 d.BodyEnable(Body);
2234 if (m_type != Vehicle.TYPE_NONE)
2235 {
2236 Enable(Body, _parent_scene);
2237 }
2238 }
2239 else
2240 {
2241 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
2242 }
2243 }
2244 //else
2245 // {
2246 //m_log.Debug("[BUG]: race!");
2247 //}
2248 }
2249 else
2250 {
2251 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2252 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2253 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2254
2255 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
2256 m_targetSpace = tempspace;
2257
2258 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2259 if (prim_geom != IntPtr.Zero)
2260 {
2261 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2262
2263 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2264 d.SpaceAdd(m_targetSpace, prim_geom);
2265 }
2266 }
2267
2268 changeSelectedStatus();
2269
2270 resetCollisionAccounting();
2271 m_taintposition = _position;
2272 }
2273
2274 public void rotate(float timestep)
2275 {
2276 d.Quaternion myrot = new d.Quaternion();
2277 myrot.X = _orientation.X;
2278 myrot.Y = _orientation.Y;
2279 myrot.Z = _orientation.Z;
2280 myrot.W = _orientation.W;
2281 if (Body != IntPtr.Zero)
2282 {
2283 // KF: If this is a root prim do BodySet
2284 d.BodySetQuaternion(Body, ref myrot);
2285 }
2286 else
2287 {
2288 // daughter prim, do Geom set
2289 d.GeomSetQuaternion(prim_geom, ref myrot);
2290 }
2291
2292 resetCollisionAccounting();
2293 m_taintrot = _orientation;
2294 }
2295
2296 private void resetCollisionAccounting()
2297 {
2298 m_collisionscore = 0;
2299 m_interpenetrationcount = 0;
2300 m_disabled = false;
2301 }
2302
2303 public void changedisable(float timestep)
2304 {
2305 m_disabled = true;
2306 if (Body != IntPtr.Zero)
2307 {
2308 d.BodyDisable(Body);
2309 Body = IntPtr.Zero;
2310 }
2311
2312 m_taintdisable = false;
2313 }
2314
2315 public void changePhysicsStatus(float timestep)
2316 {
2317 if (m_isphysical == true)
2318 {
2319 if (Body == IntPtr.Zero)
2320 {
2321 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2322 {
2323 changeshape(2f);
2324 }
2325 else
2326 {
2327 enableBody();
2328 }
2329 }
2330 }
2331 else
2332 {
2333 if (Body != IntPtr.Zero)
2334 {
2335 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2336 {
2337 _mesh = null;
2338 changeadd(2f);
2339 }
2340 if (childPrim)
2341 {
2342 if (_parent != null)
2343 {
2344 OdePrim parent = (OdePrim)_parent;
2345 parent.ChildDelink(this);
2346 }
2347 }
2348 else
2349 {
2350 disableBody();
2351 }
2352 }
2353 }
2354
2355 changeSelectedStatus();
2356
2357 resetCollisionAccounting();
2358 m_taintPhysics = m_isphysical;
2359 }
2360
2361 public void changesize(float timestamp)
2362 {
2363
2364 string oldname = _parent_scene.geom_name_map[prim_geom];
2365
2366 if (_size.X <= 0) _size.X = 0.01f;
2367 if (_size.Y <= 0) _size.Y = 0.01f;
2368 if (_size.Z <= 0) _size.Z = 0.01f;
2369
2370 // Cleanup of old prim geometry
2371 if (_mesh != null)
2372 {
2373 // Cleanup meshing here
2374 }
2375 //kill body to rebuild
2376 if (IsPhysical && Body != IntPtr.Zero)
2377 {
2378 if (childPrim)
2379 {
2380 if (_parent != null)
2381 {
2382 OdePrim parent = (OdePrim)_parent;
2383 parent.ChildDelink(this);
2384 }
2385 }
2386 else
2387 {
2388 disableBody();
2389 }
2390 }
2391 if (d.SpaceQuery(m_targetSpace, prim_geom))
2392 {
2393 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2394 d.SpaceRemove(m_targetSpace, prim_geom);
2395 }
2396 // we don't need to do space calculation because the client sends a position update also.
2397
2398 // Construction of new prim
2399 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2400 {
2401 float meshlod = _parent_scene.meshSculptLOD;
2402
2403 if (IsPhysical)
2404 meshlod = _parent_scene.MeshSculptphysicalLOD;
2405 // Don't need to re-enable body.. it's done in SetMesh
2406
2407 IMesh mesh = null;
2408
2409 try
2410 {
2411 if (_parent_scene.needsMeshing(_pbs))
2412 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
2413 }
2414 catch
2415 {
2416 m_meshfailed = true;
2417 mesh = null;
2418 m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2419 }
2420
2421 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2422 CreateGeom(m_targetSpace, mesh);
2423 }
2424 else
2425 {
2426 _mesh = null;
2427 CreateGeom(m_targetSpace, _mesh);
2428 }
2429
2430 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2431 d.Quaternion myrot = new d.Quaternion();
2432 myrot.X = _orientation.X;
2433 myrot.Y = _orientation.Y;
2434 myrot.Z = _orientation.Z;
2435 myrot.W = _orientation.W;
2436 d.GeomSetQuaternion(prim_geom, ref myrot);
2437
2438 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2439 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2440 {
2441 // Re creates body on size.
2442 // EnableBody also does setMass()
2443 enableBody();
2444 d.BodyEnable(Body);
2445 }
2446
2447 _parent_scene.geom_name_map[prim_geom] = oldname;
2448
2449 changeSelectedStatus();
2450 if (childPrim)
2451 {
2452 if (_parent is OdePrim)
2453 {
2454 OdePrim parent = (OdePrim)_parent;
2455 parent.ChildSetGeom(this);
2456 }
2457 }
2458 resetCollisionAccounting();
2459 m_taintsize = _size;
2460 }
2461
2462
2463
2464 public void changefloatonwater(float timestep)
2465 {
2466 m_collidesWater = m_taintCollidesWater;
2467
2468 if (prim_geom != IntPtr.Zero)
2469 {
2470 if (m_collidesWater)
2471 {
2472 m_collisionFlags |= CollisionCategories.Water;
2473 }
2474 else
2475 {
2476 m_collisionFlags &= ~CollisionCategories.Water;
2477 }
2478 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2479 }
2480 }
2481
2482 public void changeshape(float timestamp)
2483 {
2484 string oldname = _parent_scene.geom_name_map[prim_geom];
2485
2486 // Cleanup of old prim geometry and Bodies
2487 if (IsPhysical && Body != IntPtr.Zero)
2488 {
2489 if (childPrim)
2490 {
2491 if (_parent != null)
2492 {
2493 OdePrim parent = (OdePrim)_parent;
2494 parent.ChildDelink(this);
2495 }
2496 }
2497 else
2498 {
2499 disableBody();
2500 }
2501 }
2502
2503
2504 // we don't need to do space calculation because the client sends a position update also.
2505 if (_size.X <= 0) _size.X = 0.01f;
2506 if (_size.Y <= 0) _size.Y = 0.01f;
2507 if (_size.Z <= 0) _size.Z = 0.01f;
2508 // Construction of new prim
2509
2510 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2511 {
2512 // Don't need to re-enable body.. it's done in SetMesh
2513 float meshlod = _parent_scene.meshSculptLOD;
2514 IMesh mesh;
2515
2516 if (IsPhysical)
2517 meshlod = _parent_scene.MeshSculptphysicalLOD;
2518
2519 bool convex;
2520 if (m_shapetype == 2)
2521 convex = true;
2522 else
2523 convex = false;
2524
2525 try
2526 {
2527 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex);
2528 }
2529 catch
2530 {
2531 mesh = null;
2532 m_meshfailed = true;
2533 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2534 }
2535
2536 CreateGeom(m_targetSpace, mesh);
2537
2538 // createmesh returns null when it doesn't mesh.
2539 }
2540 else
2541 {
2542 _mesh = null;
2543 CreateGeom(m_targetSpace, null);
2544 }
2545
2546 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2547 d.Quaternion myrot = new d.Quaternion();
2548 //myrot.W = _orientation.w;
2549 myrot.W = _orientation.W;
2550 myrot.X = _orientation.X;
2551 myrot.Y = _orientation.Y;
2552 myrot.Z = _orientation.Z;
2553 d.GeomSetQuaternion(prim_geom, ref myrot);
2554
2555 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2556 if (IsPhysical && Body == IntPtr.Zero)
2557 {
2558 // Re creates body on size.
2559 // EnableBody also does setMass()
2560 enableBody();
2561 if (Body != IntPtr.Zero)
2562 {
2563 d.BodyEnable(Body);
2564 }
2565 }
2566 _parent_scene.geom_name_map[prim_geom] = oldname;
2567
2568 changeSelectedStatus();
2569 if (childPrim)
2570 {
2571 if (_parent is OdePrim)
2572 {
2573 OdePrim parent = (OdePrim)_parent;
2574 parent.ChildSetGeom(this);
2575 }
2576 }
2577 resetCollisionAccounting();
2578 m_taintshape = false;
2579 }
2580
2581 public void changeAddForce(float timestamp)
2582 {
2583 if (!m_isSelected)
2584 {
2585 lock (m_forcelist)
2586 {
2587 //m_log.Info("[PHYSICS]: dequeing forcelist");
2588 if (IsPhysical)
2589 {
2590 Vector3 iforce = Vector3.Zero;
2591 int i = 0;
2592 try
2593 {
2594 for (i = 0; i < m_forcelist.Count; i++)
2595 {
2596
2597 iforce = iforce + (m_forcelist[i] * 100);
2598 }
2599 }
2600 catch (IndexOutOfRangeException)
2601 {
2602 m_forcelist = new List<Vector3>();
2603 m_collisionscore = 0;
2604 m_interpenetrationcount = 0;
2605 m_taintforce = false;
2606 return;
2607 }
2608 catch (ArgumentOutOfRangeException)
2609 {
2610 m_forcelist = new List<Vector3>();
2611 m_collisionscore = 0;
2612 m_interpenetrationcount = 0;
2613 m_taintforce = false;
2614 return;
2615 }
2616 d.BodyEnable(Body);
2617
2618 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2619 }
2620 m_forcelist.Clear();
2621 }
2622
2623 m_collisionscore = 0;
2624 m_interpenetrationcount = 0;
2625 }
2626
2627 m_taintforce = false;
2628
2629 }
2630
2631
2632
2633 public void changeSetTorque(float timestamp)
2634 {
2635 if (!m_isSelected)
2636 {
2637 if (IsPhysical && Body != IntPtr.Zero)
2638 {
2639 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2640 }
2641 }
2642
2643 m_taintTorque = Vector3.Zero;
2644 }
2645
2646 public void changeAddAngularForce(float timestamp)
2647 {
2648 if (!m_isSelected)
2649 {
2650 lock (m_angularforcelist)
2651 {
2652 //m_log.Info("[PHYSICS]: dequeing forcelist");
2653 if (IsPhysical)
2654 {
2655 Vector3 iforce = Vector3.Zero;
2656 for (int i = 0; i < m_angularforcelist.Count; i++)
2657 {
2658 iforce = iforce + (m_angularforcelist[i] * 100);
2659 }
2660 d.BodyEnable(Body);
2661 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2662
2663 }
2664 m_angularforcelist.Clear();
2665 }
2666
2667 m_collisionscore = 0;
2668 m_interpenetrationcount = 0;
2669 }
2670
2671 m_taintaddangularforce = false;
2672 }
2673
2674 private void changevelocity(float timestep)
2675 {
2676 if (!m_isSelected)
2677 {
2678 Thread.Sleep(20);
2679 if (IsPhysical)
2680 {
2681 if (Body != IntPtr.Zero)
2682 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2683 }
2684
2685 //resetCollisionAccounting();
2686 }
2687 m_taintVelocity = Vector3.Zero;
2688 }
2689
2690 public void UpdatePositionAndVelocity()
2691 {
2692 return; // moved to the Move () method
2693 }
2694
2695 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2696 {
2697 obj.I.M00 = pMat[0, 0];
2698 obj.I.M01 = pMat[0, 1];
2699 obj.I.M02 = pMat[0, 2];
2700 obj.I.M10 = pMat[1, 0];
2701 obj.I.M11 = pMat[1, 1];
2702 obj.I.M12 = pMat[1, 2];
2703 obj.I.M20 = pMat[2, 0];
2704 obj.I.M21 = pMat[2, 1];
2705 obj.I.M22 = pMat[2, 2];
2706 return obj;
2707 }
2708
2709 public override void SubscribeEvents(int ms)
2710 {
2711 m_eventsubscription = ms;
2712 _parent_scene.addCollisionEventReporting(this);
2713 }
2714
2715 public override void UnSubscribeEvents()
2716 {
2717 _parent_scene.remCollisionEventReporting(this);
2718 m_eventsubscription = 0;
2719 }
2720
2721 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2722 {
2723 if (CollisionEventsThisFrame == null)
2724 CollisionEventsThisFrame = new CollisionEventUpdate();
2725 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
2726 }
2727
2728 public void SendCollisions()
2729 {
2730 if (CollisionEventsThisFrame == null)
2731 return;
2732
2733 base.SendCollisionUpdate(CollisionEventsThisFrame);
2734
2735 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
2736 CollisionEventsThisFrame = null;
2737 else
2738 CollisionEventsThisFrame = new CollisionEventUpdate();
2739 }
2740
2741 public override bool SubscribedEvents()
2742 {
2743 if (m_eventsubscription > 0)
2744 return true;
2745 return false;
2746 }
2747
2748 public static Matrix4 Inverse(Matrix4 pMat)
2749 {
2750 if (determinant3x3(pMat) == 0)
2751 {
2752 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2753 }
2754
2755
2756
2757 return (Adjoint(pMat) / determinant3x3(pMat));
2758 }
2759
2760 public static Matrix4 Adjoint(Matrix4 pMat)
2761 {
2762 Matrix4 adjointMatrix = new Matrix4();
2763 for (int i = 0; i < 4; i++)
2764 {
2765 for (int j = 0; j < 4; j++)
2766 {
2767 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2768 }
2769 }
2770
2771 adjointMatrix = Transpose(adjointMatrix);
2772 return adjointMatrix;
2773 }
2774
2775 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2776 {
2777 Matrix4 minor = new Matrix4();
2778 int m = 0, n = 0;
2779 for (int i = 0; i < 4; i++)
2780 {
2781 if (i == iRow)
2782 continue;
2783 n = 0;
2784 for (int j = 0; j < 4; j++)
2785 {
2786 if (j == iCol)
2787 continue;
2788 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2789 n++;
2790 }
2791 m++;
2792 }
2793 return minor;
2794 }
2795
2796 public static Matrix4 Transpose(Matrix4 pMat)
2797 {
2798 Matrix4 transposeMatrix = new Matrix4();
2799 for (int i = 0; i < 4; i++)
2800 for (int j = 0; j < 4; j++)
2801 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
2802 return transposeMatrix;
2803 }
2804
2805 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2806 {
2807 switch (r)
2808 {
2809 case 0:
2810 switch (c)
2811 {
2812 case 0:
2813 pMat.M11 = val;
2814 break;
2815 case 1:
2816 pMat.M12 = val;
2817 break;
2818 case 2:
2819 pMat.M13 = val;
2820 break;
2821 case 3:
2822 pMat.M14 = val;
2823 break;
2824 }
2825
2826 break;
2827 case 1:
2828 switch (c)
2829 {
2830 case 0:
2831 pMat.M21 = val;
2832 break;
2833 case 1:
2834 pMat.M22 = val;
2835 break;
2836 case 2:
2837 pMat.M23 = val;
2838 break;
2839 case 3:
2840 pMat.M24 = val;
2841 break;
2842 }
2843
2844 break;
2845 case 2:
2846 switch (c)
2847 {
2848 case 0:
2849 pMat.M31 = val;
2850 break;
2851 case 1:
2852 pMat.M32 = val;
2853 break;
2854 case 2:
2855 pMat.M33 = val;
2856 break;
2857 case 3:
2858 pMat.M34 = val;
2859 break;
2860 }
2861
2862 break;
2863 case 3:
2864 switch (c)
2865 {
2866 case 0:
2867 pMat.M41 = val;
2868 break;
2869 case 1:
2870 pMat.M42 = val;
2871 break;
2872 case 2:
2873 pMat.M43 = val;
2874 break;
2875 case 3:
2876 pMat.M44 = val;
2877 break;
2878 }
2879
2880 break;
2881 }
2882 }
2883 private static float determinant3x3(Matrix4 pMat)
2884 {
2885 float det = 0;
2886 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2887 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2888 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2889 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2890 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2891 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2892
2893 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2894 return det;
2895
2896 }
2897
2898 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2899 {
2900 dst.c.W = src.c.W;
2901 dst.c.X = src.c.X;
2902 dst.c.Y = src.c.Y;
2903 dst.c.Z = src.c.Z;
2904 dst.mass = src.mass;
2905 dst.I.M00 = src.I.M00;
2906 dst.I.M01 = src.I.M01;
2907 dst.I.M02 = src.I.M02;
2908 dst.I.M10 = src.I.M10;
2909 dst.I.M11 = src.I.M11;
2910 dst.I.M12 = src.I.M12;
2911 dst.I.M20 = src.I.M20;
2912 dst.I.M21 = src.I.M21;
2913 dst.I.M22 = src.I.M22;
2914 }
2915
2916 public override void SetMaterial(int pMaterial)
2917 {
2918 m_material = pMaterial;
2919 }
2920
2921 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2922 {
2923 switch (pParam)
2924 {
2925 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2926 if (pValue < 0.01f) pValue = 0.01f;
2927 // m_angularDeflectionEfficiency = pValue;
2928 break;
2929 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2930 if (pValue < 0.1f) pValue = 0.1f;
2931 // m_angularDeflectionTimescale = pValue;
2932 break;
2933 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2934 if (pValue < 0.3f) pValue = 0.3f;
2935 m_angularMotorDecayTimescale = pValue;
2936 break;
2937 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2938 if (pValue < 0.3f) pValue = 0.3f;
2939 m_angularMotorTimescale = pValue;
2940 break;
2941 case Vehicle.BANKING_EFFICIENCY:
2942 if (pValue < 0.01f) pValue = 0.01f;
2943 // m_bankingEfficiency = pValue;
2944 break;
2945 case Vehicle.BANKING_MIX:
2946 if (pValue < 0.01f) pValue = 0.01f;
2947 // m_bankingMix = pValue;
2948 break;
2949 case Vehicle.BANKING_TIMESCALE:
2950 if (pValue < 0.01f) pValue = 0.01f;
2951 // m_bankingTimescale = pValue;
2952 break;
2953 case Vehicle.BUOYANCY:
2954 if (pValue < -1f) pValue = -1f;
2955 if (pValue > 1f) pValue = 1f;
2956 m_VehicleBuoyancy = pValue;
2957 break;
2958 // case Vehicle.HOVER_EFFICIENCY:
2959 // if (pValue < 0f) pValue = 0f;
2960 // if (pValue > 1f) pValue = 1f;
2961 // m_VhoverEfficiency = pValue;
2962 // break;
2963 case Vehicle.HOVER_HEIGHT:
2964 m_VhoverHeight = pValue;
2965 break;
2966 case Vehicle.HOVER_TIMESCALE:
2967 if (pValue < 0.1f) pValue = 0.1f;
2968 m_VhoverTimescale = pValue;
2969 break;
2970 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2971 if (pValue < 0.01f) pValue = 0.01f;
2972 // m_linearDeflectionEfficiency = pValue;
2973 break;
2974 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2975 if (pValue < 0.01f) pValue = 0.01f;
2976 // m_linearDeflectionTimescale = pValue;
2977 break;
2978 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2979 if (pValue < 0.3f) pValue = 0.3f;
2980 m_linearMotorDecayTimescale = pValue;
2981 break;
2982 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2983 if (pValue < 0.1f) pValue = 0.1f;
2984 m_linearMotorTimescale = pValue;
2985 break;
2986 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2987 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2988 if (pValue > 1.0f) pValue = 1.0f;
2989 m_verticalAttractionEfficiency = pValue;
2990 break;
2991 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2992 if (pValue < 0.1f) pValue = 0.1f;
2993 m_verticalAttractionTimescale = pValue;
2994 break;
2995
2996 // These are vector properties but the engine lets you use a single float value to
2997 // set all of the components to the same value
2998 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2999 if (pValue > 30f) pValue = 30f;
3000 if (pValue < 0.1f) pValue = 0.1f;
3001 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
3002 break;
3003 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3004 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
3005 UpdateAngDecay();
3006 break;
3007 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3008 if (pValue < 0.1f) pValue = 0.1f;
3009 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
3010 break;
3011 case Vehicle.LINEAR_MOTOR_DIRECTION:
3012 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
3013 UpdateLinDecay();
3014 break;
3015 case Vehicle.LINEAR_MOTOR_OFFSET:
3016 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
3017 break;
3018
3019 }
3020
3021 }//end ProcessFloatVehicleParam
3022
3023 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
3024 {
3025 switch (pParam)
3026 {
3027 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
3028 if (pValue.X > 30f) pValue.X = 30f;
3029 if (pValue.X < 0.1f) pValue.X = 0.1f;
3030 if (pValue.Y > 30f) pValue.Y = 30f;
3031 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3032 if (pValue.Z > 30f) pValue.Z = 30f;
3033 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3034 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3035 break;
3036 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3037 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
3038 // Limit requested angular speed to 2 rps= 4 pi rads/sec
3039 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
3040 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
3041 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
3042 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
3043 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
3044 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
3045 UpdateAngDecay();
3046 break;
3047 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3048 if (pValue.X < 0.1f) pValue.X = 0.1f;
3049 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3050 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3051 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3052 break;
3053 case Vehicle.LINEAR_MOTOR_DIRECTION:
3054 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
3055 UpdateLinDecay();
3056 break;
3057 case Vehicle.LINEAR_MOTOR_OFFSET:
3058 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
3059 break;
3060 }
3061
3062 }//end ProcessVectorVehicleParam
3063
3064 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
3065 {
3066 switch (pParam)
3067 {
3068 case Vehicle.REFERENCE_FRAME:
3069 // m_referenceFrame = pValue;
3070 break;
3071 }
3072
3073 }//end ProcessRotationVehicleParam
3074
3075 internal void ProcessVehicleFlags(int pParam, bool remove)
3076 {
3077 if (remove)
3078 {
3079 m_flags &= ~((VehicleFlag)pParam);
3080 }
3081 else
3082 {
3083 m_flags |= (VehicleFlag)pParam;
3084 }
3085 }
3086
3087 internal void ProcessTypeChange(Vehicle pType)
3088 {
3089 // Set Defaults For Type
3090 m_type = pType;
3091 switch (pType)
3092 {
3093 case Vehicle.TYPE_SLED:
3094 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
3095 m_angularFrictionTimescale = new Vector3(30, 30, 30);
3096 // m_lLinMotorVel = Vector3.Zero;
3097 m_linearMotorTimescale = 1000;
3098 m_linearMotorDecayTimescale = 120;
3099 m_angularMotorDirection = Vector3.Zero;
3100 m_angularMotorDVel = Vector3.Zero;
3101 m_angularMotorTimescale = 1000;
3102 m_angularMotorDecayTimescale = 120;
3103 m_VhoverHeight = 0;
3104 // m_VhoverEfficiency = 1;
3105 m_VhoverTimescale = 10;
3106 m_VehicleBuoyancy = 0;
3107 // m_linearDeflectionEfficiency = 1;
3108 // m_linearDeflectionTimescale = 1;
3109 // m_angularDeflectionEfficiency = 1;
3110 // m_angularDeflectionTimescale = 1000;
3111 // m_bankingEfficiency = 0;
3112 // m_bankingMix = 1;
3113 // m_bankingTimescale = 10;
3114 // m_referenceFrame = Quaternion.Identity;
3115 m_flags &=
3116 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3117 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3118 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3119 break;
3120 case Vehicle.TYPE_CAR:
3121 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
3122 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
3123 // m_lLinMotorVel = Vector3.Zero;
3124 m_linearMotorTimescale = 1;
3125 m_linearMotorDecayTimescale = 60;
3126 m_angularMotorDirection = Vector3.Zero;
3127 m_angularMotorDVel = Vector3.Zero;
3128 m_angularMotorTimescale = 1;
3129 m_angularMotorDecayTimescale = 0.8f;
3130 m_VhoverHeight = 0;
3131 // m_VhoverEfficiency = 0;
3132 m_VhoverTimescale = 1000;
3133 m_VehicleBuoyancy = 0;
3134 // // m_linearDeflectionEfficiency = 1;
3135 // // m_linearDeflectionTimescale = 2;
3136 // // m_angularDeflectionEfficiency = 0;
3137 // m_angularDeflectionTimescale = 10;
3138 m_verticalAttractionEfficiency = 1f;
3139 m_verticalAttractionTimescale = 10f;
3140 // m_bankingEfficiency = -0.2f;
3141 // m_bankingMix = 1;
3142 // m_bankingTimescale = 1;
3143 // m_referenceFrame = Quaternion.Identity;
3144 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3145 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
3146 VehicleFlag.LIMIT_MOTOR_UP);
3147 break;
3148 case Vehicle.TYPE_BOAT:
3149 m_linearFrictionTimescale = new Vector3(10, 3, 2);
3150 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3151 // m_lLinMotorVel = Vector3.Zero;
3152 m_linearMotorTimescale = 5;
3153 m_linearMotorDecayTimescale = 60;
3154 m_angularMotorDirection = Vector3.Zero;
3155 m_angularMotorDVel = Vector3.Zero;
3156 m_angularMotorTimescale = 4;
3157 m_angularMotorDecayTimescale = 4;
3158 m_VhoverHeight = 0;
3159 // m_VhoverEfficiency = 0.5f;
3160 m_VhoverTimescale = 2;
3161 m_VehicleBuoyancy = 1;
3162 // m_linearDeflectionEfficiency = 0.5f;
3163 // m_linearDeflectionTimescale = 3;
3164 // m_angularDeflectionEfficiency = 0.5f;
3165 // m_angularDeflectionTimescale = 5;
3166 m_verticalAttractionEfficiency = 0.5f;
3167 m_verticalAttractionTimescale = 5f;
3168 // m_bankingEfficiency = -0.3f;
3169 // m_bankingMix = 0.8f;
3170 // m_bankingTimescale = 1;
3171 // m_referenceFrame = Quaternion.Identity;
3172 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
3173 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3174 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
3175 VehicleFlag.LIMIT_MOTOR_UP);
3176 break;
3177 case Vehicle.TYPE_AIRPLANE:
3178 m_linearFrictionTimescale = new Vector3(200, 10, 5);
3179 m_angularFrictionTimescale = new Vector3(20, 20, 20);
3180 // m_lLinMotorVel = Vector3.Zero;
3181 m_linearMotorTimescale = 2;
3182 m_linearMotorDecayTimescale = 60;
3183 m_angularMotorDirection = Vector3.Zero;
3184 m_angularMotorDVel = Vector3.Zero;
3185 m_angularMotorTimescale = 4;
3186 m_angularMotorDecayTimescale = 4;
3187 m_VhoverHeight = 0;
3188 // m_VhoverEfficiency = 0.5f;
3189 m_VhoverTimescale = 1000;
3190 m_VehicleBuoyancy = 0;
3191 // m_linearDeflectionEfficiency = 0.5f;
3192 // m_linearDeflectionTimescale = 3;
3193 // m_angularDeflectionEfficiency = 1;
3194 // m_angularDeflectionTimescale = 2;
3195 m_verticalAttractionEfficiency = 0.9f;
3196 m_verticalAttractionTimescale = 2f;
3197 // m_bankingEfficiency = 1;
3198 // m_bankingMix = 0.7f;
3199 // m_bankingTimescale = 2;
3200 // m_referenceFrame = Quaternion.Identity;
3201 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3202 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3203 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
3204 break;
3205 case Vehicle.TYPE_BALLOON:
3206 m_linearFrictionTimescale = new Vector3(5, 5, 5);
3207 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3208 m_linearMotorTimescale = 5;
3209 m_linearMotorDecayTimescale = 60;
3210 m_angularMotorDirection = Vector3.Zero;
3211 m_angularMotorDVel = Vector3.Zero;
3212 m_angularMotorTimescale = 6;
3213 m_angularMotorDecayTimescale = 10;
3214 m_VhoverHeight = 5;
3215 // m_VhoverEfficiency = 0.8f;
3216 m_VhoverTimescale = 10;
3217 m_VehicleBuoyancy = 1;
3218 // m_linearDeflectionEfficiency = 0;
3219 // m_linearDeflectionTimescale = 5;
3220 // m_angularDeflectionEfficiency = 0;
3221 // m_angularDeflectionTimescale = 5;
3222 m_verticalAttractionEfficiency = 1f;
3223 m_verticalAttractionTimescale = 100f;
3224 // m_bankingEfficiency = 0;
3225 // m_bankingMix = 0.7f;
3226 // m_bankingTimescale = 5;
3227 // m_referenceFrame = Quaternion.Identity;
3228 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3229 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3230 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3231 break;
3232
3233 }
3234 }//end SetDefaultsForType
3235
3236 internal void Enable(IntPtr pBody, OdeScene pParentScene)
3237 {
3238 if (m_type == Vehicle.TYPE_NONE)
3239 return;
3240
3241 m_body = pBody;
3242 }
3243
3244
3245 internal void Halt()
3246 { // Kill all motions, when non-physical
3247 // m_linearMotorDirection = Vector3.Zero;
3248 m_lLinMotorDVel = Vector3.Zero;
3249 m_lLinObjectVel = Vector3.Zero;
3250 m_wLinObjectVel = Vector3.Zero;
3251 m_angularMotorDirection = Vector3.Zero;
3252 m_lastAngularVelocity = Vector3.Zero;
3253 m_angularMotorDVel = Vector3.Zero;
3254 _acceleration = Vector3.Zero;
3255 }
3256
3257 private void UpdateLinDecay()
3258 {
3259 m_lLinMotorDVel.X = m_linearMotorDirection.X;
3260 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3261 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3262 } // else let the motor decay on its own
3263
3264 private void UpdateAngDecay()
3265 {
3266 m_angularMotorDVel.X = m_angularMotorDirection.X;
3267 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3268 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3269 } // else let the motor decay on its own
3270
3271 public void Move(float timestep)
3272 {
3273 float fx = 0;
3274 float fy = 0;
3275 float fz = 0;
3276 Vector3 linvel; // velocity applied, including any reversal
3277
3278 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3279 // This is a temp patch until proper region crossing is developed.
3280
3281
3282 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3283 {
3284 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3285 bool lastZeroFlag = _zeroFlag; // was it stopped
3286
3287 d.Vector3 vec = d.BodyGetPosition(Body);
3288 Vector3 l_position = Vector3.Zero;
3289 l_position.X = vec.X;
3290 l_position.Y = vec.Y;
3291 l_position.Z = vec.Z;
3292 m_lastposition = _position;
3293 _position = l_position;
3294
3295 d.Quaternion ori = d.BodyGetQuaternion(Body);
3296 // Quaternion l_orientation = Quaternion.Identity;
3297 _orientation.X = ori.X;
3298 _orientation.Y = ori.Y;
3299 _orientation.Z = ori.Z;
3300 _orientation.W = ori.W;
3301 m_lastorientation = _orientation;
3302
3303 d.Vector3 vel = d.BodyGetLinearVel(Body);
3304 m_lastVelocity = _velocity;
3305 _velocity.X = vel.X;
3306 _velocity.Y = vel.Y;
3307 _velocity.Z = vel.Z;
3308 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3309
3310 d.Vector3 torque = d.BodyGetTorque(Body);
3311 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3312
3313
3314 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3315 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3316 )
3317 {
3318 // we are outside current region
3319 // clip position to a stop just outside region and stop it only internally
3320 // do it only once using m_crossingfailures as control
3321 _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f);
3322 _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f);
3323 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3324 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3325 d.BodySetLinearVel(Body, 0, 0, 0);
3326 m_outofBounds = true;
3327 base.RequestPhysicsterseUpdate();
3328 return;
3329 }
3330
3331 base.RequestPhysicsterseUpdate();
3332
3333 if (l_position.Z < 0)
3334 {
3335 // This is so prim that get lost underground don't fall forever and suck up
3336 //
3337 // Sim resources and memory.
3338 // Disables the prim's movement physics....
3339 // It's a hack and will generate a console message if it fails.
3340
3341 //IsPhysical = false;
3342 if (_parent == null) base.RaiseOutOfBounds(_position);
3343
3344
3345 _acceleration.X = 0; // This stuff may stop client display but it has no
3346 _acceleration.Y = 0; // effect on the object in phys engine!
3347 _acceleration.Z = 0;
3348
3349 _velocity.X = 0;
3350 _velocity.Y = 0;
3351 _velocity.Z = 0;
3352 m_lastVelocity = Vector3.Zero;
3353 m_rotationalVelocity.X = 0;
3354 m_rotationalVelocity.Y = 0;
3355 m_rotationalVelocity.Z = 0;
3356
3357 if (_parent == null) base.RequestPhysicsterseUpdate();
3358
3359 m_throttleUpdates = false;
3360 throttleCounter = 0;
3361 _zeroFlag = true;
3362 //outofBounds = true;
3363 } // end neg Z check
3364
3365 // Is it moving?
3366 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3367 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3368 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3369 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3370 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3371 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3372 {
3373 _zeroFlag = true;
3374 m_throttleUpdates = false;
3375 }
3376 else
3377 {
3378 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3379 _zeroFlag = false;
3380 m_lastUpdateSent = false;
3381 //m_throttleUpdates = false;
3382 }
3383
3384 if (_zeroFlag)
3385 { // Its stopped
3386 _velocity.X = 0.0f;
3387 _velocity.Y = 0.0f;
3388 // _velocity.Z = 0.0f;
3389
3390 _acceleration.X = 0;
3391 _acceleration.Y = 0;
3392 // _acceleration.Z = 0;
3393
3394 m_rotationalVelocity.X = 0;
3395 m_rotationalVelocity.Y = 0;
3396 m_rotationalVelocity.Z = 0;
3397 // Stop it in the phys engine
3398 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3399 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3400 d.BodySetForce(Body, 0f, 0f, 0f);
3401
3402 if (!m_lastUpdateSent)
3403 {
3404 m_throttleUpdates = false;
3405 throttleCounter = 0;
3406 if (_parent == null)
3407 {
3408 base.RequestPhysicsterseUpdate();
3409 }
3410
3411 m_lastUpdateSent = true;
3412 }
3413 }
3414 else
3415 { // Its moving
3416 if (lastZeroFlag != _zeroFlag)
3417 {
3418 if (_parent == null)
3419 {
3420 base.RequestPhysicsterseUpdate();
3421 }
3422 }
3423 m_lastUpdateSent = false;
3424 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3425 {
3426 if (_parent == null)
3427 {
3428 base.RequestPhysicsterseUpdate();
3429 }
3430 }
3431 else
3432 {
3433 throttleCounter++;
3434 }
3435 }
3436 m_lastposition = l_position;
3437
3438 /// End UpdatePositionAndVelocity insert
3439
3440
3441 // Rotation lock =====================================
3442 if (m_rotateEnableUpdate)
3443 {
3444 // Snapshot current angles, set up Amotor(s)
3445 m_rotateEnableUpdate = false;
3446 m_rotateEnable = m_rotateEnableRequest;
3447 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3448
3449 if (Amotor != IntPtr.Zero)
3450 {
3451 d.JointDestroy(Amotor);
3452 Amotor = IntPtr.Zero;
3453 //Console.WriteLine("Old Amotor Destroyed");
3454 }
3455
3456 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3457 { // not all are enabled
3458 d.Quaternion r = d.BodyGetQuaternion(Body);
3459 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3460 // extract the axes vectors
3461 Vector3 vX = new Vector3(1f, 0f, 0f);
3462 Vector3 vY = new Vector3(0f, 1f, 0f);
3463 Vector3 vZ = new Vector3(0f, 0f, 1f);
3464 vX = vX * locrot;
3465 vY = vY * locrot;
3466 vZ = vZ * locrot;
3467 // snapshot the current angle vectors
3468 m_lockX = vX;
3469 m_lockY = vY;
3470 m_lockZ = vZ;
3471 // m_lockRot = locrot;
3472 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3473 d.JointAttach(Amotor, Body, IntPtr.Zero);
3474 d.JointSetAMotorMode(Amotor, 0); // User mode??
3475 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3476
3477 float axisnum = 3; // how many to lock
3478 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3479 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3480 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3481
3482 int i = 0;
3483
3484 if (m_rotateEnable.X == 0)
3485 {
3486 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3487 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3488 i++;
3489 }
3490
3491 if (m_rotateEnable.Y == 0)
3492 {
3493 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3494 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3495 i++;
3496 }
3497
3498 if (m_rotateEnable.Z == 0)
3499 {
3500 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3501 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3502 i++;
3503 }
3504
3505 // These lowstops and high stops are effectively (no wiggle room)
3506 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3507 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3508 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3509 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3510 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3511 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3512 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3513 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3514 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3515 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3516 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3517 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3518 } // else none are locked
3519 } // end Rotation Update
3520
3521
3522 // VEHICLE processing ==========================================
3523 if (m_type != Vehicle.TYPE_NONE)
3524 {
3525 // get body attitude
3526 d.Quaternion rot = d.BodyGetQuaternion(Body);
3527 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3528 Quaternion irotq = Quaternion.Inverse(rotq);
3529
3530 // VEHICLE Linear Motion
3531 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3532 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3533 m_lLinObjectVel = vel_now * irotq;
3534 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3535 {
3536 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3537 {
3538 float decayfactor = m_linearMotorDecayTimescale / timestep;
3539 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3540 m_lLinMotorDVel -= decayAmount;
3541 }
3542 else
3543 {
3544 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3545 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3546 m_lLinMotorDVel -= decel;
3547 }
3548 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3549 {
3550 m_lLinMotorDVel = Vector3.Zero;
3551 }
3552
3553 /* else
3554 {
3555 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3556 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3557 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3558 } */
3559 } // end linear motor decay
3560
3561 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3562 {
3563 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3564 if (m_linearMotorTimescale < 300.0f)
3565 {
3566 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3567 float linfactor = m_linearMotorTimescale / timestep;
3568 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3569 m_lLinObjectVel += attackAmount;
3570 }
3571 if (m_linearFrictionTimescale.X < 300.0f)
3572 {
3573 float fricfactor = m_linearFrictionTimescale.X / timestep;
3574 float fricX = m_lLinObjectVel.X / fricfactor;
3575 m_lLinObjectVel.X -= fricX;
3576 }
3577 if (m_linearFrictionTimescale.Y < 300.0f)
3578 {
3579 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3580 float fricY = m_lLinObjectVel.Y / fricfactor;
3581 m_lLinObjectVel.Y -= fricY;
3582 }
3583 if (m_linearFrictionTimescale.Z < 300.0f)
3584 {
3585 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3586 float fricZ = m_lLinObjectVel.Z / fricfactor;
3587 m_lLinObjectVel.Z -= fricZ;
3588 }
3589 }
3590 m_wLinObjectVel = m_lLinObjectVel * rotq;
3591
3592 // Gravity and Buoyancy
3593 Vector3 grav = Vector3.Zero;
3594 if (m_VehicleBuoyancy < 1.0f)
3595 {
3596 // There is some gravity, make a gravity force vector
3597 // that is applied after object velocity.
3598 d.Mass objMass;
3599 d.BodyGetMass(Body, out objMass);
3600 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3601 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3602 } // else its 1.0, no gravity.
3603
3604 // Hovering
3605 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3606 {
3607 // We should hover, get the target height
3608 d.Vector3 pos = d.BodyGetPosition(Body);
3609 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3610 {
3611 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3612 }
3613 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3614 {
3615 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3616 }
3617 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3618 {
3619 m_VhoverTargetHeight = m_VhoverHeight;
3620 }
3621
3622 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3623 {
3624 // If body is aready heigher, use its height as target height
3625 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3626 }
3627
3628 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3629 // m_VhoverTimescale = 0f; // time to acheive height
3630 // timestep is time since last frame,in secs
3631 float herr0 = pos.Z - m_VhoverTargetHeight;
3632 // Replace Vertical speed with correction figure if significant
3633 if (Math.Abs(herr0) > 0.01f)
3634 {
3635 //? d.Mass objMass;
3636 //? d.BodyGetMass(Body, out objMass);
3637 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3638 //KF: m_VhoverEfficiency is not yet implemented
3639 }
3640 else
3641 {
3642 m_wLinObjectVel.Z = 0f;
3643 }
3644 }
3645 else
3646 { // not hovering
3647 if (m_wLinObjectVel.Z == 0f)
3648 { // Gravity rules
3649 m_wLinObjectVel.Z = vel_now.Z;
3650 } // else the motor has it
3651 }
3652 linvel = m_wLinObjectVel;
3653
3654 // Vehicle Linear Motion done =======================================
3655 // Apply velocity
3656 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3657 // apply gravity force
3658 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3659 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3660 // end MoveLinear()
3661
3662
3663 // MoveAngular
3664 /*
3665 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3666
3667 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3668 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3669 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3670
3671 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3672 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3673 */
3674 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3675
3676 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3677 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3678 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3679
3680 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3681
3682 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3683 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3684 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3685 // Decay Angular Motor 2.
3686 if (m_angularMotorDecayTimescale < 300.0f)
3687 {
3688 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3689 {
3690 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3691 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3692 m_angularMotorDVel -= decayAmount;
3693 }
3694 else
3695 {
3696 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3697 m_angularMotorDVel -= decel;
3698 }
3699
3700 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3701 {
3702 m_angularMotorDVel = Vector3.Zero;
3703 }
3704 else
3705 {
3706 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3707 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3708 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3709 }
3710 } // end decay angular motor
3711 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3712
3713 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3714
3715 if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3716 { // if motor or object have motion
3717 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3718
3719 if (m_angularMotorTimescale < 300.0f)
3720 {
3721 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3722 float angfactor = m_angularMotorTimescale / timestep;
3723 Vector3 attackAmount = (attack_error / angfactor);
3724 angObjectVel += attackAmount;
3725 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3726 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3727 }
3728
3729 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3730 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3731 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3732 } // else no signif. motion
3733
3734 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3735 // Bank section tba
3736 // Deflection section tba
3737 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3738
3739
3740 /* // Rotation Axis Disables:
3741 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3742 {
3743 if (m_angularEnable.X == 0)
3744 angObjectVel.X = 0f;
3745 if (m_angularEnable.Y == 0)
3746 angObjectVel.Y = 0f;
3747 if (m_angularEnable.Z == 0)
3748 angObjectVel.Z = 0f;
3749 }
3750 */
3751 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3752
3753 // Vertical attractor section
3754 Vector3 vertattr = Vector3.Zero;
3755
3756 if (m_verticalAttractionTimescale < 300)
3757 {
3758 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3759 // make a vector pointing up
3760 Vector3 verterr = Vector3.Zero;
3761 verterr.Z = 1.0f;
3762 // rotate it to Body Angle
3763 verterr = verterr * rotq;
3764 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
3765 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
3766 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3767
3768 if (verterr.Z < 0.0f)
3769 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3770 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3771 //Console.WriteLine("InvertFlip");
3772 verterr.X = 2.0f - verterr.X;
3773 verterr.Y = 2.0f - verterr.Y;
3774 }
3775 verterr *= 0.5f;
3776 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3777 Vector3 xyav = angObjectVel;
3778 xyav.Z = 0.0f;
3779 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3780 {
3781 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3782 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3783 vertattr.X = verterr.Y;
3784 vertattr.Y = -verterr.X;
3785 vertattr.Z = 0f;
3786 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3787
3788 // scaling appears better usingsquare-law
3789 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3790 float bounce = 1.0f - damped;
3791 // 0 = crit damp, 1 = bouncy
3792 float oavz = angObjectVel.Z; // retain z velocity
3793 // time-scaled correction, which sums, therefore is bouncy:
3794 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3795 // damped, good @ < 90:
3796 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3797 angObjectVel.Z = oavz;
3798 //if(frcount == 0) Console.WriteLine("VA+");
3799 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3800 }
3801 else
3802 {
3803 // else error is very small
3804 angObjectVel.X = 0f;
3805 angObjectVel.Y = 0f;
3806 //if(frcount == 0) Console.WriteLine("VA0");
3807 }
3808 } // else vertical attractor is off
3809 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3810
3811
3812 m_lastAngularVelocity = angObjectVel;
3813 // apply Angular Velocity to body
3814 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3815 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3816
3817 } // end VEHICLES
3818 else
3819 {
3820 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3821
3822 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3823
3824 /// Dynamics Buoyancy
3825 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3826 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3827 // NB Prims in ODE are no subject to global gravity
3828 // This should only affect gravity operations
3829
3830 float m_mass = CalculateMass();
3831 // calculate z-force due togravity on object.
3832 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3833 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3834 {
3835 fz = 0; // llMoveToTarget ignores gravity.
3836 // it also ignores mass of object, and any physical resting on it.
3837 // Vector3 m_PIDTarget is where we are going
3838 // float m_PIDTau is time to get there
3839 fx = 0;
3840 fy = 0;
3841 d.Vector3 pos = d.BodyGetPosition(Body);
3842 Vector3 error = new Vector3(
3843 (m_PIDTarget.X - pos.X),
3844 (m_PIDTarget.Y - pos.Y),
3845 (m_PIDTarget.Z - pos.Z));
3846 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3847 { // Very close, Jump there and quit move
3848
3849 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3850 _target_velocity = Vector3.Zero;
3851 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3852 d.BodySetForce(Body, 0f, 0f, 0f);
3853 }
3854 else
3855 {
3856 float scale = 50.0f * timestep / m_PIDTau;
3857 if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
3858 {
3859 // Nearby, quit update of velocity
3860 }
3861 else
3862 { // Far, calc damped velocity
3863 _target_velocity = error * scale;
3864 }
3865 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3866 }
3867 } // end PID MoveToTarget
3868
3869
3870 /// Dynamics Hover ===================================================================================
3871 // Hover PID Controller can only run if the PIDcontroller is not in use.
3872 if (m_useHoverPID && !m_usePID)
3873 {
3874 //Console.WriteLine("Hover " + m_primName);
3875
3876 // If we're using the PID controller, then we have no gravity
3877 fz = (-1 * _parent_scene.gravityz) * m_mass;
3878
3879 // no lock; for now it's only called from within Simulate()
3880
3881 // If the PID Controller isn't active then we set our force
3882 // calculating base velocity to the current position
3883
3884 if ((m_PIDTau < 1))
3885 {
3886 PID_G = PID_G / m_PIDTau;
3887 }
3888
3889 if ((PID_G - m_PIDTau) <= 0)
3890 {
3891 PID_G = m_PIDTau + 1;
3892 }
3893
3894
3895 // Where are we, and where are we headed?
3896 d.Vector3 pos = d.BodyGetPosition(Body);
3897 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3898
3899
3900 // Non-Vehicles have a limited set of Hover options.
3901 // determine what our target height really is based on HoverType
3902 switch (m_PIDHoverType)
3903 {
3904 case PIDHoverType.Ground:
3905 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3906 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3907 break;
3908 case PIDHoverType.GroundAndWater:
3909 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3910 m_waterHeight = _parent_scene.GetWaterLevel();
3911 if (m_groundHeight > m_waterHeight)
3912 {
3913 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3914 }
3915 else
3916 {
3917 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3918 }
3919 break;
3920
3921 } // end switch (m_PIDHoverType)
3922
3923
3924 _target_velocity =
3925 new Vector3(0.0f, 0.0f,
3926 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3927 );
3928
3929 // if velocity is zero, use position control; otherwise, velocity control
3930
3931 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3932 {
3933 // keep track of where we stopped. No more slippin' & slidin'
3934
3935 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3936 // react to the physics scene by moving it's position.
3937 // Avatar to Avatar collisions
3938 // Prim to avatar collisions
3939 d.Vector3 dlinvel = vel;
3940 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3941 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3942 d.BodyAddForce(Body, 0, 0, fz);
3943 //KF this prevents furthur motions return;
3944 }
3945 else
3946 {
3947 _zeroFlag = false;
3948
3949 // We're flying and colliding with something
3950 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3951 }
3952 } // end m_useHoverPID && !m_usePID
3953
3954
3955 /// Dynamics Apply Forces ===================================================================================
3956 fx *= m_mass;
3957 fy *= m_mass;
3958 //fz *= m_mass;
3959 fx += m_force.X;
3960 fy += m_force.Y;
3961 fz += m_force.Z;
3962
3963 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3964 if (fx != 0 || fy != 0 || fz != 0)
3965 {
3966 //m_taintdisable = true;
3967 //base.RaiseOutOfBounds(Position);
3968 //d.BodySetLinearVel(Body, fx, fy, 0f);
3969 if (!d.BodyIsEnabled(Body))
3970 {
3971 // A physical body at rest on a surface will auto-disable after a while,
3972 // this appears to re-enable it incase the surface it is upon vanishes,
3973 // and the body should fall again.
3974 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3975 d.BodySetForce(Body, 0f, 0f, 0f);
3976 enableBodySoft();
3977 }
3978
3979 // 35x10 = 350n times the mass per second applied maximum.
3980 float nmax = 35f * m_mass;
3981 float nmin = -35f * m_mass;
3982
3983
3984 if (fx > nmax)
3985 fx = nmax;
3986 if (fx < nmin)
3987 fx = nmin;
3988 if (fy > nmax)
3989 fy = nmax;
3990 if (fy < nmin)
3991 fy = nmin;
3992 d.BodyAddForce(Body, fx, fy, fz);
3993 } // end apply forces
3994 } // end Vehicle/Dynamics
3995
3996 /// RotLookAt / LookAt =================================================================================
3997 if (m_useAPID)
3998 {
3999 // RotLookAt, apparently overrides all other rotation sources. Inputs:
4000 // Quaternion m_APIDTarget
4001 // float m_APIDStrength // From SL experiments, this is the time to get there
4002 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
4003 // Also in SL the mass of the object has no effect on time to get there.
4004 // Factors:
4005 // get present body rotation
4006 float limit = 1.0f;
4007 float rscaler = 50f; // adjusts rotation damping time
4008 float lscaler = 10f; // adjusts linear damping time in llLookAt
4009 float RLAservo = 0f;
4010 Vector3 diff_axis;
4011 float diff_angle;
4012 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
4013 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
4014 Quaternion rtarget = new Quaternion();
4015
4016 if (m_APIDTarget.W == -99.9f)
4017 {
4018 // this is really a llLookAt(), x,y,z is the target vector
4019 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
4020 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
4021 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
4022 float twopi = 2.0f * (float)Math.PI;
4023 Vector3 dir = target - _position;
4024 dir.Normalize();
4025 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
4026 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
4027 float terot = (float)Math.Atan2(dir.Z, txy);
4028 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
4029 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
4030 float oerot = (float)Math.Atan2(ospin.Z, oxy);
4031 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
4032 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
4033 float roll = (float)Math.Atan2(ra, rb);
4034 float errorz = tzrot - ozrot;
4035 if (errorz > (float)Math.PI) errorz -= twopi;
4036 else if (errorz < -(float)Math.PI) errorz += twopi;
4037 float errory = oerot - terot;
4038 if (errory > (float)Math.PI) errory -= twopi;
4039 else if (errory < -(float)Math.PI) errory += twopi;
4040 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
4041 if (diff_angle > 0.01f * m_APIDdamper)
4042 {
4043 m_APIDdamper = 1.0f;
4044 RLAservo = timestep / m_APIDStrength * rscaler;
4045 errorz *= RLAservo;
4046 errory *= RLAservo;
4047 error.X = -roll * 8.0f;
4048 error.Y = errory;
4049 error.Z = errorz;
4050 error *= rotq;
4051 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
4052 }
4053 else
4054 {
4055 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4056 m_APIDdamper = 2.0f;
4057 }
4058 }
4059 else
4060 {
4061 // this is a llRotLookAt()
4062 rtarget = m_APIDTarget;
4063
4064 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
4065 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
4066 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
4067
4068 // diff_axis.Normalize(); it already is!
4069 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
4070 {
4071 m_APIDdamper = 1.0f;
4072 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
4073 rotforce = rotforce * rotq;
4074 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
4075 RLAservo = timestep / m_APIDStrength * lscaler;
4076 rotforce = rotforce * RLAservo * diff_angle;
4077 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
4078 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
4079 }
4080 else
4081 { // close enough
4082 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4083 m_APIDdamper = 2.0f;
4084 }
4085 } // end llLookAt/llRotLookAt
4086 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
4087 } // end m_useAPID
4088 } // end root prims
4089 } // end Move()
4090 } // end class
4091}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..712029e
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,384 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenMetaverse;
34using OpenSim.Region.Physics.Manager;
35using Ode.NET;
36using log4net;
37
38namespace OpenSim.Region.Physics.OdePlugin
39{
40 /// <summary>
41 /// Processes raycast requests as ODE is in a state to be able to do them.
42 /// This ensures that it's thread safe and there will be no conflicts.
43 /// Requests get returned by a different thread then they were requested by.
44 /// </summary>
45 public class ODERayCastRequestManager
46 {
47 /// <summary>
48 /// Pending Raycast Requests
49 /// </summary>
50 protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
51
52 /// <summary>
53 /// Scene that created this object.
54 /// </summary>
55 private OdeScene m_scene;
56
57 /// <summary>
58 /// ODE contact array to be filled by the collision testing
59 /// </summary>
60 d.ContactGeom[] contacts = new d.ContactGeom[5];
61
62 /// <summary>
63 /// ODE near callback delegate
64 /// </summary>
65 private d.NearCallback nearCallback;
66 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
67 private List<ContactResult> m_contactResults = new List<ContactResult>();
68
69
70 public ODERayCastRequestManager(OdeScene pScene)
71 {
72 m_scene = pScene;
73 nearCallback = near;
74
75 }
76
77 /// <summary>
78 /// Queues a raycast
79 /// </summary>
80 /// <param name="position">Origin of Ray</param>
81 /// <param name="direction">Ray normal</param>
82 /// <param name="length">Ray length</param>
83 /// <param name="retMethod">Return method to send the results</param>
84 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
85 {
86 lock (m_PendingRequests)
87 {
88 ODERayCastRequest req = new ODERayCastRequest();
89 req.callbackMethod = retMethod;
90 req.length = length;
91 req.Normal = direction;
92 req.Origin = position;
93
94 m_PendingRequests.Add(req);
95 }
96 }
97
98 /// <summary>
99 /// Process all queued raycast requests
100 /// </summary>
101 /// <returns>Time in MS the raycasts took to process.</returns>
102 public int ProcessQueuedRequests()
103 {
104 int time = System.Environment.TickCount;
105 lock (m_PendingRequests)
106 {
107 if (m_PendingRequests.Count > 0)
108 {
109 ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
110 for (int i = 0; i < reqs.Length; i++)
111 {
112 try
113 {
114 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
115 RayCast(reqs[i]); // if there isn't anyone to send results
116 }
117 catch
118 {
119 //Fail silently
120 //This can genuinely happen because raycast requests are queued, and the actor may have
121 //been removed from the scene since it was queued
122 }
123 }
124 /*
125 foreach (ODERayCastRequest req in m_PendingRequests)
126 {
127 if (req.callbackMethod != null) // quick optimization here, don't raycast
128 RayCast(req); // if there isn't anyone to send results to
129
130 }
131 */
132 m_PendingRequests.Clear();
133 }
134 }
135
136 lock (m_contactResults)
137 m_contactResults.Clear();
138
139 return System.Environment.TickCount - time;
140 }
141
142 /// <summary>
143 /// Method that actually initiates the raycast
144 /// </summary>
145 /// <param name="req"></param>
146 private void RayCast(ODERayCastRequest req)
147 {
148 // Create the ray
149 IntPtr ray = d.CreateRay(m_scene.space, req.length);
150 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
151
152 // Collide test
153 d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
154
155 // Remove Ray
156 d.GeomDestroy(ray);
157
158
159 // Define default results
160 bool hitYN = false;
161 uint hitConsumerID = 0;
162 float distance = 999999999999f;
163 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
164 Vector3 snormal = Vector3.Zero;
165
166 // Find closest contact and object.
167 lock (m_contactResults)
168 {
169 foreach (ContactResult cResult in m_contactResults)
170 {
171 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
172 {
173 closestcontact = cResult.Pos;
174 hitConsumerID = cResult.ConsumerID;
175 distance = cResult.Depth;
176 hitYN = true;
177 snormal = cResult.Normal;
178 }
179 }
180
181 m_contactResults.Clear();
182 }
183
184 // Return results
185 if (req.callbackMethod != null)
186 req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
187 }
188
189 // This is the standard Near. Uses space AABBs to speed up detection.
190 private void near(IntPtr space, IntPtr g1, IntPtr g2)
191 {
192
193 //Don't test against heightfield Geom, or you'll be sorry!
194
195 /*
196 terminate called after throwing an instance of 'std::bad_alloc'
197 what(): std::bad_alloc
198 Stacktrace:
199
200 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
201 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
202 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
203 at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
204 fffff>
205 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
206 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
207 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
208 0x00114>
209 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
210 at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
211 at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
212 at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
213 at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
214 at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
215
216 Native stacktrace:
217
218 mono [0x80d2a42]
219 [0xb7f5840c]
220 /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
221 /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
222 /usr/lib/libstdc++.so.6 [0xb45fa865]
223 /usr/lib/libstdc++.so.6 [0xb45fa8a2]
224 /usr/lib/libstdc++.so.6 [0xb45fa9da]
225 /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
226 /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
227 libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
228 libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
229 libode.so(dCollide+0x102) [0xb46571b2]
230 [0x95cfdec9]
231 [0x8ea07fe1]
232 [0xab260146]
233 libode.so [0xb465a5c4]
234 libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
235 libode.so(dSpaceCollide2+0x177) [0xb465ac67]
236 [0x95cf978e]
237 [0x8ea07945]
238 [0x95cf2bbc]
239 [0xab2787e7]
240 [0xab419fb3]
241 [0xab416657]
242 [0xab415bda]
243 [0xb609b08e]
244 mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
245 mono [0x81a2f0f]
246 mono [0x81d28b6]
247 mono [0x81ea2c6]
248 /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
249 /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
250 */
251
252 // Exclude heightfield geom
253
254 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
255 return;
256 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
257 return;
258
259 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
260 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
261 {
262 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
263 return;
264
265 // Separating static prim geometry spaces.
266 // We'll be calling near recursivly if one
267 // of them is a space to find all of the
268 // contact points in the space
269 try
270 {
271 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
272 }
273 catch (AccessViolationException)
274 {
275 m_log.Warn("[PHYSICS]: Unable to collide test a space");
276 return;
277 }
278 //Colliding a space or a geom with a space or a geom. so drill down
279
280 //Collide all geoms in each space..
281 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
282 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
283 return;
284 }
285
286 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
287 return;
288
289 int count = 0;
290 try
291 {
292
293 if (g1 == g2)
294 return; // Can't collide with yourself
295
296 lock (contacts)
297 {
298 count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
299 }
300 }
301 catch (SEHException)
302 {
303 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.");
304 }
305 catch (Exception e)
306 {
307 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
308 return;
309 }
310
311 PhysicsActor p1 = null;
312 PhysicsActor p2 = null;
313
314 if (g1 != IntPtr.Zero)
315 m_scene.actor_name_map.TryGetValue(g1, out p1);
316
317 if (g2 != IntPtr.Zero)
318 m_scene.actor_name_map.TryGetValue(g1, out p2);
319
320 // Loop over contacts, build results.
321 for (int i = 0; i < count; i++)
322 {
323 if (p1 != null) {
324 if (p1 is OdePrim)
325 {
326 ContactResult collisionresult = new ContactResult();
327
328 collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
329 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
330 collisionresult.Depth = contacts[i].depth;
331 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
332 contacts[i].normal.Z);
333 lock (m_contactResults)
334 m_contactResults.Add(collisionresult);
335 }
336 }
337
338 if (p2 != null)
339 {
340 if (p2 is OdePrim)
341 {
342 ContactResult collisionresult = new ContactResult();
343
344 collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
345 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
346 collisionresult.Depth = contacts[i].depth;
347 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
348 contacts[i].normal.Z);
349
350 lock (m_contactResults)
351 m_contactResults.Add(collisionresult);
352 }
353 }
354
355
356 }
357
358 }
359
360 /// <summary>
361 /// Dereference the creator scene so that it can be garbage collected if needed.
362 /// </summary>
363 internal void Dispose()
364 {
365 m_scene = null;
366 }
367 }
368
369 public struct ODERayCastRequest
370 {
371 public Vector3 Origin;
372 public Vector3 Normal;
373 public float length;
374 public RaycastCallback callbackMethod;
375 }
376
377 public struct ContactResult
378 {
379 public Vector3 Pos;
380 public float Depth;
381 public uint ConsumerID;
382 public Vector3 Normal;
383 }
384}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
new file mode 100644
index 0000000..b4a3c48
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
@@ -0,0 +1,48 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using OpenMetaverse;
30using Ode.NET;
31using OpenSim.Framework;
32using OpenSim.Region.Physics.Manager;
33using OpenSim.Region.Physics.OdePlugin;
34
35namespace OpenSim.Region.Physics.OdePlugin
36{
37 class OdePhysicsJoint : PhysicsJoint
38 {
39 public override bool IsInPhysicsEngine
40 {
41 get
42 {
43 return (jointID != IntPtr.Zero);
44 }
45 }
46 public IntPtr jointID;
47 }
48}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..00f5122
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3887 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28//#define USE_DRAWSTUFF
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using Ode.NET;
40#if USE_DRAWSTUFF
41using Drawstuff.NET;
42#endif
43using OpenSim.Framework;
44using OpenSim.Region.Physics.Manager;
45using OpenMetaverse;
46
47//using OpenSim.Region.Physics.OdePlugin.Meshing;
48
49namespace OpenSim.Region.Physics.OdePlugin
50{
51 /// <summary>
52 /// ODE plugin
53 /// </summary>
54 public class OdePlugin : IPhysicsPlugin
55 {
56 //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
57
58 private CollisionLocker ode;
59 private OdeScene _mScene;
60
61 public OdePlugin()
62 {
63 ode = new CollisionLocker();
64 }
65
66 public bool Init()
67 {
68 return true;
69 }
70
71 public PhysicsScene GetScene(String sceneIdentifier)
72 {
73 if (_mScene == null)
74 {
75 if (Util.IsWindows())
76 Util.LoadArchSpecificWindowsDll("ode.dll");
77
78 // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
79 // http://opensimulator.org/mantis/view.php?id=2750).
80 d.InitODE();
81
82 _mScene = new OdeScene(ode, sceneIdentifier);
83 }
84 return (_mScene);
85 }
86
87 public string GetName()
88 {
89 return ("ChODE");
90 }
91
92 public void Dispose()
93 {
94 }
95 }
96
97 public enum StatusIndicators : int
98 {
99 Generic = 0,
100 Start = 1,
101 End = 2
102 }
103
104 public struct sCollisionData
105 {
106 public uint ColliderLocalId;
107 public uint CollidedWithLocalId;
108 public int NumberOfCollisions;
109 public int CollisionType;
110 public int StatusIndicator;
111 public int lastframe;
112 }
113
114 [Flags]
115 public enum CollisionCategories : int
116 {
117 Disabled = 0,
118 Geom = 0x00000001,
119 Body = 0x00000002,
120 Space = 0x00000004,
121 Character = 0x00000008,
122 Land = 0x00000010,
123 Water = 0x00000020,
124 Wind = 0x00000040,
125 Sensor = 0x00000080,
126 Selected = 0x00000100
127 }
128
129 /// <summary>
130 /// Material type for a primitive
131 /// </summary>
132 public enum Material : int
133 {
134 /// <summary></summary>
135 Stone = 0,
136 /// <summary></summary>
137 Metal = 1,
138 /// <summary></summary>
139 Glass = 2,
140 /// <summary></summary>
141 Wood = 3,
142 /// <summary></summary>
143 Flesh = 4,
144 /// <summary></summary>
145 Plastic = 5,
146 /// <summary></summary>
147 Rubber = 6
148
149 }
150
151 public sealed class OdeScene : PhysicsScene
152 {
153 private readonly ILog m_log;
154 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
155
156 CollisionLocker ode;
157
158 private Random fluidRandomizer = new Random(Environment.TickCount);
159
160 private const uint m_regionWidth = Constants.RegionSize;
161 private const uint m_regionHeight = Constants.RegionSize;
162
163 private float ODE_STEPSIZE = 0.020f;
164 private float metersInSpace = 29.9f;
165 private float m_timeDilation = 1.0f;
166
167 public float gravityx = 0f;
168 public float gravityy = 0f;
169 public float gravityz = -9.8f;
170
171 private float contactsurfacelayer = 0.001f;
172
173 private int worldHashspaceLow = -4;
174 private int worldHashspaceHigh = 128;
175
176 private int smallHashspaceLow = -4;
177 private int smallHashspaceHigh = 66;
178
179 private float waterlevel = 0f;
180 private int framecount = 0;
181 //private int m_returncollisions = 10;
182
183 private readonly IntPtr contactgroup;
184
185 internal IntPtr LandGeom;
186 internal IntPtr WaterGeom;
187
188 private float nmTerrainContactFriction = 255.0f;
189 private float nmTerrainContactBounce = 0.1f;
190 private float nmTerrainContactERP = 0.1025f;
191
192 private float mTerrainContactFriction = 75f;
193 private float mTerrainContactBounce = 0.1f;
194 private float mTerrainContactERP = 0.05025f;
195
196 private float nmAvatarObjectContactFriction = 250f;
197 private float nmAvatarObjectContactBounce = 0.1f;
198
199 private float mAvatarObjectContactFriction = 75f;
200 private float mAvatarObjectContactBounce = 0.1f;
201
202 private float avPIDD = 3200f;
203 private float avPIDP = 1400f;
204 private float avCapRadius = 0.37f;
205 private float avStandupTensor = 2000000f;
206 private bool avCapsuleTilted = true; // true = old compatibility mode with leaning capsule; false = new corrected mode
207 public bool IsAvCapsuleTilted { get { return avCapsuleTilted; } set { avCapsuleTilted = value; } }
208 private float avDensity = 80f;
209 private float avHeightFudgeFactor = 0.52f;
210 private float avMovementDivisorWalk = 1.3f;
211 private float avMovementDivisorRun = 0.8f;
212 private float minimumGroundFlightOffset = 3f;
213 public float maximumMassObject = 10000.01f;
214
215 public bool meshSculptedPrim = true;
216 public bool forceSimplePrimMeshing = false;
217
218 public float meshSculptLOD = 32;
219 public float MeshSculptphysicalLOD = 16;
220
221 public float geomDefaultDensity = 10.000006836f;
222
223 public int geomContactPointsStartthrottle = 3;
224 public int geomUpdatesPerThrottledUpdate = 15;
225
226 public float bodyPIDD = 35f;
227 public float bodyPIDG = 25;
228
229 public int geomCrossingFailuresBeforeOutofbounds = 5;
230 public float geomRegionFence = 0.0f;
231
232 public float bodyMotorJointMaxforceTensor = 2;
233
234 public int bodyFramesAutoDisable = 20;
235
236 private DateTime m_lastframe = DateTime.UtcNow;
237
238 private float[] _watermap;
239 private bool m_filterCollisions = true;
240
241 private d.NearCallback nearCallback;
242 public d.TriCallback triCallback;
243 public d.TriArrayCallback triArrayCallback;
244 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
245 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
246 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
247 private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
248 private readonly Object _taintedPrimLock = new Object();
249 private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
250 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
251 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
252 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
253 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
254 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
255 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
256 private bool m_NINJA_physics_joints_enabled = false;
257 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
258 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
259 private d.ContactGeom[] contacts;
260 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
261 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
262 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
263 private readonly List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
264 private Object externalJointRequestsLock = new Object();
265 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
266 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
267 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
268 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
269
270 private d.Contact ContactCopy; // local copy that can be modified
271 private d.Contact TerrainContact;
272 private d.Contact AvatarStaticprimContact; // was 'contact'
273 private d.Contact AvatarMovementprimContact;
274 private d.Contact AvatarMovementTerrainContact;
275 private d.Contact WaterContact;
276 private d.Contact[,] m_materialContacts;
277
278//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
279//Ckrinke private int m_randomizeWater = 200;
280 private int m_physicsiterations = 10;
281 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
282 private readonly PhysicsActor PANull = new NullPhysicsActor();
283 private float step_time = 0.0f;
284//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
285//Ckrinke private int ms = 0;
286 public IntPtr world;
287 //private bool returncollisions = false;
288 // private uint obj1LocalID = 0;
289 private uint obj2LocalID = 0;
290 //private int ctype = 0;
291 private OdeCharacter cc1;
292 private OdePrim cp1;
293 private OdeCharacter cc2;
294 private OdePrim cp2;
295 //private int cStartStop = 0;
296 //private string cDictKey = "";
297
298 public IntPtr space;
299
300 //private IntPtr tmpSpace;
301 // split static geometry collision handling into spaces of 30 meters
302 public IntPtr[,] staticPrimspace;
303
304 public Object OdeLock;
305
306 public IMesher mesher;
307
308 private IConfigSource m_config;
309
310 public bool physics_logging = false;
311 public int physics_logging_interval = 0;
312 public bool physics_logging_append_existing_logfile = false;
313
314 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
315 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
316
317 // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
318 // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
319 // TODO: unused: private uint heightmapWidthSamples;
320 // TODO: unused: private uint heightmapHeightSamples;
321
322 private volatile int m_global_contactcount = 0;
323
324 private Vector3 m_worldOffset = Vector3.Zero;
325 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
326 private PhysicsScene m_parentScene = null;
327
328 private ODERayCastRequestManager m_rayCastManager;
329
330 /// <summary>
331 /// Initiailizes the scene
332 /// Sets many properties that ODE requires to be stable
333 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
334 /// </summary>
335 public OdeScene(CollisionLocker dode, string sceneIdentifier)
336 {
337 m_log
338 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
339
340 OdeLock = new Object();
341 ode = dode;
342 nearCallback = near;
343 triCallback = TriCallback;
344 triArrayCallback = TriArrayCallback;
345 m_rayCastManager = new ODERayCastRequestManager(this);
346 lock (OdeLock)
347 {
348 // Create the world and the first space
349 world = d.WorldCreate();
350 space = d.HashSpaceCreate(IntPtr.Zero);
351
352
353 contactgroup = d.JointGroupCreate(0);
354 //contactgroup
355
356 d.WorldSetAutoDisableFlag(world, false);
357 #if USE_DRAWSTUFF
358
359 Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
360 viewthread.Start();
361 #endif
362 }
363
364
365 _watermap = new float[258 * 258];
366
367 // Zero out the prim spaces array (we split our space into smaller spaces so
368 // we can hit test less.
369 }
370
371#if USE_DRAWSTUFF
372 public void startvisualization(object o)
373 {
374 ds.Functions fn;
375 fn.version = ds.VERSION;
376 fn.start = new ds.CallbackFunction(start);
377 fn.step = new ds.CallbackFunction(step);
378 fn.command = new ds.CallbackFunction(command);
379 fn.stop = null;
380 fn.path_to_textures = "./textures";
381 string[] args = new string[0];
382 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
383 }
384#endif
385
386 // Initialize the mesh plugin
387 public override void Initialise(IMesher meshmerizer, IConfigSource config)
388 {
389 mesher = meshmerizer;
390 m_config = config;
391 // Defaults
392
393 if (Environment.OSVersion.Platform == PlatformID.Unix)
394 {
395 avPIDD = 3200.0f;
396 avPIDP = 1400.0f;
397 avStandupTensor = 2000000f;
398 }
399 else
400 {
401 avPIDD = 2200.0f;
402 avPIDP = 900.0f;
403 avStandupTensor = 550000f;
404 }
405
406 int contactsPerCollision = 80;
407
408 if (m_config != null)
409 {
410 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
411 if (physicsconfig != null)
412 {
413 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
414 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
415 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
416
417 worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
418 worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
419
420 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
421 smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
422 smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
423
424 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
425
426 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
427 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
428 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
429
430 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
431 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
432 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
433
434 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
435 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
436
437 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
438 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
439
440 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
441 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
442
443 avDensity = physicsconfig.GetFloat("av_density", 80f);
444 avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
445 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
446 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
447 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
448 avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
449
450 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
451
452 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
453 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
454 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
455 geomRegionFence = physicsconfig.GetFloat("region_border_fence", 0.0f);
456
457 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
458 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
459
460 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
461 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
462
463 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
464 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
465 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
466 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
467 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
468
469 if (Environment.OSVersion.Platform == PlatformID.Unix)
470 {
471 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
472 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
473 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
474 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
475 }
476 else
477 {
478 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
479 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
480 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
481 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
482 }
483
484 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
485 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
486 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
487
488 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
489 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
490 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
491 }
492 }
493
494 contacts = new d.ContactGeom[contactsPerCollision];
495
496 staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
497
498 // Avatar static on a Prim parameters
499 AvatarStaticprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
500 AvatarStaticprimContact.surface.mu = 255.0f;
501 AvatarStaticprimContact.surface.bounce = 0.0f;
502 AvatarStaticprimContact.surface.soft_cfm = 0.0f;
503 AvatarStaticprimContact.surface.soft_erp = 0.30f; // If this is too small static Av will fall through a sloping prim. 1.0 prevents fall-thru
504
505 // Avatar moving on a Prim parameters
506 AvatarMovementprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
507 AvatarMovementprimContact.surface.mu = 255.0f;
508 AvatarMovementprimContact.surface.bounce = 0.0f;
509 AvatarMovementprimContact.surface.soft_cfm = 0.0f; // if this is 0.01 then prims become phantom to Avs!
510 AvatarMovementprimContact.surface.soft_erp = 0.3f;
511
512 // Static Avatar on Terrain parameters
513 // Keeps Avatar in place better
514 TerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
515 TerrainContact.surface.mu = 255.0f;
516 TerrainContact.surface.bounce = 0.0f;
517 TerrainContact.surface.soft_cfm = 0.0f;
518 TerrainContact.surface.soft_erp = 0.05f;
519
520 // Moving Avatar on Terrain parameters
521 AvatarMovementTerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
522 AvatarMovementTerrainContact.surface.mu = 75f;
523 AvatarMovementTerrainContact.surface.bounce = 0.0f;
524 AvatarMovementTerrainContact.surface.soft_cfm = 0.0f;
525 AvatarMovementTerrainContact.surface.soft_erp = 0.05f;
526
527 // Avatar or prim the the water, this may not be used, possibly water is same as air?
528 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
529 WaterContact.surface.mu = 0.0f; // No friction
530 WaterContact.surface.bounce = 0.0f; // No bounce
531 WaterContact.surface.soft_cfm = 0.010f;
532 WaterContact.surface.soft_erp = 0.010f;
533
534
535 // Prim static or moving on a prim, depends on material type
536 m_materialContacts = new d.Contact[7,2];
537 // V 1 = Sliding; 0 = static or fell onto
538 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
539 m_materialContacts[(int)Material.Stone, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
540 m_materialContacts[(int)Material.Stone, 0].surface.mu = 60f; // friction, 1 = slippery, 255 = no slip
541 m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f;
542 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.0f;
543 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; // erp also changes friction, more erp=less friction
544
545 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
546 m_materialContacts[(int)Material.Stone, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
547 m_materialContacts[(int)Material.Stone, 1].surface.mu = 40f;
548 m_materialContacts[(int)Material.Stone, 1].surface.bounce = 0.0f;
549 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.0f;
550 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.50f;
551
552 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
553 m_materialContacts[(int)Material.Metal, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
554 m_materialContacts[(int)Material.Metal, 0].surface.mu = 15f;
555 m_materialContacts[(int)Material.Metal, 0].surface.bounce = 0.2f;
556 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.0f;
557 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.50f;
558
559 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
560 m_materialContacts[(int)Material.Metal, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
561 m_materialContacts[(int)Material.Metal, 1].surface.mu = 10f;
562 m_materialContacts[(int)Material.Metal, 1].surface.bounce = 0.2f;
563 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.0f;
564 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.50f;
565
566 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
567 m_materialContacts[(int)Material.Glass, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
568 m_materialContacts[(int)Material.Glass, 0].surface.mu = 7.5f;
569 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.0f;
570 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.0f;
571 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.50f;
572
573 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
574 m_materialContacts[(int)Material.Glass, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
575 m_materialContacts[(int)Material.Glass, 1].surface.mu = 5f;
576 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.0f;
577 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.0f;
578 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.50f;
579
580 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
581 m_materialContacts[(int)Material.Wood, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
582 m_materialContacts[(int)Material.Wood, 0].surface.mu = 45f;
583 m_materialContacts[(int)Material.Wood, 0].surface.bounce = 0.1f;
584 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.0f;
585 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.50f;
586
587 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
588 m_materialContacts[(int)Material.Wood, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
589 m_materialContacts[(int)Material.Wood, 1].surface.mu = 30f;
590 m_materialContacts[(int)Material.Wood, 1].surface.bounce = 0.1f;
591 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.0f;
592 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.50f;
593
594 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
595 m_materialContacts[(int)Material.Flesh, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
596 m_materialContacts[(int)Material.Flesh, 0].surface.mu = 150f;
597 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = 0.0f;
598 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.0f;
599 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.50f;
600
601 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
602 m_materialContacts[(int)Material.Flesh, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
603 m_materialContacts[(int)Material.Flesh, 1].surface.mu = 100f;
604 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = 0.0f;
605 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.0f;
606 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.50f;
607
608 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
609 m_materialContacts[(int)Material.Plastic, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
610 m_materialContacts[(int)Material.Plastic, 0].surface.mu = 30f;
611 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = 0.2f;
612 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.0f;
613 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.50f;
614
615 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
616 m_materialContacts[(int)Material.Plastic, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
617 m_materialContacts[(int)Material.Plastic, 1].surface.mu = 20f;
618 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = 0.2f;
619 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.0f;
620 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.50f;
621
622 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
623 m_materialContacts[(int)Material.Rubber, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
624 m_materialContacts[(int)Material.Rubber, 0].surface.mu = 150f;
625 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = 0.7f;
626 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.0f;
627 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.50f;
628
629 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
630 m_materialContacts[(int)Material.Rubber, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
631 m_materialContacts[(int)Material.Rubber, 1].surface.mu = 100f;
632 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = 0.7f;
633 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.0f;
634 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.50f;
635
636 d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
637
638 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
639
640 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
641 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
642
643
644 d.WorldSetLinearDampingThreshold(world, 256f);
645 d.WorldSetLinearDamping(world, 256f);
646// d.WorldSetLinearDampingThreshold(world, 0.01f);
647// d.WorldSetLinearDamping(world, 0.1f);
648 d.WorldSetAngularDampingThreshold(world, 256f);
649 d.WorldSetAngularDamping(world, 256f);
650 d.WorldSetMaxAngularSpeed(world, 256f);
651
652 // Set how many steps we go without running collision testing
653 // This is in addition to the step size.
654 // Essentially Steps * m_physicsiterations
655 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
656 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
657
658
659
660 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
661 {
662 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
663 {
664 staticPrimspace[i, j] = IntPtr.Zero;
665 }
666 }
667 }
668
669 internal void waitForSpaceUnlock(IntPtr space)
670 {
671 //if (space != IntPtr.Zero)
672 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
673 }
674
675 /// <summary>
676 /// Debug space message for printing the space that a prim/avatar is in.
677 /// </summary>
678 /// <param name="pos"></param>
679 /// <returns>Returns which split up space the given position is in.</returns>
680 public string whichspaceamIin(Vector3 pos)
681 {
682 return calculateSpaceForGeom(pos).ToString();
683 }
684
685 #region Collision Detection
686
687 /// <summary>
688 /// This is our near callback. A geometry is near a body
689 /// </summary>
690 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
691 /// <param name="g1">a geometry or space</param>
692 /// <param name="g2">another geometry or space</param>
693 private void near(IntPtr space, IntPtr g1, IntPtr g2)
694 {
695 // no lock here! It's invoked from within Simulate(), which is thread-locked
696
697 // Test if we're colliding a geom with a space.
698 // If so we have to drill down into the space recursively
699//Console.WriteLine("near -----------"); //##
700 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
701 {
702 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
703 return;
704
705 // Separating static prim geometry spaces.
706 // We'll be calling near recursivly if one
707 // of them is a space to find all of the
708 // contact points in the space
709 try
710 {
711 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
712 }
713 catch (AccessViolationException)
714 {
715 m_log.Warn("[PHYSICS]: Unable to collide test a space");
716 return;
717 }
718 //Colliding a space or a geom with a space or a geom. so drill down
719
720 //Collide all geoms in each space..
721 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
722 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
723 return;
724 }
725
726 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
727 return;
728
729 IntPtr b1 = d.GeomGetBody(g1);
730 IntPtr b2 = d.GeomGetBody(g2);
731
732 // d.GeomClassID id = d.GeomGetClass(g1);
733
734 String name1 = null;
735 String name2 = null;
736
737 if (!geom_name_map.TryGetValue(g1, out name1))
738 {
739 name1 = "null";
740 }
741 if (!geom_name_map.TryGetValue(g2, out name2))
742 {
743 name2 = "null";
744 }
745
746 //if (id == d.GeomClassId.TriMeshClass)
747 //{
748 // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
749 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
750 //}
751
752 // Figure out how many contact points we have
753 int count = 0;
754 try
755 {
756 // Colliding Geom To Geom
757 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
758
759 if (g1 == g2)
760 return; // Can't collide with yourself
761
762 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
763 return;
764
765 lock (contacts)
766 {
767 count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
768 if (count > contacts.Length)
769 m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
770 }
771 }
772 catch (SEHException)
773 {
774 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.");
775 ode.drelease(world);
776 base.TriggerPhysicsBasedRestart();
777 }
778 catch (Exception e)
779 {
780 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
781 return;
782 }
783
784 PhysicsActor p1;
785 PhysicsActor p2;
786
787 if (!actor_name_map.TryGetValue(g1, out p1))
788 {
789 p1 = PANull;
790 }
791
792 if (!actor_name_map.TryGetValue(g2, out p2))
793 {
794 p2 = PANull;
795 }
796
797 ContactPoint maxDepthContact = new ContactPoint();
798 if (p1.CollisionScore + count >= float.MaxValue)
799 p1.CollisionScore = 0;
800 p1.CollisionScore += count;
801
802 if (p2.CollisionScore + count >= float.MaxValue)
803 p2.CollisionScore = 0;
804 p2.CollisionScore += count;
805 for (int i = 0; i < count; i++)
806 {
807 d.ContactGeom curContact = contacts[i];
808
809 if (curContact.depth > maxDepthContact.PenetrationDepth)
810 {
811 maxDepthContact = new ContactPoint(
812 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
813 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
814 curContact.depth
815 );
816 }
817
818 //m_log.Warn("[CCOUNT]: " + count);
819 IntPtr joint;
820 // If we're colliding with terrain, use 'TerrainContact' instead of AvatarStaticprimContact.
821 // allows us to have different settings
822
823 // We only need to test p2 for 'jump crouch purposes'
824 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
825 {
826 // Testing if the collision is at the feet of the avatar
827
828 //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));
829//#@ if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
830//#@ p2.IsColliding = true;
831 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)){ //##
832 p2.IsColliding = true; //##
833 }else{
834
835 } //##
836 }
837 else
838 {
839 p2.IsColliding = true;
840 }
841
842 //if ((framecount % m_returncollisions) == 0)
843
844 switch (p1.PhysicsActorType)
845 {
846 case (int)ActorTypes.Agent:
847 p2.CollidingObj = true;
848 break;
849 case (int)ActorTypes.Prim:
850 if (p2.Velocity.LengthSquared() > 0.0f)
851 p2.CollidingObj = true;
852 break;
853 case (int)ActorTypes.Unknown:
854 p2.CollidingGround = true;
855 break;
856 default:
857 p2.CollidingGround = true;
858 break;
859 }
860
861 // we don't want prim or avatar to explode
862
863 #region InterPenetration Handling - Unintended physics explosions
864# region disabled code1
865
866 if (curContact.depth >= 0.08f)
867 {
868 //This is disabled at the moment only because it needs more tweaking
869 //It will eventually be uncommented
870 /*
871 if (AvatarStaticprimContact.depth >= 1.00f)
872 {
873 //m_log.Debug("[PHYSICS]: " + AvatarStaticprimContact.depth.ToString());
874 }
875
876 //If you interpenetrate a prim with an agent
877 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
878 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
879 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
880 p2.PhysicsActorType == (int) ActorTypes.Prim))
881 {
882
883 //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth * 4.15f;
884 /*
885 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
886 {
887 p2.CollidingObj = true;
888 AvatarStaticprimContact.depth = 0.003f;
889 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
890 OdeCharacter character = (OdeCharacter) p2;
891 character.SetPidStatus(true);
892 AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p1.Size.X / 2), AvatarStaticprimContact.pos.Y + (p1.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p1.Size.Z / 2));
893
894 }
895 else
896 {
897
898 //AvatarStaticprimContact.depth = 0.0000000f;
899 }
900 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
901 {
902
903 p1.CollidingObj = true;
904 AvatarStaticprimContact.depth = 0.003f;
905 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
906 AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p2.Size.X / 2), AvatarStaticprimContact.pos.Y + (p2.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p2.Size.Z / 2));
907 OdeCharacter character = (OdeCharacter)p1;
908 character.SetPidStatus(true);
909 }
910 else
911 {
912
913 //AvatarStaticprimContact.depth = 0.0000000f;
914 }
915
916
917
918 }
919*/
920 // If you interpenetrate a prim with another prim
921 /*
922 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
923 {
924 #region disabledcode2
925 //OdePrim op1 = (OdePrim)p1;
926 //OdePrim op2 = (OdePrim)p2;
927 //op1.m_collisionscore++;
928 //op2.m_collisionscore++;
929
930 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
931 //{
932 //op1.m_taintdisable = true;
933 //AddPhysicsActorTaint(p1);
934 //op2.m_taintdisable = true;
935 //AddPhysicsActorTaint(p2);
936 //}
937
938 //if (AvatarStaticprimContact.depth >= 0.25f)
939 //{
940 // Don't collide, one or both prim will expld.
941
942 //op1.m_interpenetrationcount++;
943 //op2.m_interpenetrationcount++;
944 //interpenetrations_before_disable = 200;
945 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
946 //{
947 //op1.m_taintdisable = true;
948 //AddPhysicsActorTaint(p1);
949 //}
950 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
951 //{
952 // op2.m_taintdisable = true;
953 //AddPhysicsActorTaint(p2);
954 //}
955
956 //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth / 8f;
957 //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1);
958 //}
959 //if (op1.m_disabled || op2.m_disabled)
960 //{
961 //Manually disabled objects stay disabled
962 //AvatarStaticprimContact.depth = 0f;
963 //}
964 #endregion
965 }
966 */
967#endregion
968 if (curContact.depth >= 1.00f)
969 {
970 //m_log.Info("[P]: " + AvatarStaticprimContact.depth.ToString());
971 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
972 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
973 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
974 p2.PhysicsActorType == (int) ActorTypes.Unknown))
975 {
976 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
977 {
978 if (p2 is OdeCharacter)
979 {
980 OdeCharacter character = (OdeCharacter) p2;
981
982 //p2.CollidingObj = true;
983 curContact.depth = 0.00000003f;
984 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
985 curContact.pos =
986 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
987 curContact.pos.Y + (p1.Size.Y/2),
988 curContact.pos.Z + (p1.Size.Z/2));
989 character.SetPidStatus(true);
990 }
991 }
992
993
994 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
995 {
996 if (p1 is OdeCharacter)
997 {
998 OdeCharacter character = (OdeCharacter) p1;
999
1000 //p2.CollidingObj = true;
1001 curContact.depth = 0.00000003f;
1002 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1003 curContact.pos =
1004 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1005 curContact.pos.Y + (p1.Size.Y/2),
1006 curContact.pos.Z + (p1.Size.Z/2));
1007 character.SetPidStatus(true);
1008 }
1009 }
1010 }
1011 }
1012 }
1013
1014 #endregion
1015
1016 // Logic for collision handling
1017 // Note, that if *all* contacts are skipped (VolumeDetect)
1018 // The prim still detects (and forwards) collision events but
1019 // appears to be phantom for the world
1020 Boolean skipThisContact = false;
1021
1022 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1023 skipThisContact = true; // No collision on volume detect prims
1024
1025 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1026 skipThisContact = true; // No collision on volume detect prims
1027
1028 if (!skipThisContact && curContact.depth < 0f)
1029 skipThisContact = true;
1030
1031 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1032 skipThisContact = true;
1033
1034 const int maxContactsbeforedeath = 4000;
1035 joint = IntPtr.Zero;
1036
1037 if (!skipThisContact)
1038 {
1039 // Add contact joints with materials params----------------------------------
1040 // p1 is what is being hit, p2 is the physical object doing the hitting
1041 int material = (int) Material.Wood;
1042 int movintYN = 0; // 1 = Sliding; 0 = static or fell onto
1043 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) movintYN = 1;
1044
1045 // If we're colliding against terrain
1046 if (name1 == "Terrain" || name2 == "Terrain")
1047 {
1048 // If we're moving
1049 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && (movintYN == 1))
1050 {
1051 //$ Av walk/run on terrain (not falling) Use the Avatar movement terrain contact
1052 AvatarMovementTerrainContact.geom = curContact;
1053 _perloopContact.Add(curContact);
1054 if (m_global_contactcount < maxContactsbeforedeath)
1055 {
1056 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1057 m_global_contactcount++;
1058 }
1059 }
1060 else
1061 {
1062 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1063 {
1064 //$ Av standing on terrain, Use the non moving Avata terrain contact
1065 TerrainContact.geom = curContact;
1066 _perloopContact.Add(curContact);
1067 if (m_global_contactcount < maxContactsbeforedeath)
1068 {
1069 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1070 m_global_contactcount++;
1071 }
1072 }
1073 else
1074 {
1075 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1076 {
1077 //& THIS NEVER HAPPENS? prim prim contact In terrain contact?
1078 // int pj294950 = 0;
1079 // prim terrain contact
1080
1081 if (p2 is OdePrim)
1082 material = ((OdePrim)p2).m_material;
1083 //m_log.DebugFormat("Material: {0}", material);
1084 m_materialContacts[material, movintYN].geom = curContact;
1085 _perloopContact.Add(curContact);
1086
1087 if (m_global_contactcount < maxContactsbeforedeath)
1088 {
1089 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1090 m_global_contactcount++;
1091
1092 }
1093
1094 }
1095 else
1096 {
1097 //$ prim on terrain contact
1098 if (p2 is OdePrim)
1099 material = ((OdePrim)p2).m_material;
1100 //m_log.DebugFormat("Material: {0}", material);
1101 m_materialContacts[material, movintYN].geom = curContact;
1102 _perloopContact.Add(curContact);
1103
1104 ContactCopy = m_materialContacts[material, movintYN];
1105 if(movintYN == 1)
1106 {
1107 // prevent excessive slide on terrain
1108 ContactCopy.surface.mu = m_materialContacts[material, movintYN].surface.mu * 30.0f;
1109 }
1110
1111 if (m_global_contactcount < maxContactsbeforedeath)
1112 {
1113 joint = d.JointCreateContact(world, contactgroup, ref ContactCopy);
1114 m_global_contactcount++;
1115 }
1116 }
1117 }
1118 }
1119 }
1120 else if (name1 == "Water" || name2 == "Water")
1121 {
1122 //$ This never happens! Perhaps water is treated like air?
1123 /*
1124 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1125 {
1126 }
1127 else
1128 {
1129 }
1130 */
1131 //WaterContact.surface.soft_cfm = 0.0000f;
1132 //WaterContact.surface.soft_erp = 0.00000f;
1133 if (curContact.depth > 0.1f)
1134 {
1135 curContact.depth *= 52;
1136 //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1);
1137 //AvatarStaticprimContact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1138 }
1139 WaterContact.geom = curContact;
1140 _perloopContact.Add(curContact);
1141 if (m_global_contactcount < maxContactsbeforedeath)
1142 {
1143 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1144 m_global_contactcount++;
1145 }
1146 //m_log.Info("[PHYSICS]: Prim Water Contact" + AvatarStaticprimContact.depth);
1147 }
1148 else
1149 {
1150
1151 // no terrain and no water, we're colliding with prim or avatar
1152 // check if we're moving
1153 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1154 {
1155 //$ Avatar on Prim or other Avatar
1156 if (movintYN == 1)
1157 {
1158 // Use the AV Movement / prim contact
1159 AvatarMovementprimContact.geom = curContact;
1160 _perloopContact.Add(curContact);
1161 if (m_global_contactcount < maxContactsbeforedeath)
1162 {
1163 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1164 m_global_contactcount++;
1165 }
1166 }
1167 else
1168 {
1169 // Use the Av non movement / prim contact
1170 AvatarStaticprimContact.geom = curContact;
1171 _perloopContact.Add(curContact);
1172 ContactCopy = AvatarStaticprimContact; // local copy so we can change locally
1173
1174 if (m_global_contactcount < maxContactsbeforedeath)
1175 {
1176 if (curContact.depth > 0.2)
1177 { // embedded, eject slowly
1178 ContactCopy.surface.soft_erp = 0.1f;
1179 ContactCopy.surface.soft_cfm = 0.1f;
1180 }
1181 else
1182 { // keep on the surface
1183 ContactCopy.surface.soft_erp = 0.3f;
1184 ContactCopy.surface.soft_cfm = 0.0f;
1185 }
1186 joint = d.JointCreateContact(world, contactgroup, ref ContactCopy);
1187 m_global_contactcount++;
1188 }
1189 }
1190 }
1191 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1192 {
1193 //$ Prim on Prim
1194 //p1.PhysicsActorType
1195
1196 if (p2 is OdePrim) material = ((OdePrim)p2).m_material;
1197 //m_log.DebugFormat("Material: {0}", material);
1198
1199 m_materialContacts[material, movintYN].geom = curContact;
1200 _perloopContact.Add(curContact);
1201
1202 if (m_global_contactcount < maxContactsbeforedeath)
1203 {
1204 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1205 m_global_contactcount++;
1206 }
1207 }
1208 }
1209
1210 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1211 {
1212 d.JointAttach(joint, b1, b2);
1213 m_global_contactcount++;
1214 }
1215
1216 }
1217 collision_accounting_events(p1, p2, maxDepthContact);
1218 if (count > geomContactPointsStartthrottle)
1219 {
1220 // If there are more then 3 contact points, it's likely
1221 // that we've got a pile of objects, so ...
1222 // We don't want to send out hundreds of terse updates over and over again
1223 // so lets throttle them and send them again after it's somewhat sorted out.
1224 p2.ThrottleUpdates = true;
1225 }
1226 //m_log.Debug(count.ToString());
1227 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1228 } // end for i.. loop
1229 } // end near
1230
1231 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1232 {
1233 bool result = false;
1234 //return result;
1235 if (!m_filterCollisions)
1236 return false;
1237
1238 ActorTypes at = (ActorTypes)atype;
1239 lock (_perloopContact)
1240 {
1241 foreach (d.ContactGeom contact in _perloopContact)
1242 {
1243 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1244 //{
1245 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1246 if (at == ActorTypes.Agent)
1247 {
1248 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)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
1249 {
1250
1251 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1252 {
1253 //contactGeom.depth *= .00005f;
1254 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1255 // 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));
1256 result = true;
1257 break;
1258 }
1259 else
1260 {
1261 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1262 }
1263 }
1264 else
1265 {
1266 //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));
1267 //int i = 0;
1268 }
1269 }
1270 else if (at == ActorTypes.Prim)
1271 {
1272 //d.AABB aabb1 = new d.AABB();
1273 //d.AABB aabb2 = new d.AABB();
1274
1275 //d.GeomGetAABB(contactGeom.g2, out aabb2);
1276 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1277 //aabb1.
1278 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)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
1279 {
1280 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1281 {
1282 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1283 {
1284 result = true;
1285 break;
1286 }
1287 }
1288 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1289 //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));
1290 }
1291
1292 }
1293
1294 //}
1295
1296 }
1297 }
1298 return result;
1299 }
1300
1301 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1302 {
1303 // obj1LocalID = 0;
1304 //returncollisions = false;
1305 obj2LocalID = 0;
1306 //ctype = 0;
1307 //cStartStop = 0;
1308 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1309 return;
1310
1311 switch ((ActorTypes)p2.PhysicsActorType)
1312 {
1313 case ActorTypes.Agent:
1314 cc2 = (OdeCharacter)p2;
1315
1316 // obj1LocalID = cc2.m_localID;
1317 switch ((ActorTypes)p1.PhysicsActorType)
1318 {
1319 case ActorTypes.Agent:
1320 cc1 = (OdeCharacter)p1;
1321 obj2LocalID = cc1.m_localID;
1322 cc1.AddCollisionEvent(cc2.m_localID, contact);
1323 //ctype = (int)CollisionCategories.Character;
1324
1325 //if (cc1.CollidingObj)
1326 //cStartStop = (int)StatusIndicators.Generic;
1327 //else
1328 //cStartStop = (int)StatusIndicators.Start;
1329
1330 //returncollisions = true;
1331 break;
1332 case ActorTypes.Prim:
1333 if (p1 is OdePrim)
1334 {
1335 cp1 = (OdePrim) p1;
1336 obj2LocalID = cp1.m_localID;
1337 cp1.AddCollisionEvent(cc2.m_localID, contact);
1338 }
1339 //ctype = (int)CollisionCategories.Geom;
1340
1341 //if (cp1.CollidingObj)
1342 //cStartStop = (int)StatusIndicators.Generic;
1343 //else
1344 //cStartStop = (int)StatusIndicators.Start;
1345
1346 //returncollisions = true;
1347 break;
1348
1349 case ActorTypes.Ground:
1350 case ActorTypes.Unknown:
1351 obj2LocalID = 0;
1352 //ctype = (int)CollisionCategories.Land;
1353 //returncollisions = true;
1354 break;
1355 }
1356
1357 cc2.AddCollisionEvent(obj2LocalID, contact);
1358 break;
1359 case ActorTypes.Prim:
1360
1361 if (p2 is OdePrim)
1362 {
1363 cp2 = (OdePrim) p2;
1364
1365 // obj1LocalID = cp2.m_localID;
1366 switch ((ActorTypes) p1.PhysicsActorType)
1367 {
1368 case ActorTypes.Agent:
1369 if (p1 is OdeCharacter)
1370 {
1371 cc1 = (OdeCharacter) p1;
1372 obj2LocalID = cc1.m_localID;
1373 cc1.AddCollisionEvent(cp2.m_localID, contact);
1374 //ctype = (int)CollisionCategories.Character;
1375
1376 //if (cc1.CollidingObj)
1377 //cStartStop = (int)StatusIndicators.Generic;
1378 //else
1379 //cStartStop = (int)StatusIndicators.Start;
1380 //returncollisions = true;
1381 }
1382 break;
1383 case ActorTypes.Prim:
1384
1385 if (p1 is OdePrim)
1386 {
1387 cp1 = (OdePrim) p1;
1388 obj2LocalID = cp1.m_localID;
1389 cp1.AddCollisionEvent(cp2.m_localID, contact);
1390 //ctype = (int)CollisionCategories.Geom;
1391
1392 //if (cp1.CollidingObj)
1393 //cStartStop = (int)StatusIndicators.Generic;
1394 //else
1395 //cStartStop = (int)StatusIndicators.Start;
1396
1397 //returncollisions = true;
1398 }
1399 break;
1400
1401 case ActorTypes.Ground:
1402 case ActorTypes.Unknown:
1403 obj2LocalID = 0;
1404 //ctype = (int)CollisionCategories.Land;
1405
1406 //returncollisions = true;
1407 break;
1408 }
1409
1410 cp2.AddCollisionEvent(obj2LocalID, contact);
1411 }
1412 break;
1413 }
1414 //if (returncollisions)
1415 //{
1416
1417 //lock (m_storedCollisions)
1418 //{
1419 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1420 //if (m_storedCollisions.ContainsKey(cDictKey))
1421 //{
1422 //sCollisionData objd = m_storedCollisions[cDictKey];
1423 //objd.NumberOfCollisions += 1;
1424 //objd.lastframe = framecount;
1425 //m_storedCollisions[cDictKey] = objd;
1426 //}
1427 //else
1428 //{
1429 //sCollisionData objd = new sCollisionData();
1430 //objd.ColliderLocalId = obj1LocalID;
1431 //objd.CollidedWithLocalId = obj2LocalID;
1432 //objd.CollisionType = ctype;
1433 //objd.NumberOfCollisions = 1;
1434 //objd.lastframe = framecount;
1435 //objd.StatusIndicator = cStartStop;
1436 //m_storedCollisions.Add(cDictKey, objd);
1437 //}
1438 //}
1439 // }
1440 }
1441
1442 public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1443 {
1444 /* String name1 = null;
1445 String name2 = null;
1446
1447 if (!geom_name_map.TryGetValue(trimesh, out name1))
1448 {
1449 name1 = "null";
1450 }
1451 if (!geom_name_map.TryGetValue(refObject, out name2))
1452 {
1453 name2 = "null";
1454 }
1455
1456 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
1457 */
1458 return 1;
1459 }
1460
1461 public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1462 {
1463 String name1 = null;
1464 String name2 = null;
1465
1466 if (!geom_name_map.TryGetValue(trimesh, out name1))
1467 {
1468 name1 = "null";
1469 }
1470
1471 if (!geom_name_map.TryGetValue(refObject, out name2))
1472 {
1473 name2 = "null";
1474 }
1475
1476 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1477
1478 d.Vector3 v0 = new d.Vector3();
1479 d.Vector3 v1 = new d.Vector3();
1480 d.Vector3 v2 = new d.Vector3();
1481
1482 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1483 // 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);
1484
1485 return 1;
1486 }
1487
1488 /// <summary>
1489 /// This is our collision testing routine in ODE
1490 /// </summary>
1491 /// <param name="timeStep"></param>
1492 private void collision_optimized(float timeStep)
1493 {
1494 _perloopContact.Clear();
1495
1496 lock (_characters)
1497 {
1498 foreach (OdeCharacter chr in _characters)
1499 {
1500 // Reset the collision values to false
1501 // since we don't know if we're colliding yet
1502
1503 // For some reason this can happen. Don't ask...
1504 //
1505 if (chr == null)
1506 continue;
1507
1508 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1509 continue;
1510
1511 chr.IsColliding = false;
1512 chr.CollidingGround = false;
1513 chr.CollidingObj = false;
1514
1515 // test the avatar's geometry for collision with the space
1516 // This will return near and the space that they are the closest to
1517 // And we'll run this again against the avatar and the space segment
1518 // This will return with a bunch of possible objects in the space segment
1519 // and we'll run it again on all of them.
1520 try
1521 {
1522 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
1523 }
1524 catch (AccessViolationException)
1525 {
1526 m_log.Warn("[PHYSICS]: Unable to space collide");
1527 }
1528 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1529 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1530 //{
1531 //chr.Position.Z = terrainheight + 10.0f;
1532 //forcedZ = true;
1533 //}
1534 }
1535 }
1536
1537 lock (_activeprims)
1538 {
1539 List<OdePrim> removeprims = null;
1540 foreach (OdePrim chr in _activeprims)
1541 {
1542 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled) && !chr.m_outofBounds)
1543 {
1544 try
1545 {
1546 lock (chr)
1547 {
1548 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1549 {
1550 d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
1551 }
1552 else
1553 {
1554 if (removeprims == null)
1555 {
1556 removeprims = new List<OdePrim>();
1557 }
1558 removeprims.Add(chr);
1559 /// Commented this because it triggers on every bullet
1560 //m_log.Debug("[PHYSICS]: 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!");
1561 }
1562 }
1563 }
1564 catch (AccessViolationException)
1565 {
1566 m_log.Warn("[PHYSICS]: Unable to space collide");
1567 }
1568 }
1569 }
1570 if (removeprims != null)
1571 {
1572 foreach (OdePrim chr in removeprims)
1573 {
1574 _activeprims.Remove(chr);
1575 }
1576 }
1577 }
1578
1579 _perloopContact.Clear();
1580 }
1581
1582 #endregion
1583
1584 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1585 {
1586 m_worldOffset = offset;
1587 WorldExtents = new Vector2(extents.X, extents.Y);
1588 m_parentScene = pScene;
1589
1590 }
1591
1592 // Recovered for use by fly height. Kitto Flora
1593 public float GetTerrainHeightAtXY(float x, float y)
1594 {
1595
1596 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1597 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1598
1599 IntPtr heightFieldGeom = IntPtr.Zero;
1600
1601 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1602 {
1603 if (heightFieldGeom != IntPtr.Zero)
1604 {
1605 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1606 {
1607
1608 int index;
1609
1610
1611 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1612 (int)x < 0.001f || (int)y < 0.001f)
1613 return 0;
1614
1615 x = x - offsetX;
1616 y = y - offsetY;
1617
1618 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1619
1620 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1621 {
1622 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1623 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1624 }
1625
1626 else
1627 return 0f;
1628 }
1629 else
1630 {
1631 return 0f;
1632 }
1633
1634 }
1635 else
1636 {
1637 return 0f;
1638 }
1639
1640 }
1641 else
1642 {
1643 return 0f;
1644 }
1645
1646
1647 }
1648// End recovered. Kitto Flora
1649
1650 public void addCollisionEventReporting(PhysicsActor obj)
1651 {
1652 lock (_collisionEventPrim)
1653 {
1654 if (!_collisionEventPrim.Contains(obj))
1655 _collisionEventPrim.Add(obj);
1656 }
1657 }
1658
1659 public void remCollisionEventReporting(PhysicsActor obj)
1660 {
1661 lock (_collisionEventPrim)
1662 {
1663 if (!_collisionEventPrim.Contains(obj))
1664 _collisionEventPrim.Remove(obj);
1665 }
1666 }
1667
1668 #region Add/Remove Entities
1669
1670 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1671 {
1672 Vector3 pos;
1673 pos.X = position.X;
1674 pos.Y = position.Y;
1675 pos.Z = position.Z;
1676 OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
1677 newAv.Flying = isFlying;
1678 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1679
1680 return newAv;
1681 }
1682
1683 public void AddCharacter(OdeCharacter chr)
1684 {
1685 lock (_characters)
1686 {
1687 if (!_characters.Contains(chr))
1688 {
1689 _characters.Add(chr);
1690 if (chr.bad)
1691 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1692 }
1693 }
1694 }
1695
1696 public void RemoveCharacter(OdeCharacter chr)
1697 {
1698 lock (_characters)
1699 {
1700 if (_characters.Contains(chr))
1701 {
1702 _characters.Remove(chr);
1703 }
1704 }
1705 }
1706 public void BadCharacter(OdeCharacter chr)
1707 {
1708 lock (_badCharacter)
1709 {
1710 if (!_badCharacter.Contains(chr))
1711 _badCharacter.Add(chr);
1712 }
1713 }
1714
1715 public override void RemoveAvatar(PhysicsActor actor)
1716 {
1717 //m_log.Debug("[PHYSICS]:ODELOCK");
1718 ((OdeCharacter) actor).Destroy();
1719
1720 }
1721
1722 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1723 IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, bool isphantom, byte shapetype, uint localid)
1724 {
1725
1726 Vector3 pos = position;
1727 Vector3 siz = size;
1728 Quaternion rot = rotation;
1729
1730 OdePrim newPrim;
1731 lock (OdeLock)
1732 {
1733 newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, isphantom, shapetype, ode, localid);
1734
1735 lock (_prims)
1736 _prims.Add(newPrim);
1737 }
1738
1739 return newPrim;
1740 }
1741
1742
1743 public void addActivePrim(OdePrim activatePrim)
1744 {
1745 // adds active prim.. (ones that should be iterated over in collisions_optimized
1746 lock (_activeprims)
1747 {
1748 if (!_activeprims.Contains(activatePrim))
1749 _activeprims.Add(activatePrim);
1750 //else
1751 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1752 }
1753 }
1754
1755 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1756 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1757 {
1758 PhysicsActor result;
1759 IMesh mesh = null;
1760
1761 if (needsMeshing(pbs))
1762 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1763
1764 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical,false,0, localid);
1765
1766 return result;
1767 }
1768
1769 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1770 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1771 {
1772 PhysicsActor result;
1773 IMesh mesh = null;
1774
1775 if (needsMeshing(pbs))
1776 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1777
1778 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom,0, localid);
1779
1780 return result;
1781 }
1782
1783 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1784 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
1785 {
1786 PhysicsActor result;
1787 IMesh mesh = null;
1788
1789 if (needsMeshing(pbs))
1790 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1791
1792 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom, shapetype, localid);
1793
1794 return result;
1795 }
1796
1797 public override float TimeDilation
1798 {
1799 get { return m_timeDilation; }
1800 }
1801
1802 public override bool SupportsNINJAJoints
1803 {
1804 get { return m_NINJA_physics_joints_enabled; }
1805 }
1806
1807 // internal utility function: must be called within a lock (OdeLock)
1808 private void InternalAddActiveJoint(PhysicsJoint joint)
1809 {
1810 activeJoints.Add(joint);
1811 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1812 }
1813
1814 // internal utility function: must be called within a lock (OdeLock)
1815 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1816 {
1817 pendingJoints.Add(joint);
1818 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1819 }
1820
1821 // internal utility function: must be called within a lock (OdeLock)
1822 private void InternalRemovePendingJoint(PhysicsJoint joint)
1823 {
1824 pendingJoints.Remove(joint);
1825 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1826 }
1827
1828 // internal utility function: must be called within a lock (OdeLock)
1829 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1830 {
1831 activeJoints.Remove(joint);
1832 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1833 }
1834
1835 public override void DumpJointInfo()
1836 {
1837 string hdr = "[NINJA] JOINTINFO: ";
1838 foreach (PhysicsJoint j in pendingJoints)
1839 {
1840 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1841 }
1842 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1843 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1844 {
1845 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1846 }
1847 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1848 foreach (PhysicsJoint j in activeJoints)
1849 {
1850 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1851 }
1852 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1853 foreach (string jointName in SOPName_to_activeJoint.Keys)
1854 {
1855 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1856 }
1857 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1858
1859 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1860 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1861 foreach (string actorName in joints_connecting_actor.Keys)
1862 {
1863 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1864 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1865 {
1866 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1867 }
1868 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1869 }
1870 }
1871
1872 public override void RequestJointDeletion(string ObjectNameInScene)
1873 {
1874 lock (externalJointRequestsLock)
1875 {
1876 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1877 {
1878 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1879 }
1880 }
1881 }
1882
1883 private void DeleteRequestedJoints()
1884 {
1885 List<string> myRequestedJointsToBeDeleted;
1886 lock (externalJointRequestsLock)
1887 {
1888 // make a local copy of the shared list for processing (threading issues)
1889 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1890 }
1891
1892 foreach (string jointName in myRequestedJointsToBeDeleted)
1893 {
1894 lock (OdeLock)
1895 {
1896 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1897 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1898 {
1899 OdePhysicsJoint joint = null;
1900 if (SOPName_to_activeJoint.ContainsKey(jointName))
1901 {
1902 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1903 InternalRemoveActiveJoint(joint);
1904 }
1905 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1906 {
1907 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1908 InternalRemovePendingJoint(joint);
1909 }
1910
1911 if (joint != null)
1912 {
1913 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1914 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1915 {
1916 string bodyName = joint.BodyNames[iBodyName];
1917 if (bodyName != "NULL")
1918 {
1919 joints_connecting_actor[bodyName].Remove(joint);
1920 if (joints_connecting_actor[bodyName].Count == 0)
1921 {
1922 joints_connecting_actor.Remove(bodyName);
1923 }
1924 }
1925 }
1926
1927 DoJointDeactivated(joint);
1928 if (joint.jointID != IntPtr.Zero)
1929 {
1930 d.JointDestroy(joint.jointID);
1931 joint.jointID = IntPtr.Zero;
1932 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1933 }
1934 else
1935 {
1936 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1937 }
1938 }
1939 else
1940 {
1941 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1942 }
1943 }
1944 else
1945 {
1946 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1947 }
1948 }
1949 }
1950
1951 // remove processed joints from the shared list
1952 lock (externalJointRequestsLock)
1953 {
1954 foreach (string jointName in myRequestedJointsToBeDeleted)
1955 {
1956 requestedJointsToBeDeleted.Remove(jointName);
1957 }
1958 }
1959 }
1960
1961 // for pending joints we don't know if their associated bodies exist yet or not.
1962 // the joint is actually created during processing of the taints
1963 private void CreateRequestedJoints()
1964 {
1965 List<PhysicsJoint> myRequestedJointsToBeCreated;
1966 lock (externalJointRequestsLock)
1967 {
1968 // make a local copy of the shared list for processing (threading issues)
1969 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1970 }
1971
1972 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1973 {
1974 lock (OdeLock)
1975 {
1976 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
1977 {
1978 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);
1979 continue;
1980 }
1981 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
1982 {
1983 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);
1984 continue;
1985 }
1986
1987 InternalAddPendingJoint(joint as OdePhysicsJoint);
1988
1989 if (joint.BodyNames.Count >= 2)
1990 {
1991 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1992 {
1993 string bodyName = joint.BodyNames[iBodyName];
1994 if (bodyName != "NULL")
1995 {
1996 if (!joints_connecting_actor.ContainsKey(bodyName))
1997 {
1998 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
1999 }
2000 joints_connecting_actor[bodyName].Add(joint);
2001 }
2002 }
2003 }
2004 }
2005 }
2006
2007 // remove processed joints from shared list
2008 lock (externalJointRequestsLock)
2009 {
2010 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2011 {
2012 requestedJointsToBeCreated.Remove(joint);
2013 }
2014 }
2015
2016 }
2017
2018 // public function to add an request for joint creation
2019 // this joint will just be added to a waiting list that is NOT processed during the main
2020 // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2021
2022 public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2023 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2024
2025 {
2026
2027 OdePhysicsJoint joint = new OdePhysicsJoint();
2028 joint.ObjectNameInScene = objectNameInScene;
2029 joint.Type = jointType;
2030 joint.Position = position;
2031 joint.Rotation = rotation;
2032 joint.RawParams = parms;
2033 joint.BodyNames = new List<string>(bodyNames);
2034 joint.TrackedBodyName = trackedBodyName;
2035 joint.LocalRotation = localRotation;
2036 joint.jointID = IntPtr.Zero;
2037 joint.ErrorMessageCount = 0;
2038
2039 lock (externalJointRequestsLock)
2040 {
2041 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2042 {
2043 requestedJointsToBeCreated.Add(joint);
2044 }
2045 }
2046 return joint;
2047 }
2048
2049 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2050 {
2051 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2052 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2053 {
2054
2055 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2056 //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)
2057 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2058 {
2059 jointsToRemove.Add(j);
2060 }
2061 foreach (PhysicsJoint j in jointsToRemove)
2062 {
2063 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2064 RequestJointDeletion(j.ObjectNameInScene);
2065 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2066 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)
2067 }
2068 }
2069 }
2070
2071 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2072 {
2073 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2074 lock (OdeLock)
2075 {
2076 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2077 RemoveAllJointsConnectedToActor(actor);
2078 }
2079 }
2080
2081 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2082 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2083 {
2084 Debug.Assert(joint.IsInPhysicsEngine);
2085 d.Vector3 pos = new d.Vector3();
2086
2087 if (!(joint is OdePhysicsJoint))
2088 {
2089 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2090 }
2091 else
2092 {
2093 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2094 switch (odeJoint.Type)
2095 {
2096 case PhysicsJointType.Ball:
2097 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2098 break;
2099 case PhysicsJointType.Hinge:
2100 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2101 break;
2102 }
2103 }
2104 return new Vector3(pos.X, pos.Y, pos.Z);
2105 }
2106
2107 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2108 // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2109 // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2110 // keeping track of the joint's original orientation relative to one of the involved bodies.
2111 public override Vector3 GetJointAxis(PhysicsJoint joint)
2112 {
2113 Debug.Assert(joint.IsInPhysicsEngine);
2114 d.Vector3 axis = new d.Vector3();
2115
2116 if (!(joint is OdePhysicsJoint))
2117 {
2118 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2119 }
2120 else
2121 {
2122 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2123 switch (odeJoint.Type)
2124 {
2125 case PhysicsJointType.Ball:
2126 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2127 break;
2128 case PhysicsJointType.Hinge:
2129 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2130 break;
2131 }
2132 }
2133 return new Vector3(axis.X, axis.Y, axis.Z);
2134 }
2135
2136
2137 public void remActivePrim(OdePrim deactivatePrim)
2138 {
2139 lock (_activeprims)
2140 {
2141 _activeprims.Remove(deactivatePrim);
2142 }
2143 }
2144
2145 public override void RemovePrim(PhysicsActor prim)
2146 {
2147 if (prim is OdePrim)
2148 {
2149 lock (OdeLock)
2150 {
2151 OdePrim p = (OdePrim) prim;
2152
2153 p.setPrimForRemoval();
2154 AddPhysicsActorTaint(prim);
2155 //RemovePrimThreadLocked(p);
2156 }
2157 }
2158 }
2159
2160 /// <summary>
2161 /// This is called from within simulate but outside the locked portion
2162 /// We need to do our own locking here
2163 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2164 ///
2165 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2166 /// that the space was using.
2167 /// </summary>
2168 /// <param name="prim"></param>
2169 public void RemovePrimThreadLocked(OdePrim prim)
2170 {
2171//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2172 lock (prim)
2173 {
2174 remCollisionEventReporting(prim);
2175 lock (ode)
2176 {
2177 if (prim.prim_geom != IntPtr.Zero)
2178 {
2179 prim.ResetTaints();
2180
2181 try
2182 {
2183 if (prim._triMeshData != IntPtr.Zero)
2184 {
2185 d.GeomTriMeshDataDestroy(prim._triMeshData);
2186 prim._triMeshData = IntPtr.Zero;
2187 }
2188 }
2189 catch { };
2190
2191 if (prim.IsPhysical)
2192 {
2193 prim.disableBody();
2194 if (prim.childPrim)
2195 {
2196 prim.childPrim = false;
2197 prim.Body = IntPtr.Zero;
2198 prim.m_disabled = true;
2199 prim.IsPhysical = false;
2200 }
2201
2202 }
2203 // we don't want to remove the main space
2204
2205 // If the geometry is in the targetspace, remove it from the target space
2206 //m_log.Warn(prim.m_targetSpace);
2207
2208 //if (prim.m_targetSpace != IntPtr.Zero)
2209 //{
2210 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2211 //{
2212
2213 //if (d.GeomIsSpace(prim.m_targetSpace))
2214 //{
2215 //waitForSpaceUnlock(prim.m_targetSpace);
2216 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2217 prim.m_targetSpace = IntPtr.Zero;
2218 //}
2219 //else
2220 //{
2221 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2222 //((OdePrim)prim).m_targetSpace.ToString());
2223 //}
2224
2225 //}
2226 //}
2227 //m_log.Warn(prim.prim_geom);
2228 try
2229 {
2230 if (prim.prim_geom != IntPtr.Zero)
2231 {
2232
2233//string tPA;
2234//geom_name_map.TryGetValue(prim.prim_geom, out tPA);
2235//Console.WriteLine("**** Remove {0}", tPA);
2236 if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom);
2237 if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom);
2238 d.GeomDestroy(prim.prim_geom);
2239 prim.prim_geom = IntPtr.Zero;
2240 }
2241 else
2242 {
2243 m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
2244 }
2245 }
2246 catch (AccessViolationException)
2247 {
2248 m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
2249 }
2250 lock (_prims)
2251 _prims.Remove(prim);
2252
2253 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2254 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2255 //{
2256 //if (prim.m_targetSpace != null)
2257 //{
2258 //if (d.GeomIsSpace(prim.m_targetSpace))
2259 //{
2260 //waitForSpaceUnlock(prim.m_targetSpace);
2261 //d.SpaceRemove(space, prim.m_targetSpace);
2262 // free up memory used by the space.
2263 //d.SpaceDestroy(prim.m_targetSpace);
2264 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2265 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2266 //}
2267 //else
2268 //{
2269 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2270 //((OdePrim) prim).m_targetSpace.ToString());
2271 //}
2272 //}
2273 //}
2274
2275 if (SupportsNINJAJoints)
2276 {
2277 RemoveAllJointsConnectedToActorThreadLocked(prim);
2278 }
2279 }
2280 }
2281 }
2282 }
2283
2284 #endregion
2285
2286 #region Space Separation Calculation
2287
2288 /// <summary>
2289 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2290 /// </summary>
2291 /// <param name="pSpace"></param>
2292 public void resetSpaceArrayItemToZero(IntPtr pSpace)
2293 {
2294 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2295 {
2296 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2297 {
2298 if (staticPrimspace[x, y] == pSpace)
2299 staticPrimspace[x, y] = IntPtr.Zero;
2300 }
2301 }
2302 }
2303
2304 public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2305 {
2306 staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2307 }
2308
2309 /// <summary>
2310 /// Called when a static prim moves. Allocates a space for the prim based on its position
2311 /// </summary>
2312 /// <param name="geom">the pointer to the geom that moved</param>
2313 /// <param name="pos">the position that the geom moved to</param>
2314 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2315 /// <returns>a pointer to the new space it's in</returns>
2316 public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2317 {
2318 // Called from setting the Position and Size of an ODEPrim so
2319 // it's already in locked space.
2320
2321 // we don't want to remove the main space
2322 // we don't need to test physical here because this function should
2323 // never be called if the prim is physical(active)
2324
2325 // All physical prim end up in the root space
2326 //Thread.Sleep(20);
2327 if (currentspace != space)
2328 {
2329 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2330 //if (currentspace == IntPtr.Zero)
2331 //{
2332 //int adfadf = 0;
2333 //}
2334 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2335 {
2336 if (d.GeomIsSpace(currentspace))
2337 {
2338 waitForSpaceUnlock(currentspace);
2339 d.SpaceRemove(currentspace, geom);
2340 }
2341 else
2342 {
2343 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2344 " Geom:" + geom);
2345 }
2346 }
2347 else
2348 {
2349 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2350 if (sGeomIsIn != IntPtr.Zero)
2351 {
2352 if (d.GeomIsSpace(currentspace))
2353 {
2354 waitForSpaceUnlock(sGeomIsIn);
2355 d.SpaceRemove(sGeomIsIn, geom);
2356 }
2357 else
2358 {
2359 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2360 sGeomIsIn + " Geom:" + geom);
2361 }
2362 }
2363 }
2364
2365 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2366 if (d.SpaceGetNumGeoms(currentspace) == 0)
2367 {
2368 if (currentspace != IntPtr.Zero)
2369 {
2370 if (d.GeomIsSpace(currentspace))
2371 {
2372 waitForSpaceUnlock(currentspace);
2373 waitForSpaceUnlock(space);
2374 d.SpaceRemove(space, currentspace);
2375 // free up memory used by the space.
2376
2377 //d.SpaceDestroy(currentspace);
2378 resetSpaceArrayItemToZero(currentspace);
2379 }
2380 else
2381 {
2382 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2383 currentspace + " Geom:" + geom);
2384 }
2385 }
2386 }
2387 }
2388 else
2389 {
2390 // this is a physical object that got disabled. ;.;
2391 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2392 {
2393 if (d.SpaceQuery(currentspace, geom))
2394 {
2395 if (d.GeomIsSpace(currentspace))
2396 {
2397 waitForSpaceUnlock(currentspace);
2398 d.SpaceRemove(currentspace, geom);
2399 }
2400 else
2401 {
2402 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2403 currentspace + " Geom:" + geom);
2404 }
2405 }
2406 else
2407 {
2408 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2409 if (sGeomIsIn != IntPtr.Zero)
2410 {
2411 if (d.GeomIsSpace(sGeomIsIn))
2412 {
2413 waitForSpaceUnlock(sGeomIsIn);
2414 d.SpaceRemove(sGeomIsIn, geom);
2415 }
2416 else
2417 {
2418 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2419 sGeomIsIn + " Geom:" + geom);
2420 }
2421 }
2422 }
2423 }
2424 }
2425
2426 // The routines in the Position and Size sections do the 'inserting' into the space,
2427 // so all we have to do is make sure that the space that we're putting the prim into
2428 // is in the 'main' space.
2429 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2430 IntPtr newspace = calculateSpaceForGeom(pos);
2431
2432 if (newspace == IntPtr.Zero)
2433 {
2434 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2435 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2436 }
2437
2438 return newspace;
2439 }
2440
2441 /// <summary>
2442 /// Creates a new space at X Y
2443 /// </summary>
2444 /// <param name="iprimspaceArrItemX"></param>
2445 /// <param name="iprimspaceArrItemY"></param>
2446 /// <returns>A pointer to the created space</returns>
2447 public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2448 {
2449 // creating a new space for prim and inserting it into main space.
2450 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2451 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2452 waitForSpaceUnlock(space);
2453 d.SpaceSetSublevel(space, 1);
2454 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2455 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2456 }
2457
2458 /// <summary>
2459 /// Calculates the space the prim should be in by its position
2460 /// </summary>
2461 /// <param name="pos"></param>
2462 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2463 public IntPtr calculateSpaceForGeom(Vector3 pos)
2464 {
2465 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2466 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2467 return staticPrimspace[xyspace[0], xyspace[1]];
2468 }
2469
2470 /// <summary>
2471 /// Holds the space allocation logic
2472 /// </summary>
2473 /// <param name="pos"></param>
2474 /// <returns>an array item based on the position</returns>
2475 public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2476 {
2477 int[] returnint = new int[2];
2478
2479 returnint[0] = (int) (pos.X/metersInSpace);
2480
2481 if (returnint[0] > ((int) (259f/metersInSpace)))
2482 returnint[0] = ((int) (259f/metersInSpace));
2483 if (returnint[0] < 0)
2484 returnint[0] = 0;
2485
2486 returnint[1] = (int) (pos.Y/metersInSpace);
2487 if (returnint[1] > ((int) (259f/metersInSpace)))
2488 returnint[1] = ((int) (259f/metersInSpace));
2489 if (returnint[1] < 0)
2490 returnint[1] = 0;
2491
2492 return returnint;
2493 }
2494
2495 #endregion
2496
2497 /// <summary>
2498 /// Routine to figure out if we need to mesh this prim with our mesher
2499 /// </summary>
2500 /// <param name="pbs"></param>
2501 /// <returns></returns>
2502 public bool needsMeshing(PrimitiveBaseShape pbs)
2503 {
2504 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2505 // but we still need to check for sculptie meshing being enabled so this is the most
2506 // convenient place to do it for now...
2507
2508 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2509 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2510 int iPropertiesNotSupportedDefault = 0;
2511
2512 if (pbs.SculptEntry && !meshSculptedPrim)
2513 {
2514#if SPAM
2515 m_log.Warn("NonMesh");
2516#endif
2517 return false;
2518 }
2519
2520 // 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
2521 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2522 {
2523 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2524 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2525 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2526 {
2527
2528 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2529 && pbs.ProfileHollow == 0
2530 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2531 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2532 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2533 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2534 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2535 {
2536#if SPAM
2537 m_log.Warn("NonMesh");
2538#endif
2539 return false;
2540 }
2541 }
2542 }
2543
2544 if (forceSimplePrimMeshing)
2545 return true;
2546
2547 if (pbs.ProfileHollow != 0)
2548 iPropertiesNotSupportedDefault++;
2549
2550 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2551 iPropertiesNotSupportedDefault++;
2552
2553 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2554 iPropertiesNotSupportedDefault++;
2555
2556 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2557 iPropertiesNotSupportedDefault++;
2558
2559 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2560 iPropertiesNotSupportedDefault++;
2561
2562 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2563 iPropertiesNotSupportedDefault++;
2564
2565 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))
2566 iPropertiesNotSupportedDefault++;
2567
2568 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2569 iPropertiesNotSupportedDefault++;
2570
2571 // test for torus
2572 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2573 {
2574 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2575 {
2576 iPropertiesNotSupportedDefault++;
2577 }
2578 }
2579 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2580 {
2581 if (pbs.PathCurve == (byte)Extrusion.Straight)
2582 {
2583 iPropertiesNotSupportedDefault++;
2584 }
2585
2586 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2587 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2588 {
2589 iPropertiesNotSupportedDefault++;
2590 }
2591 }
2592 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2593 {
2594 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2595 {
2596 iPropertiesNotSupportedDefault++;
2597 }
2598 }
2599 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2600 {
2601 if (pbs.PathCurve == (byte)Extrusion.Straight)
2602 {
2603 iPropertiesNotSupportedDefault++;
2604 }
2605 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2606 {
2607 iPropertiesNotSupportedDefault++;
2608 }
2609 }
2610
2611 if (pbs.SculptEntry && meshSculptedPrim)
2612 iPropertiesNotSupportedDefault++;
2613
2614 if (iPropertiesNotSupportedDefault == 0)
2615 {
2616#if SPAM
2617 m_log.Warn("NonMesh");
2618#endif
2619 return false;
2620 }
2621#if SPAM
2622 m_log.Debug("Mesh");
2623#endif
2624 return true;
2625 }
2626
2627 /// <summary>
2628 /// Called after our prim properties are set Scale, position etc.
2629 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2630 /// This assures us that we have no race conditions
2631 /// </summary>
2632 /// <param name="prim"></param>
2633 public override void AddPhysicsActorTaint(PhysicsActor prim)
2634 {
2635
2636 if (prim is OdePrim)
2637 {
2638 OdePrim taintedprim = ((OdePrim) prim);
2639 lock (_taintedPrimLock)
2640 {
2641 if (!(_taintedPrimH.Contains(taintedprim)))
2642 {
2643//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2644 _taintedPrimH.Add(taintedprim); // HashSet for searching
2645 _taintedPrimL.Add(taintedprim); // List for ordered readout
2646 }
2647 }
2648 return;
2649 }
2650 else if (prim is OdeCharacter)
2651 {
2652 OdeCharacter taintedchar = ((OdeCharacter)prim);
2653 lock (_taintedActors)
2654 {
2655 if (!(_taintedActors.Contains(taintedchar)))
2656 {
2657 _taintedActors.Add(taintedchar);
2658 if (taintedchar.bad)
2659 m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2660 }
2661 }
2662 }
2663 }
2664
2665 /// <summary>
2666 /// This is our main simulate loop
2667 /// It's thread locked by a Mutex in the scene.
2668 /// It holds Collisions, it instructs ODE to step through the physical reactions
2669 /// It moves the objects around in memory
2670 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2671 /// </summary>
2672 /// <param name="timeStep"></param>
2673 /// <returns></returns>
2674 public override float Simulate(float timeStep)
2675 {
2676 if (framecount >= int.MaxValue)
2677 framecount = 0;
2678 //if (m_worldOffset != Vector3.Zero)
2679 // return 0;
2680
2681 framecount++;
2682
2683 DateTime now = DateTime.UtcNow;
2684 TimeSpan SinceLastFrame = now - m_lastframe;
2685 m_lastframe = now;
2686 float realtime = (float)SinceLastFrame.TotalSeconds;
2687// Console.WriteLine("ts={0} rt={1}", timeStep, realtime);
2688 timeStep = realtime;
2689
2690 // float fps = 1.0f / realtime;
2691 float fps = 0.0f; // number of ODE steps in this Simulate step
2692 //m_log.Info(timeStep.ToString());
2693 step_time += timeStep;
2694
2695 // If We're loaded down by something else,
2696 // or debugging with the Visual Studio project on pause
2697 // skip a few frames to catch up gracefully.
2698 // without shooting the physicsactors all over the place
2699
2700 if (step_time >= m_SkipFramesAtms)
2701 {
2702 // Instead of trying to catch up, it'll do 5 physics frames only
2703 step_time = ODE_STEPSIZE;
2704 m_physicsiterations = 5;
2705 }
2706 else
2707 {
2708 m_physicsiterations = 10;
2709 }
2710
2711 if (SupportsNINJAJoints)
2712 {
2713 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2714 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2715 }
2716
2717 lock (OdeLock)
2718 {
2719 // Process 10 frames if the sim is running normal..
2720 // process 5 frames if the sim is running slow
2721 //try
2722 //{
2723 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2724 //}
2725 //catch (StackOverflowException)
2726 //{
2727 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2728 // ode.drelease(world);
2729 //base.TriggerPhysicsBasedRestart();
2730 //}
2731
2732 int i = 0;
2733
2734 // Figure out the Frames Per Second we're going at.
2735 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
2736
2737 // fps = (step_time / ODE_STEPSIZE) * 1000;
2738 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
2739 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
2740
2741 // step_time = 0.09375f;
2742
2743 while (step_time > 0.0f)
2744 {
2745 //lock (ode)
2746 //{
2747 //if (!ode.lockquery())
2748 //{
2749 // ode.dlock(world);
2750 try
2751 {
2752 // Insert, remove Characters
2753 bool processedtaints = false;
2754
2755 lock (_taintedActors)
2756 {
2757 if (_taintedActors.Count > 0)
2758 {
2759 foreach (OdeCharacter character in _taintedActors)
2760 {
2761
2762 character.ProcessTaints(ODE_STEPSIZE);
2763
2764 processedtaints = true;
2765 //character.m_collisionscore = 0;
2766 }
2767
2768 if (processedtaints)
2769 _taintedActors.Clear();
2770 }
2771 } // end lock _taintedActors
2772
2773 // Modify other objects in the scene.
2774 processedtaints = false;
2775
2776 lock (_taintedPrimLock)
2777 {
2778 foreach (OdePrim prim in _taintedPrimL)
2779 {
2780 if (prim.m_taintremove)
2781 {
2782 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2783 RemovePrimThreadLocked(prim);
2784 }
2785 else
2786 {
2787 //Console.WriteLine("Simulate calls ProcessTaints");
2788 prim.ProcessTaints(ODE_STEPSIZE);
2789 }
2790 processedtaints = true;
2791 prim.m_collisionscore = 0;
2792
2793 // This loop can block up the Heartbeat for a very long time on large regions.
2794 // We need to let the Watchdog know that the Heartbeat is not dead
2795 // NOTE: This is currently commented out, but if things like OAR loading are
2796 // timing the heartbeat out we will need to uncomment it
2797 //Watchdog.UpdateThread();
2798 }
2799
2800 if (SupportsNINJAJoints)
2801 {
2802 // Create pending joints, if possible
2803
2804 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2805 // a joint requires specifying the body id of both involved bodies
2806 if (pendingJoints.Count > 0)
2807 {
2808 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2809 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2810 foreach (PhysicsJoint joint in pendingJoints)
2811 {
2812 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2813 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(),
2814 System.StringSplitOptions.RemoveEmptyEntries);
2815 List<IntPtr> jointBodies = new List<IntPtr>();
2816 bool allJointBodiesAreReady = true;
2817 foreach (string jointParam in jointParams)
2818 {
2819 if (jointParam == "NULL")
2820 {
2821 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2822 jointBodies.Add(IntPtr.Zero);
2823 }
2824 else
2825 {
2826 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2827 bool foundPrim = false;
2828 lock (_prims)
2829 {
2830 foreach (OdePrim prim in _prims) // FIXME: inefficient
2831 {
2832 if (prim.SOPName == jointParam)
2833 {
2834 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2835 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2836 {
2837 jointBodies.Add(prim.Body);
2838 foundPrim = true;
2839 break;
2840 }
2841 else
2842 {
2843 DoJointErrorMessage(joint, "prim name " + jointParam +
2844 " exists but is not (yet) physical; deferring joint creation. " +
2845 "IsPhysical property is " + prim.IsPhysical +
2846 " and body is " + prim.Body);
2847 foundPrim = false;
2848 break;
2849 }
2850 }
2851 }
2852 }
2853 if (foundPrim)
2854 {
2855 // all is fine
2856 }
2857 else
2858 {
2859 allJointBodiesAreReady = false;
2860 break;
2861 }
2862 }
2863 }
2864 if (allJointBodiesAreReady)
2865 {
2866 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2867 if (jointBodies[0] == jointBodies[1])
2868 {
2869 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2870 }
2871 else
2872 {
2873 switch (joint.Type)
2874 {
2875 case PhysicsJointType.Ball:
2876 {
2877 IntPtr odeJoint;
2878 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2879 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2880 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2881 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2882 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2883 d.JointSetBallAnchor(odeJoint,
2884 joint.Position.X,
2885 joint.Position.Y,
2886 joint.Position.Z);
2887 //DoJointErrorMessage(joint, "ODE joint setting OK");
2888 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2889 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2890 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2891 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2892
2893 if (joint is OdePhysicsJoint)
2894 {
2895 ((OdePhysicsJoint)joint).jointID = odeJoint;
2896 }
2897 else
2898 {
2899 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2900 }
2901 }
2902 break;
2903 case PhysicsJointType.Hinge:
2904 {
2905 IntPtr odeJoint;
2906 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2907 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2908 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2909 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2910 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2911 d.JointSetHingeAnchor(odeJoint,
2912 joint.Position.X,
2913 joint.Position.Y,
2914 joint.Position.Z);
2915 // We use the orientation of the x-axis of the joint's coordinate frame
2916 // as the axis for the hinge.
2917
2918 // Therefore, we must get the joint's coordinate frame based on the
2919 // joint.Rotation field, which originates from the orientation of the
2920 // joint's proxy object in the scene.
2921
2922 // The joint's coordinate frame is defined as the transformation matrix
2923 // that converts a vector from joint-local coordinates into world coordinates.
2924 // World coordinates are defined as the XYZ coordinate system of the sim,
2925 // as shown in the top status-bar of the viewer.
2926
2927 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2928 // and use that as the hinge axis.
2929
2930 //joint.Rotation.Normalize();
2931 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2932
2933 // Now extract the X axis of the joint's coordinate frame.
2934
2935 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2936 // tar pit of transposed, inverted, and generally messed-up orientations.
2937 // (In other words, Matrix4.AtAxis() is borked.)
2938 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2939
2940 // Instead, compute the X axis of the coordinate frame by transforming
2941 // the (1,0,0) vector. At least that works.
2942
2943 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2944 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2945 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2946 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2947 d.JointSetHingeAxis(odeJoint,
2948 jointAxis.X,
2949 jointAxis.Y,
2950 jointAxis.Z);
2951 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2952 if (joint is OdePhysicsJoint)
2953 {
2954 ((OdePhysicsJoint)joint).jointID = odeJoint;
2955 }
2956 else
2957 {
2958 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2959 }
2960 }
2961 break;
2962 }
2963 successfullyProcessedPendingJoints.Add(joint);
2964 }
2965 }
2966 else
2967 {
2968 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2969 }
2970 }
2971 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2972 {
2973 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2974 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2975 InternalRemovePendingJoint(successfullyProcessedJoint);
2976 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2977 InternalAddActiveJoint(successfullyProcessedJoint);
2978 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2979 }
2980 }
2981 } // end SupportsNINJAJoints
2982
2983 if (processedtaints)
2984//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2985 _taintedPrimH.Clear(); // ??? if this only ???
2986 _taintedPrimL.Clear();
2987 } // end lock _taintedPrimLock
2988
2989 // Move characters
2990 lock (_characters)
2991 {
2992 List<OdeCharacter> defects = new List<OdeCharacter>();
2993 foreach (OdeCharacter actor in _characters)
2994 {
2995 if (actor != null)
2996 actor.Move(ODE_STEPSIZE, defects);
2997 }
2998 if (0 != defects.Count)
2999 {
3000 foreach (OdeCharacter defect in defects)
3001 {
3002 RemoveCharacter(defect);
3003 }
3004 }
3005 } // end lock _characters
3006
3007 // Move other active objects
3008 lock (_activeprims)
3009 {
3010 foreach (OdePrim prim in _activeprims)
3011 {
3012 prim.m_collisionscore = 0;
3013 prim.Move(ODE_STEPSIZE);
3014 }
3015 } // end lock _activeprims
3016
3017 //if ((framecount % m_randomizeWater) == 0)
3018 // randomizeWater(waterlevel);
3019
3020 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
3021 m_rayCastManager.ProcessQueuedRequests();
3022
3023 collision_optimized(ODE_STEPSIZE);
3024
3025 lock (_collisionEventPrim)
3026 {
3027 foreach (PhysicsActor obj in _collisionEventPrim)
3028 {
3029 if (obj == null)
3030 continue;
3031
3032 switch ((ActorTypes)obj.PhysicsActorType)
3033 {
3034 case ActorTypes.Agent:
3035 OdeCharacter cobj = (OdeCharacter)obj;
3036 cobj.AddCollisionFrameTime(100);
3037 cobj.SendCollisions();
3038 break;
3039 case ActorTypes.Prim:
3040 OdePrim pobj = (OdePrim)obj;
3041 pobj.SendCollisions();
3042 break;
3043 }
3044 }
3045 } // end lock _collisionEventPrim
3046
3047 //if (m_global_contactcount > 5)
3048 //{
3049 // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
3050 //}
3051
3052 m_global_contactcount = 0;
3053
3054 d.WorldQuickStep(world, ODE_STEPSIZE);
3055 d.JointGroupEmpty(contactgroup);
3056 fps++;
3057 //ode.dunlock(world);
3058 } // end try
3059 catch (Exception e)
3060 {
3061 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3062 ode.dunlock(world);
3063 }
3064
3065 step_time -= ODE_STEPSIZE;
3066 i++;
3067 //}
3068 //else
3069 //{
3070 //fps = 0;
3071 //}
3072 //}
3073 } // end while (step_time > 0.0f)
3074
3075 lock (_characters)
3076 {
3077 foreach (OdeCharacter actor in _characters)
3078 {
3079 if (actor != null)
3080 {
3081 if (actor.bad)
3082 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3083 actor.UpdatePositionAndVelocity();
3084 }
3085 }
3086 }
3087
3088 lock (_badCharacter)
3089 {
3090 if (_badCharacter.Count > 0)
3091 {
3092 foreach (OdeCharacter chr in _badCharacter)
3093 {
3094 RemoveCharacter(chr);
3095 }
3096 _badCharacter.Clear();
3097 }
3098 }
3099
3100 lock (_activeprims)
3101 {
3102 //if (timeStep < 0.2f)
3103 {
3104 foreach (OdePrim actor in _activeprims)
3105 {
3106 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
3107 {
3108 actor.UpdatePositionAndVelocity();
3109
3110 if (SupportsNINJAJoints)
3111 {
3112 // If an actor moved, move its joint proxy objects as well.
3113 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3114 // for this purpose but it is never called! So we just do the joint
3115 // movement code here.
3116
3117 if (actor.SOPName != null &&
3118 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3119 joints_connecting_actor[actor.SOPName] != null &&
3120 joints_connecting_actor[actor.SOPName].Count > 0)
3121 {
3122 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3123 {
3124 if (affectedJoint.IsInPhysicsEngine)
3125 {
3126 DoJointMoved(affectedJoint);
3127 }
3128 else
3129 {
3130 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);
3131 }
3132 }
3133 }
3134 }
3135 }
3136 }
3137 }
3138 } // end lock _activeprims
3139
3140 //DumpJointInfo();
3141
3142 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3143 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3144 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3145 if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
3146 {
3147 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3148 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3149
3150 if (physics_logging_append_existing_logfile)
3151 {
3152 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3153 TextWriter fwriter = File.AppendText(fname);
3154 fwriter.WriteLine(header);
3155 fwriter.Close();
3156 }
3157 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3158 }
3159 } // end lock OdeLock
3160
3161 return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
3162 } // end Simulate
3163
3164 public override void GetResults()
3165 {
3166 }
3167
3168 public override bool IsThreaded
3169 {
3170 // for now we won't be multithreaded
3171 get { return (false); }
3172 }
3173
3174 #region ODE Specific Terrain Fixes
3175 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3176 {
3177 float[] returnarr = new float[262144];
3178 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3179
3180 // Filling out the array into its multi-dimensional components
3181 for (int y = 0; y < WorldExtents.Y; y++)
3182 {
3183 for (int x = 0; x < WorldExtents.X; x++)
3184 {
3185 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3186 }
3187 }
3188
3189 // Resize using Nearest Neighbour
3190
3191 // This particular way is quick but it only works on a multiple of the original
3192
3193 // The idea behind this method can be described with the following diagrams
3194 // second pass and third pass happen in the same loop really.. just separated
3195 // them to show what this does.
3196
3197 // First Pass
3198 // ResultArr:
3199 // 1,1,1,1,1,1
3200 // 1,1,1,1,1,1
3201 // 1,1,1,1,1,1
3202 // 1,1,1,1,1,1
3203 // 1,1,1,1,1,1
3204 // 1,1,1,1,1,1
3205
3206 // Second Pass
3207 // ResultArr2:
3208 // 1,,1,,1,,1,,1,,1,
3209 // ,,,,,,,,,,
3210 // 1,,1,,1,,1,,1,,1,
3211 // ,,,,,,,,,,
3212 // 1,,1,,1,,1,,1,,1,
3213 // ,,,,,,,,,,
3214 // 1,,1,,1,,1,,1,,1,
3215 // ,,,,,,,,,,
3216 // 1,,1,,1,,1,,1,,1,
3217 // ,,,,,,,,,,
3218 // 1,,1,,1,,1,,1,,1,
3219
3220 // Third pass fills in the blanks
3221 // ResultArr2:
3222 // 1,1,1,1,1,1,1,1,1,1,1,1
3223 // 1,1,1,1,1,1,1,1,1,1,1,1
3224 // 1,1,1,1,1,1,1,1,1,1,1,1
3225 // 1,1,1,1,1,1,1,1,1,1,1,1
3226 // 1,1,1,1,1,1,1,1,1,1,1,1
3227 // 1,1,1,1,1,1,1,1,1,1,1,1
3228 // 1,1,1,1,1,1,1,1,1,1,1,1
3229 // 1,1,1,1,1,1,1,1,1,1,1,1
3230 // 1,1,1,1,1,1,1,1,1,1,1,1
3231 // 1,1,1,1,1,1,1,1,1,1,1,1
3232 // 1,1,1,1,1,1,1,1,1,1,1,1
3233
3234 // X,Y = .
3235 // X+1,y = ^
3236 // X,Y+1 = *
3237 // X+1,Y+1 = #
3238
3239 // Filling in like this;
3240 // .*
3241 // ^#
3242 // 1st .
3243 // 2nd *
3244 // 3rd ^
3245 // 4th #
3246 // on single loop.
3247
3248 float[,] resultarr2 = new float[512, 512];
3249 for (int y = 0; y < WorldExtents.Y; y++)
3250 {
3251 for (int x = 0; x < WorldExtents.X; x++)
3252 {
3253 resultarr2[y * 2, x * 2] = resultarr[y, x];
3254
3255 if (y < WorldExtents.Y)
3256 {
3257 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3258 }
3259 if (x < WorldExtents.X)
3260 {
3261 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3262 }
3263 if (x < WorldExtents.X && y < WorldExtents.Y)
3264 {
3265 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3266 }
3267 }
3268 }
3269
3270 //Flatten out the array
3271 int i = 0;
3272 for (int y = 0; y < 512; y++)
3273 {
3274 for (int x = 0; x < 512; x++)
3275 {
3276 if (resultarr2[y, x] <= 0)
3277 returnarr[i] = 0.0000001f;
3278 else
3279 returnarr[i] = resultarr2[y, x];
3280
3281 i++;
3282 }
3283 }
3284
3285 return returnarr;
3286 }
3287
3288 public float[] ResizeTerrain512Interpolation(float[] heightMap)
3289 {
3290 float[] returnarr = new float[262144];
3291 float[,] resultarr = new float[512,512];
3292
3293 // Filling out the array into its multi-dimensional components
3294 for (int y = 0; y < 256; y++)
3295 {
3296 for (int x = 0; x < 256; x++)
3297 {
3298 resultarr[y, x] = heightMap[y * 256 + x];
3299 }
3300 }
3301
3302 // Resize using interpolation
3303
3304 // This particular way is quick but it only works on a multiple of the original
3305
3306 // The idea behind this method can be described with the following diagrams
3307 // second pass and third pass happen in the same loop really.. just separated
3308 // them to show what this does.
3309
3310 // First Pass
3311 // ResultArr:
3312 // 1,1,1,1,1,1
3313 // 1,1,1,1,1,1
3314 // 1,1,1,1,1,1
3315 // 1,1,1,1,1,1
3316 // 1,1,1,1,1,1
3317 // 1,1,1,1,1,1
3318
3319 // Second Pass
3320 // ResultArr2:
3321 // 1,,1,,1,,1,,1,,1,
3322 // ,,,,,,,,,,
3323 // 1,,1,,1,,1,,1,,1,
3324 // ,,,,,,,,,,
3325 // 1,,1,,1,,1,,1,,1,
3326 // ,,,,,,,,,,
3327 // 1,,1,,1,,1,,1,,1,
3328 // ,,,,,,,,,,
3329 // 1,,1,,1,,1,,1,,1,
3330 // ,,,,,,,,,,
3331 // 1,,1,,1,,1,,1,,1,
3332
3333 // Third pass fills in the blanks
3334 // ResultArr2:
3335 // 1,1,1,1,1,1,1,1,1,1,1,1
3336 // 1,1,1,1,1,1,1,1,1,1,1,1
3337 // 1,1,1,1,1,1,1,1,1,1,1,1
3338 // 1,1,1,1,1,1,1,1,1,1,1,1
3339 // 1,1,1,1,1,1,1,1,1,1,1,1
3340 // 1,1,1,1,1,1,1,1,1,1,1,1
3341 // 1,1,1,1,1,1,1,1,1,1,1,1
3342 // 1,1,1,1,1,1,1,1,1,1,1,1
3343 // 1,1,1,1,1,1,1,1,1,1,1,1
3344 // 1,1,1,1,1,1,1,1,1,1,1,1
3345 // 1,1,1,1,1,1,1,1,1,1,1,1
3346
3347 // X,Y = .
3348 // X+1,y = ^
3349 // X,Y+1 = *
3350 // X+1,Y+1 = #
3351
3352 // Filling in like this;
3353 // .*
3354 // ^#
3355 // 1st .
3356 // 2nd *
3357 // 3rd ^
3358 // 4th #
3359 // on single loop.
3360
3361 float[,] resultarr2 = new float[512,512];
3362 for (int y = 0; y < (int)Constants.RegionSize; y++)
3363 {
3364 for (int x = 0; x < (int)Constants.RegionSize; x++)
3365 {
3366 resultarr2[y*2, x*2] = resultarr[y, x];
3367
3368 if (y < (int)Constants.RegionSize)
3369 {
3370 if (y + 1 < (int)Constants.RegionSize)
3371 {
3372 if (x + 1 < (int)Constants.RegionSize)
3373 {
3374 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3375 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3376 }
3377 else
3378 {
3379 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3380 }
3381 }
3382 else
3383 {
3384 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3385 }
3386 }
3387 if (x < (int)Constants.RegionSize)
3388 {
3389 if (x + 1 < (int)Constants.RegionSize)
3390 {
3391 if (y + 1 < (int)Constants.RegionSize)
3392 {
3393 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3394 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3395 }
3396 else
3397 {
3398 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3399 }
3400 }
3401 else
3402 {
3403 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3404 }
3405 }
3406 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3407 {
3408 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3409 {
3410 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3411 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3412 }
3413 else
3414 {
3415 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3416 }
3417 }
3418 }
3419 }
3420 //Flatten out the array
3421 int i = 0;
3422 for (int y = 0; y < 512; y++)
3423 {
3424 for (int x = 0; x < 512; x++)
3425 {
3426 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3427 {
3428 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
3429 resultarr2[y, x] = 0;
3430 }
3431 returnarr[i] = resultarr2[y, x];
3432 i++;
3433 }
3434 }
3435
3436 return returnarr;
3437 }
3438
3439 #endregion
3440
3441 public override void SetTerrain(float[] heightMap)
3442 {
3443 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3444 {
3445 if (m_parentScene is OdeScene)
3446 {
3447 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3448 }
3449 }
3450 else
3451 {
3452 SetTerrain(heightMap, m_worldOffset);
3453 }
3454 }
3455
3456 public void SetTerrain(float[] heightMap, Vector3 pOffset)
3457 {
3458
3459 int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M)
3460
3461 int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra
3462 int heightmapHeight = regionsize + 2;
3463
3464 int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples
3465 int heightmapHeightSamples = (int)regionsize + 2;
3466
3467 // Array of height samples for ODE
3468 float[] _heightmap;
3469 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258
3470
3471 // Other ODE parameters
3472 const float scale = 1.0f;
3473 const float offset = 0.0f;
3474 const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
3475 const int wrap = 0;
3476
3477 float hfmin = 2000f;
3478 float hfmax = -2000f;
3479 float minele = 0.0f; // Dont allow -ve heights
3480
3481 int x = 0;
3482 int y = 0;
3483 int xx = 0;
3484 int yy = 0;
3485
3486 // load the height samples array from the heightMap
3487 for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257
3488 {
3489 for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257
3490 {
3491 xx = x - 1;
3492 if (xx < 0) xx = 0;
3493 if (xx > (regionsize - 1)) xx = regionsize - 1;
3494
3495 yy = y - 1;
3496 if (yy < 0) yy = 0;
3497 if (yy > (regionsize - 1)) yy = regionsize - 1;
3498 // Input xx = 0 0 1 2 ..... 254 255 255 256 total in
3499 // Output x = 0 1 2 3 ..... 255 256 257 258 total out
3500 float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255>
3501 if (val < minele) val = minele;
3502 _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257>
3503 hfmin = (val < hfmin) ? val : hfmin;
3504 hfmax = (val > hfmax) ? val : hfmax;
3505 }
3506 }
3507
3508 lock (OdeLock)
3509 {
3510 IntPtr GroundGeom = IntPtr.Zero;
3511 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3512 {
3513 RegionTerrain.Remove(pOffset);
3514 if (GroundGeom != IntPtr.Zero)
3515 {
3516 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3517 {
3518 TerrainHeightFieldHeights.Remove(GroundGeom);
3519 }
3520 d.SpaceRemove(space, GroundGeom);
3521 d.GeomDestroy(GroundGeom);
3522 }
3523 }
3524 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3525 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3526 heightmapWidth, heightmapHeight, (int)heightmapWidthSamples,
3527 (int)heightmapHeightSamples, scale, offset, thickness, wrap);
3528 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3529 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3530 if (GroundGeom != IntPtr.Zero)
3531 {
3532 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3533 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3534 }
3535 geom_name_map[GroundGeom] = "Terrain";
3536
3537 d.Matrix3 R = new d.Matrix3();
3538
3539 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3540 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3541 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3542
3543 q1 = q1 * q2;
3544 //q1 = q1 * q3;
3545 Vector3 v3;
3546 float angle;
3547 q1.GetAxisAngle(out v3, out angle);
3548
3549 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3550 d.GeomSetRotation(GroundGeom, ref R);
3551 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
3552 IntPtr testGround = IntPtr.Zero;
3553 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3554 {
3555 RegionTerrain.Remove(pOffset);
3556 }
3557 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3558 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3559 }
3560 }
3561
3562 public override void DeleteTerrain()
3563 {
3564 }
3565
3566 public float GetWaterLevel()
3567 {
3568 return waterlevel;
3569 }
3570
3571 public override bool SupportsCombining()
3572 {
3573 return true;
3574 }
3575
3576 public override void UnCombine(PhysicsScene pScene)
3577 {
3578 IntPtr localGround = IntPtr.Zero;
3579// float[] localHeightfield;
3580 bool proceed = false;
3581 List<IntPtr> geomDestroyList = new List<IntPtr>();
3582
3583 lock (OdeLock)
3584 {
3585 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3586 {
3587 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3588 {
3589 if (geom == localGround)
3590 {
3591// localHeightfield = TerrainHeightFieldHeights[geom];
3592 proceed = true;
3593 }
3594 else
3595 {
3596 geomDestroyList.Add(geom);
3597 }
3598 }
3599
3600 if (proceed)
3601 {
3602 m_worldOffset = Vector3.Zero;
3603 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3604 m_parentScene = null;
3605
3606 foreach (IntPtr g in geomDestroyList)
3607 {
3608 // removingHeightField needs to be done or the garbage collector will
3609 // collect the terrain data before we tell ODE to destroy it causing
3610 // memory corruption
3611 if (TerrainHeightFieldHeights.ContainsKey(g))
3612 {
3613// float[] removingHeightField = TerrainHeightFieldHeights[g];
3614 TerrainHeightFieldHeights.Remove(g);
3615
3616 if (RegionTerrain.ContainsKey(g))
3617 {
3618 RegionTerrain.Remove(g);
3619 }
3620
3621 d.GeomDestroy(g);
3622 //removingHeightField = new float[0];
3623 }
3624 }
3625
3626 }
3627 else
3628 {
3629 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3630
3631 }
3632 }
3633 }
3634 }
3635
3636 public override void SetWaterLevel(float baseheight)
3637 {
3638 waterlevel = baseheight;
3639 randomizeWater(waterlevel);
3640 }
3641
3642 public void randomizeWater(float baseheight)
3643 {
3644 const uint heightmapWidth = m_regionWidth + 2;
3645 const uint heightmapHeight = m_regionHeight + 2;
3646 const uint heightmapWidthSamples = m_regionWidth + 2;
3647 const uint heightmapHeightSamples = m_regionHeight + 2;
3648 const float scale = 1.0f;
3649 const float offset = 0.0f;
3650 const float thickness = 2.9f;
3651 const int wrap = 0;
3652
3653 for (int i = 0; i < (258 * 258); i++)
3654 {
3655 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
3656 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
3657 }
3658
3659 lock (OdeLock)
3660 {
3661 if (WaterGeom != IntPtr.Zero)
3662 {
3663 d.SpaceRemove(space, WaterGeom);
3664 }
3665 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3666 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
3667 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3668 offset, thickness, wrap);
3669 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
3670 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
3671 if (WaterGeom != IntPtr.Zero)
3672 {
3673 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
3674 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
3675
3676 }
3677 geom_name_map[WaterGeom] = "Water";
3678
3679 d.Matrix3 R = new d.Matrix3();
3680
3681 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3682 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3683 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3684
3685 q1 = q1 * q2;
3686 //q1 = q1 * q3;
3687 Vector3 v3;
3688 float angle;
3689 q1.GetAxisAngle(out v3, out angle);
3690
3691 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3692 d.GeomSetRotation(WaterGeom, ref R);
3693 d.GeomSetPosition(WaterGeom, 128, 128, 0);
3694
3695 }
3696
3697 }
3698
3699 public override void Dispose()
3700 {
3701 m_rayCastManager.Dispose();
3702 m_rayCastManager = null;
3703
3704 lock (OdeLock)
3705 {
3706 lock (_prims)
3707 {
3708 foreach (OdePrim prm in _prims)
3709 {
3710 RemovePrim(prm);
3711 }
3712 }
3713
3714 //foreach (OdeCharacter act in _characters)
3715 //{
3716 //RemoveAvatar(act);
3717 //}
3718 d.WorldDestroy(world);
3719 //d.CloseODE();
3720 }
3721 }
3722 public override Dictionary<uint, float> GetTopColliders()
3723 {
3724 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
3725 int cnt = 0;
3726 lock (_prims)
3727 {
3728 foreach (OdePrim prm in _prims)
3729 {
3730 if (prm.CollisionScore > 0)
3731 {
3732 returncolliders.Add(prm.m_localID, prm.CollisionScore);
3733 cnt++;
3734 prm.CollisionScore = 0f;
3735 if (cnt > 25)
3736 {
3737 break;
3738 }
3739 }
3740 }
3741 }
3742 return returncolliders;
3743 }
3744
3745 public override bool SupportsRayCast()
3746 {
3747 return true;
3748 }
3749
3750 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3751 {
3752 if (retMethod != null)
3753 {
3754 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3755 }
3756 }
3757
3758#if USE_DRAWSTUFF
3759 // Keyboard callback
3760 public void command(int cmd)
3761 {
3762 IntPtr geom;
3763 d.Mass mass;
3764 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
3765
3766
3767
3768 Char ch = Char.ToLower((Char)cmd);
3769 switch ((Char)ch)
3770 {
3771 case 'w':
3772 try
3773 {
3774 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));
3775
3776 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
3777 ds.SetViewpoint(ref xyz, ref hpr);
3778 }
3779 catch (ArgumentException)
3780 { hpr.X = 0; }
3781 break;
3782
3783 case 'a':
3784 hpr.X++;
3785 ds.SetViewpoint(ref xyz, ref hpr);
3786 break;
3787
3788 case 's':
3789 try
3790 {
3791 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));
3792
3793 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
3794 ds.SetViewpoint(ref xyz, ref hpr);
3795 }
3796 catch (ArgumentException)
3797 { hpr.X = 0; }
3798 break;
3799 case 'd':
3800 hpr.X--;
3801 ds.SetViewpoint(ref xyz, ref hpr);
3802 break;
3803 case 'r':
3804 xyz.Z++;
3805 ds.SetViewpoint(ref xyz, ref hpr);
3806 break;
3807 case 'f':
3808 xyz.Z--;
3809 ds.SetViewpoint(ref xyz, ref hpr);
3810 break;
3811 case 'e':
3812 xyz.Y++;
3813 ds.SetViewpoint(ref xyz, ref hpr);
3814 break;
3815 case 'q':
3816 xyz.Y--;
3817 ds.SetViewpoint(ref xyz, ref hpr);
3818 break;
3819 }
3820 }
3821
3822 public void step(int pause)
3823 {
3824
3825 ds.SetColor(1.0f, 1.0f, 0.0f);
3826 ds.SetTexture(ds.Texture.Wood);
3827 lock (_prims)
3828 {
3829 foreach (OdePrim prm in _prims)
3830 {
3831 //IntPtr body = d.GeomGetBody(prm.prim_geom);
3832 if (prm.prim_geom != IntPtr.Zero)
3833 {
3834 d.Vector3 pos;
3835 d.GeomCopyPosition(prm.prim_geom, out pos);
3836 //d.BodyCopyPosition(body, out pos);
3837
3838 d.Matrix3 R;
3839 d.GeomCopyRotation(prm.prim_geom, out R);
3840 //d.BodyCopyRotation(body, out R);
3841
3842
3843 d.Vector3 sides = new d.Vector3();
3844 sides.X = prm.Size.X;
3845 sides.Y = prm.Size.Y;
3846 sides.Z = prm.Size.Z;
3847
3848 ds.DrawBox(ref pos, ref R, ref sides);
3849 }
3850 }
3851 }
3852 ds.SetColor(1.0f, 0.0f, 0.0f);
3853 lock (_characters)
3854 {
3855 foreach (OdeCharacter chr in _characters)
3856 {
3857 if (chr.Shell != IntPtr.Zero)
3858 {
3859 IntPtr body = d.GeomGetBody(chr.Shell);
3860
3861 d.Vector3 pos;
3862 d.GeomCopyPosition(chr.Shell, out pos);
3863 //d.BodyCopyPosition(body, out pos);
3864
3865 d.Matrix3 R;
3866 d.GeomCopyRotation(chr.Shell, out R);
3867 //d.BodyCopyRotation(body, out R);
3868
3869 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
3870 d.Vector3 sides = new d.Vector3();
3871 sides.X = 0.5f;
3872 sides.Y = 0.5f;
3873 sides.Z = 0.5f;
3874
3875 ds.DrawBox(ref pos, ref R, ref sides);
3876 }
3877 }
3878 }
3879 }
3880
3881 public void start(int unused)
3882 {
3883 ds.SetViewpoint(ref xyz, ref hpr);
3884 }
3885#endif
3886 }
3887}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
new file mode 100644
index 0000000..e7e7bb3
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
@@ -0,0 +1,353 @@
1/* Ubit 2012
2 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
3 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
4 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
5 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
6 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
7 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
8 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
9 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
10 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
11 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
12*/
13
14// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian
15
16using System;
17using System.IO;
18using OpenMetaverse;
19
20namespace OpenSim.Region.Physics.OdePlugin
21{
22
23 unsafe public class wstreamer
24 {
25 byte[] buf;
26 int index;
27 byte* src;
28
29 public wstreamer()
30 {
31 buf = new byte[1024];
32 index = 0;
33 }
34 public wstreamer(int size)
35 {
36 buf = new byte[size];
37 index = 0;
38 }
39
40 public byte[] close()
41 {
42 byte[] data = new byte[index];
43 Buffer.BlockCopy(buf, 0, data, 0, index);
44 return data;
45 }
46
47 public void Seek(int pos)
48 {
49 index = pos;
50 }
51
52 public void Seekrel(int pos)
53 {
54 index += pos;
55 }
56
57 public void Wbyte(byte value)
58 {
59 buf[index++] = value;
60 }
61 public void Wshort(short value)
62 {
63 src = (byte*)&value;
64 buf[index++] = *src++;
65 buf[index++] = *src;
66 }
67 public void Wushort(ushort value)
68 {
69 src = (byte*)&value;
70 buf[index++] = *src++;
71 buf[index++] = *src;
72 }
73 public void Wint(int value)
74 {
75 src = (byte*)&value;
76 buf[index++] = *src++;
77 buf[index++] = *src++;
78 buf[index++] = *src++;
79 buf[index++] = *src;
80 }
81 public void Wuint(uint value)
82 {
83 src = (byte*)&value;
84 buf[index++] = *src++;
85 buf[index++] = *src++;
86 buf[index++] = *src++;
87 buf[index++] = *src;
88 }
89 public void Wlong(long value)
90 {
91 src = (byte*)&value;
92 buf[index++] = *src++;
93 buf[index++] = *src++;
94 buf[index++] = *src++;
95 buf[index++] = *src++;
96 buf[index++] = *src++;
97 buf[index++] = *src++;
98 buf[index++] = *src++;
99 buf[index++] = *src;
100 }
101 public void Wulong(ulong value)
102 {
103 src = (byte*)&value;
104 buf[index++] = *src++;
105 buf[index++] = *src++;
106 buf[index++] = *src++;
107 buf[index++] = *src++;
108 buf[index++] = *src++;
109 buf[index++] = *src++;
110 buf[index++] = *src++;
111 buf[index++] = *src;
112 }
113
114 public void Wfloat(float value)
115 {
116 src = (byte*)&value;
117 buf[index++] = *src++;
118 buf[index++] = *src++;
119 buf[index++] = *src++;
120 buf[index++] = *src;
121 }
122
123 public void Wdouble(double value)
124 {
125 src = (byte*)&value;
126 buf[index++] = *src++;
127 buf[index++] = *src++;
128 buf[index++] = *src++;
129 buf[index++] = *src++;
130 buf[index++] = *src++;
131 buf[index++] = *src++;
132 buf[index++] = *src++;
133 buf[index++] = *src;
134 }
135
136 public void Wvector3(Vector3 value)
137 {
138 src = (byte*)&value.X;
139 buf[index++] = *src++;
140 buf[index++] = *src++;
141 buf[index++] = *src++;
142 buf[index++] = *src;
143 src = (byte*)&value.Y; // it may have padding ??
144 buf[index++] = *src++;
145 buf[index++] = *src++;
146 buf[index++] = *src++;
147 buf[index++] = *src;
148 src = (byte*)&value.Z;
149 buf[index++] = *src++;
150 buf[index++] = *src++;
151 buf[index++] = *src++;
152 buf[index++] = *src;
153 }
154 public void Wquat(Quaternion value)
155 {
156 src = (byte*)&value.X;
157 buf[index++] = *src++;
158 buf[index++] = *src++;
159 buf[index++] = *src++;
160 buf[index++] = *src;
161 src = (byte*)&value.Y; // it may have padding ??
162 buf[index++] = *src++;
163 buf[index++] = *src++;
164 buf[index++] = *src++;
165 buf[index++] = *src;
166 src = (byte*)&value.Z;
167 buf[index++] = *src++;
168 buf[index++] = *src++;
169 buf[index++] = *src++;
170 buf[index++] = *src;
171 src = (byte*)&value.W;
172 buf[index++] = *src++;
173 buf[index++] = *src++;
174 buf[index++] = *src++;
175 buf[index++] = *src;
176 }
177 }
178
179 unsafe public class rstreamer
180 {
181 private byte[] rbuf;
182 private int ptr;
183 private byte* dst;
184
185 public rstreamer(byte[] data)
186 {
187 rbuf = data;
188 ptr = 0;
189 }
190
191 public void close()
192 {
193 }
194
195 public void Seek(int pos)
196 {
197 ptr = pos;
198 }
199
200 public void Seekrel(int pos)
201 {
202 ptr += pos;
203 }
204
205 public byte Rbyte()
206 {
207 return (byte)rbuf[ptr++];
208 }
209
210 public short Rshort()
211 {
212 short v;
213 dst = (byte*)&v;
214 *dst++ = rbuf[ptr++];
215 *dst = rbuf[ptr++];
216 return v;
217 }
218 public ushort Rushort()
219 {
220 ushort v;
221 dst = (byte*)&v;
222 *dst++ = rbuf[ptr++];
223 *dst = rbuf[ptr++];
224 return v;
225 }
226 public int Rint()
227 {
228 int v;
229 dst = (byte*)&v;
230 *dst++ = rbuf[ptr++];
231 *dst++ = rbuf[ptr++];
232 *dst++ = rbuf[ptr++];
233 *dst = rbuf[ptr++];
234 return v;
235 }
236 public uint Ruint()
237 {
238 uint v;
239 dst = (byte*)&v;
240 *dst++ = rbuf[ptr++];
241 *dst++ = rbuf[ptr++];
242 *dst++ = rbuf[ptr++];
243 *dst = rbuf[ptr++];
244 return v;
245 }
246 public long Rlong()
247 {
248 long v;
249 dst = (byte*)&v;
250 *dst++ = rbuf[ptr++];
251 *dst++ = rbuf[ptr++];
252 *dst++ = rbuf[ptr++];
253 *dst++ = rbuf[ptr++];
254 *dst++ = rbuf[ptr++];
255 *dst++ = rbuf[ptr++];
256 *dst++ = rbuf[ptr++];
257 *dst = rbuf[ptr++];
258 return v;
259 }
260 public ulong Rulong()
261 {
262 ulong v;
263 dst = (byte*)&v;
264 *dst++ = rbuf[ptr++];
265 *dst++ = rbuf[ptr++];
266 *dst++ = rbuf[ptr++];
267 *dst++ = rbuf[ptr++];
268 *dst++ = rbuf[ptr++];
269 *dst++ = rbuf[ptr++];
270 *dst++ = rbuf[ptr++];
271 *dst = rbuf[ptr++];
272 return v;
273 }
274 public float Rfloat()
275 {
276 float v;
277 dst = (byte*)&v;
278 *dst++ = rbuf[ptr++];
279 *dst++ = rbuf[ptr++];
280 *dst++ = rbuf[ptr++];
281 *dst = rbuf[ptr++];
282 return v;
283 }
284
285 public double Rdouble()
286 {
287 double v;
288 dst = (byte*)&v;
289 *dst++ = rbuf[ptr++];
290 *dst++ = rbuf[ptr++];
291 *dst++ = rbuf[ptr++];
292 *dst++ = rbuf[ptr++];
293 *dst++ = rbuf[ptr++];
294 *dst++ = rbuf[ptr++];
295 *dst++ = rbuf[ptr++];
296 *dst = rbuf[ptr++];
297 return v;
298 }
299
300 public Vector3 Rvector3()
301 {
302 Vector3 v;
303 dst = (byte*)&v.X;
304 *dst++ = rbuf[ptr++];
305 *dst++ = rbuf[ptr++];
306 *dst++ = rbuf[ptr++];
307 *dst = rbuf[ptr++];
308
309 dst = (byte*)&v.Y;
310 *dst++ = rbuf[ptr++];
311 *dst++ = rbuf[ptr++];
312 *dst++ = rbuf[ptr++];
313 *dst = rbuf[ptr++];
314
315 dst = (byte*)&v.Z;
316 *dst++ = rbuf[ptr++];
317 *dst++ = rbuf[ptr++];
318 *dst++ = rbuf[ptr++];
319 *dst = rbuf[ptr++];
320 return v;
321 }
322
323 public Quaternion Rquat()
324 {
325 Quaternion v;
326 dst = (byte*)&v.X;
327 *dst++ = rbuf[ptr++];
328 *dst++ = rbuf[ptr++];
329 *dst++ = rbuf[ptr++];
330 *dst = rbuf[ptr++];
331
332 dst = (byte*)&v.Y;
333 *dst++ = rbuf[ptr++];
334 *dst++ = rbuf[ptr++];
335 *dst++ = rbuf[ptr++];
336 *dst = rbuf[ptr++];
337
338 dst = (byte*)&v.Z;
339 *dst++ = rbuf[ptr++];
340 *dst++ = rbuf[ptr++];
341 *dst++ = rbuf[ptr++];
342 *dst = rbuf[ptr++];
343
344 dst = (byte*)&v.W;
345 *dst++ = rbuf[ptr++];
346 *dst++ = rbuf[ptr++];
347 *dst++ = rbuf[ptr++];
348 *dst = rbuf[ptr++];
349
350 return v;
351 }
352 }
353}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
new file mode 100644
index 0000000..69e2d03
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
@@ -0,0 +1,122 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using Nini.Config;
30using NUnit.Framework;
31using OpenMetaverse;
32using OpenSim.Framework;
33using OpenSim.Region.Physics.Manager;
34using log4net;
35using System.Reflection;
36
37namespace OpenSim.Region.Physics.OdePlugin
38{
39 [TestFixture]
40 public class ODETestClass
41 {
42 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
43
44 private OdePlugin cbt;
45 private PhysicsScene ps;
46 private IMeshingPlugin imp;
47
48 [SetUp]
49 public void Initialize()
50 {
51 // Loading ODEPlugin
52 cbt = new OdePlugin();
53 // Loading Zero Mesher
54 imp = new ZeroMesherPlugin();
55 // Getting Physics Scene
56 ps = cbt.GetScene("test");
57 // Initializing Physics Scene.
58 ps.Initialise(imp.GetMesher(),null);
59 float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
60 for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++)
61 {
62 _heightmap[i] = 21f;
63 }
64 ps.SetTerrain(_heightmap);
65 }
66
67 [TearDown]
68 public void Terminate()
69 {
70 ps.DeleteTerrain();
71 ps.Dispose();
72
73 }
74
75 [Test]
76 public void CreateAndDropPhysicalCube()
77 {
78 PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
79 Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
80 Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
81 Quaternion rot = Quaternion.Identity;
82 PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
83 OdePrim oprim = (OdePrim)prim;
84 OdeScene pscene = (OdeScene) ps;
85
86 Assert.That(oprim.m_taintadd);
87
88 prim.LocalID = 5;
89
90 for (int i = 0; i < 58; i++)
91 {
92 ps.Simulate(0.133f);
93
94 Assert.That(oprim.prim_geom != (IntPtr)0);
95
96 Assert.That(oprim.m_targetSpace != (IntPtr)0);
97
98 //Assert.That(oprim.m_targetSpace == pscene.space);
99 m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
100
101 Assert.That(!oprim.m_taintadd);
102 m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString());
103
104 // Make sure we're above the ground
105 //Assert.That(prim.Position.Z > 20f);
106 //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
107
108 // Make sure we've got a Body
109 Assert.That(oprim.Body != (IntPtr)0);
110 //m_log.Info(
111 }
112
113 // Make sure we're not somewhere above the ground
114 Assert.That(prim.Position.Z < 21.5f);
115
116 ps.RemovePrim(prim);
117 Assert.That(oprim.m_taintremove);
118 ps.Simulate(0.133f);
119 Assert.That(oprim.Body == (IntPtr)0);
120 }
121 }
122}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
@@ -0,0 +1,98 @@
1/*
2 * Copyright ODE
3 * Ode.NET - .NET bindings for ODE
4 * Jason Perkins (starkos@industriousone.com)
5 * Licensed under the New BSD
6 * Part of the OpenDynamicsEngine
7Open Dynamics Engine
8Copyright (c) 2001-2007, Russell L. Smith.
9All rights reserved.
10
11Redistribution and use in source and binary forms, with or without
12modification, are permitted provided that the following conditions
13are met:
14
15Redistributions of source code must retain the above copyright notice,
16this list of conditions and the following disclaimer.
17
18Redistributions in binary form must reproduce the above copyright notice,
19this list of conditions and the following disclaimer in the documentation
20and/or other materials provided with the distribution.
21
22Neither the names of ODE's copyright owner nor the names of its
23contributors may be used to endorse or promote products derived from
24this software without specific prior written permission.
25
26THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
30OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
31SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
32TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
33PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
34LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
35NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
36SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
37 *
38 *
39 */
40
41using System;
42using System.Runtime.InteropServices;
43using Ode.NET;
44
45namespace Drawstuff.NET
46{
47#if dDOUBLE
48 using dReal = System.Double;
49#else
50 using dReal = System.Single;
51#endif
52
53 public static class ds
54 {
55 public const int VERSION = 2;
56
57 public enum Texture
58 {
59 None,
60 Wood
61 }
62
63 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
64 public delegate void CallbackFunction(int arg);
65
66 [StructLayout(LayoutKind.Sequential)]
67 public struct Functions
68 {
69 public int version;
70 public CallbackFunction start;
71 public CallbackFunction step;
72 public CallbackFunction command;
73 public CallbackFunction stop;
74 public string path_to_textures;
75 }
76
77 [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
78 public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
79
80 [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
81 public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
82
83 [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
84 public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
85
86 [DllImport("drawstuff", EntryPoint = "dsSetColor")]
87 public static extern void SetColor(float red, float green, float blue);
88
89 [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
90 public static extern void SetTexture(Texture texture);
91
92 [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
93 public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
94
95 [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
96 public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
97 }
98}
generated by cgit v1.1 at 2024-12-21 06:16:07 +0000