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