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