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