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