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