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