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