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