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