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