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