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