aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1477
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs1087
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs4032
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs624
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs2014
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs90
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2816
8 files changed, 12198 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d46341b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System.Reflection;
29using System.Runtime.InteropServices;
30
31// Information about this assembly is defined by the following
32// attributes.
33//
34// change them to the information which is associated with the assembly
35// you compile.
36
37[assembly : AssemblyTitle("OdePlugin")]
38[assembly : AssemblyDescription("Ubit Variation")]
39[assembly : AssemblyConfiguration("")]
40[assembly : AssemblyCompany("http://opensimulator.org")]
41[assembly : AssemblyProduct("OdePlugin")]
42[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
43[assembly : AssemblyTrademark("")]
44[assembly : AssemblyCulture("")]
45
46// This sets the default COM visibility of types in the assembly to invisible.
47// If you need to expose a type to COM, use [ComVisible(true)] on that type.
48
49[assembly : ComVisible(false)]
50
51// The assembly version has following format :
52//
53// Major.Minor.Build.Revision
54//
55// You can specify all values by your own or you can build default build and revision
56// numbers with the '*' character (the default):
57
58[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..c363310
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1477 @@
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// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private bool _zeroFlag = false;
78 private Vector3 _velocity;
79 private Vector3 _target_velocity;
80 private Vector3 _acceleration;
81 private Vector3 m_rotationalVelocity;
82 private float m_mass = 80f;
83 public float m_density = 60f;
84 private bool m_pidControllerActive = true;
85 public float PID_D = 800.0f;
86 public float PID_P = 900.0f;
87 //private static float POSTURE_SERVO = 10000.0f;
88 public float CAPSULE_RADIUS = 0.37f;
89 public float CAPSULE_LENGTH = 2.140599f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool m_iscolliding = false;
94 private bool m_iscollidingGround = false;
95 private bool m_iscollidingObj = false;
96 private bool m_alwaysRun = false;
97 private int m_requestedUpdateFrequency = 0;
98 private uint m_localID = 0;
99 public bool m_returnCollisions = false;
100 // taints and their non-tainted counterparts
101 public bool m_isPhysical = false; // the current physical status
102 public float MinimumGroundFlightOffset = 3f;
103
104 private float m_buoyancy = 0f;
105
106 private bool m_freemove = false;
107 // private CollisionLocker ode;
108
109// private string m_name = String.Empty;
110 // other filter control
111 int m_colliderfilter = 0;
112 int m_colliderGroundfilter = 0;
113 int m_colliderObjectfilter = 0;
114
115 // Default we're a Character
116 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
117
118 // Default, Collide with Other Geometries, spaces, bodies and characters.
119 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
120 | CollisionCategories.Geom
121 | CollisionCategories.VolumeDtc
122 );
123 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero;
127 public IntPtr Amotor = IntPtr.Zero;
128 public d.Mass ShellMass;
129// public bool collidelock = false;
130
131 public int m_eventsubscription = 0;
132 private int m_cureventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = null;
134 private bool SentEmptyCollisionsEvent;
135
136 // unique UUID of this character object
137 public UUID m_uuid;
138 public bool bad = false;
139
140 float mu;
141
142 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
143 {
144 m_uuid = UUID.Random();
145
146 if (pos.IsFinite())
147 {
148 if (pos.Z > 99999f)
149 {
150 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
151 }
152 if (pos.Z < -100f) // shouldn't this be 0 ?
153 {
154 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
155 }
156 _position = pos;
157 }
158 else
159 {
160 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
161 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
162 }
163
164 _parent_scene = parent_scene;
165
166 PID_D = pid_d;
167 PID_P = pid_p;
168 CAPSULE_RADIUS = capsule_radius;
169 m_density = density;
170 m_mass = 80f; // sure we have a default
171
172 // force lower density for testing
173 m_density = 3.0f;
174
175
176 mu = parent_scene.AvatarFriction;
177
178 walkDivisor = walk_divisor;
179 runDivisor = rundivisor;
180
181 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
182 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
183
184 m_isPhysical = false; // current status: no ODE information exists
185
186 Name = avName;
187
188 AddChange(changes.Add, null);
189 }
190
191 public override int PhysicsActorType
192 {
193 get { return (int)ActorTypes.Agent; }
194 set { return; }
195 }
196
197 public override void getContactData(ref ContactData cdata)
198 {
199 cdata.mu = mu;
200 cdata.bounce = 0;
201 cdata.softcolide = false;
202 }
203
204 public override bool Building { get; set; }
205
206 /// <summary>
207 /// If this is set, the avatar will move faster
208 /// </summary>
209 public override bool SetAlwaysRun
210 {
211 get { return m_alwaysRun; }
212 set { m_alwaysRun = value; }
213 }
214
215 public override uint LocalID
216 {
217 get { return m_localID; }
218 set { m_localID = value; }
219 }
220
221 public override PhysicsActor ParentActor
222 {
223 get { return (PhysicsActor)this; }
224 }
225
226 public override bool Grabbed
227 {
228 set { return; }
229 }
230
231 public override bool Selected
232 {
233 set { return; }
234 }
235
236 public override float Buoyancy
237 {
238 get { return m_buoyancy; }
239 set { m_buoyancy = value; }
240 }
241
242 public override bool FloatOnWater
243 {
244 set { return; }
245 }
246
247 public override bool IsPhysical
248 {
249 get { return m_isPhysical; }
250 set { return; }
251 }
252
253 public override bool ThrottleUpdates
254 {
255 get { return false; }
256 set { return; }
257 }
258
259 public override bool Flying
260 {
261 get { return flying; }
262 set
263 {
264 flying = value;
265 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
266 }
267 }
268
269 /// <summary>
270 /// Returns if the avatar is colliding in general.
271 /// This includes the ground and objects and avatar.
272 /// </summary>
273 public override bool IsColliding
274 {
275 get { return (m_iscolliding || m_iscollidingGround); }
276 set
277 {
278 if (value)
279 {
280 m_colliderfilter += 2;
281 if (m_colliderfilter > 2)
282 m_colliderfilter = 2;
283 }
284 else
285 {
286 m_colliderfilter--;
287 if (m_colliderfilter < 0)
288 m_colliderfilter = 0;
289 }
290
291 if (m_colliderfilter == 0)
292 m_iscolliding = false;
293 else
294 {
295 m_pidControllerActive = true;
296 m_iscolliding = true;
297 }
298 }
299 }
300
301 /// <summary>
302 /// Returns if an avatar is colliding with the ground
303 /// </summary>
304 public override bool CollidingGround
305 {
306 get { return m_iscollidingGround; }
307 set
308 {
309 /* we now control this
310 if (value)
311 {
312 m_colliderGroundfilter += 2;
313 if (m_colliderGroundfilter > 2)
314 m_colliderGroundfilter = 2;
315 }
316 else
317 {
318 m_colliderGroundfilter--;
319 if (m_colliderGroundfilter < 0)
320 m_colliderGroundfilter = 0;
321 }
322
323 if (m_colliderGroundfilter == 0)
324 m_iscollidingGround = false;
325 else
326 m_iscollidingGround = true;
327 */
328 }
329
330 }
331
332 /// <summary>
333 /// Returns if the avatar is colliding with an object
334 /// </summary>
335 public override bool CollidingObj
336 {
337 get { return m_iscollidingObj; }
338 set
339 {
340 // Ubit filter this also
341 if (value)
342 {
343 m_colliderObjectfilter += 2;
344 if (m_colliderObjectfilter > 2)
345 m_colliderObjectfilter = 2;
346 }
347 else
348 {
349 m_colliderObjectfilter--;
350 if (m_colliderObjectfilter < 0)
351 m_colliderObjectfilter = 0;
352 }
353
354 if (m_colliderObjectfilter == 0)
355 m_iscollidingObj = false;
356 else
357 m_iscollidingObj = true;
358
359 // m_iscollidingObj = value;
360
361 if (m_iscollidingObj)
362 m_pidControllerActive = false;
363 else
364 m_pidControllerActive = true;
365 }
366 }
367
368 /// <summary>
369 /// turn the PID controller on or off.
370 /// The PID Controller will turn on all by itself in many situations
371 /// </summary>
372 /// <param name="status"></param>
373 public void SetPidStatus(bool status)
374 {
375 m_pidControllerActive = status;
376 }
377
378 public override bool Stopped
379 {
380 get { return _zeroFlag; }
381 }
382
383 /// <summary>
384 /// This 'puts' an avatar somewhere in the physics space.
385 /// Not really a good choice unless you 'know' it's a good
386 /// spot otherwise you're likely to orbit the avatar.
387 /// </summary>
388 public override Vector3 Position
389 {
390 get { return _position; }
391 set
392 {
393 if (value.IsFinite())
394 {
395 if (value.Z > 9999999f)
396 {
397 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
398 }
399 if (value.Z < -100f)
400 {
401 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
402 }
403 AddChange(changes.Position, value);
404 }
405 else
406 {
407 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
408 }
409 }
410 }
411
412 public override Vector3 RotationalVelocity
413 {
414 get { return m_rotationalVelocity; }
415 set { m_rotationalVelocity = value; }
416 }
417
418 /// <summary>
419 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
420 /// and use it to offset landings properly
421 /// </summary>
422 public override Vector3 Size
423 {
424 get {
425 float d = CAPSULE_RADIUS * 2;
426 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
427 set
428 {
429 if (value.IsFinite())
430 {
431 AddChange(changes.Size, value);
432 }
433 else
434 {
435 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
436 }
437 }
438 }
439
440 /// <summary>
441 /// This creates the Avatar's physical Surrogate at the position supplied
442 /// </summary>
443 /// <param name="npositionX"></param>
444 /// <param name="npositionY"></param>
445 /// <param name="npositionZ"></param>
446
447 //
448 /// <summary>
449 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
450 /// This may be used in calculations in the scene/scenepresence
451 /// </summary>
452 public override float Mass
453 {
454 get
455 {
456 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
457 return m_density * AVvolume;
458 }
459 }
460 public override void link(PhysicsActor obj)
461 {
462
463 }
464
465 public override void delink()
466 {
467
468 }
469
470 public override void LockAngularMotion(Vector3 axis)
471 {
472
473 }
474
475
476 public override Vector3 Force
477 {
478 get { return _target_velocity; }
479 set { return; }
480 }
481
482 public override int VehicleType
483 {
484 get { return 0; }
485 set { return; }
486 }
487
488 public override void VehicleFloatParam(int param, float value)
489 {
490
491 }
492
493 public override void VehicleVectorParam(int param, Vector3 value)
494 {
495
496 }
497
498 public override void VehicleRotationParam(int param, Quaternion rotation)
499 {
500
501 }
502
503 public override void VehicleFlags(int param, bool remove)
504 {
505
506 }
507
508 public override void SetVolumeDetect(int param)
509 {
510
511 }
512
513 public override Vector3 CenterOfMass
514 {
515 get
516 {
517 Vector3 pos = _position;
518 return pos;
519 }
520 }
521
522 public override Vector3 GeometricCenter
523 {
524 get
525 {
526 Vector3 pos = _position;
527 return pos;
528 }
529 }
530
531 public override PrimitiveBaseShape Shape
532 {
533 set { return; }
534 }
535
536 public override Vector3 Velocity
537 {
538 get
539 {
540 return _velocity;
541 }
542 set
543 {
544 if (value.IsFinite())
545 {
546 AddChange(changes.Velocity, value);
547 }
548 else
549 {
550 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
551 }
552 }
553 }
554
555 public override Vector3 Torque
556 {
557 get { return Vector3.Zero; }
558 set { return; }
559 }
560
561 public override float CollisionScore
562 {
563 get { return 0f; }
564 set { }
565 }
566
567 public override bool Kinematic
568 {
569 get { return false; }
570 set { }
571 }
572
573 public override Quaternion Orientation
574 {
575 get { return Quaternion.Identity; }
576 set
577 {
578 }
579 }
580
581 public override Vector3 Acceleration
582 {
583 get { return _acceleration; }
584 set { }
585 }
586
587 public void SetAcceleration(Vector3 accel)
588 {
589 m_pidControllerActive = true;
590 _acceleration = accel;
591 }
592
593 /// <summary>
594 /// Adds the force supplied to the Target Velocity
595 /// The PID controller takes this target velocity and tries to make it a reality
596 /// </summary>
597 /// <param name="force"></param>
598 public override void AddForce(Vector3 force, bool pushforce)
599 {
600 if (force.IsFinite())
601 {
602 if (pushforce)
603 {
604 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
605 }
606 else
607 {
608 AddChange(changes.Velocity, force);
609 }
610 }
611 else
612 {
613 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
614 }
615 //m_lastUpdateSent = false;
616 }
617
618 public override void AddAngularForce(Vector3 force, bool pushforce)
619 {
620
621 }
622
623 public override void SetMomentum(Vector3 momentum)
624 {
625 if (momentum.IsFinite())
626 AddChange(changes.Momentum, momentum);
627 }
628
629
630 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
631 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
632 // place that is safe to call this routine AvatarGeomAndBodyCreation.
633 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
634 {
635 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
636 if (CAPSULE_LENGTH <= 0)
637 {
638 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
639 CAPSULE_LENGTH = 0.01f;
640
641 }
642
643 if (CAPSULE_RADIUS <= 0)
644 {
645 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
646 CAPSULE_RADIUS = 0.01f;
647
648 }
649 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
650
651 d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories);
652 d.GeomSetCollideBits(Shell, (uint)m_collisionFlags);
653
654 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
655
656 m_mass = ShellMass.mass; // update mass
657
658 // rescale PID parameters
659 PID_D = _parent_scene.avPIDD;
660 PID_P = _parent_scene.avPIDP;
661
662 // rescale PID parameters so that this aren't affected by mass
663 // and so don't get unstable for some masses
664 // also scale by ode time step so you don't need to refix them
665
666 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
667 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
668 PID_P /= 50 * 80;
669 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
670
671 Body = d.BodyCreate(_parent_scene.world);
672
673 _zeroFlag = false;
674 m_pidControllerActive = true;
675 m_freemove = false;
676
677 d.BodySetAutoDisableFlag(Body, false);
678 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
679
680 _position.X = npositionX;
681 _position.Y = npositionY;
682 _position.Z = npositionZ;
683
684 d.BodySetMass(Body, ref ShellMass);
685 d.GeomSetBody(Shell, Body);
686
687 // The purpose of the AMotor here is to keep the avatar's physical
688 // surrogate from rotating while moving
689 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
690 d.JointAttach(Amotor, Body, IntPtr.Zero);
691
692 d.JointSetAMotorMode(Amotor, 0);
693 d.JointSetAMotorNumAxes(Amotor, 3);
694 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
695 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
696 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
697
698 d.JointSetAMotorAngle(Amotor, 0, 0);
699 d.JointSetAMotorAngle(Amotor, 1, 0);
700 d.JointSetAMotorAngle(Amotor, 2, 0);
701
702 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
703 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
704 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
705 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
706 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
707 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
708
709 // These lowstops and high stops are effectively (no wiggle room)
710 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
711 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
712 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
713 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
714 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
715 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
716
717 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
718 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
719 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
720
721 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
722 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
723 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
724 }
725
726 /// <summary>
727 /// Destroys the avatar body and geom
728
729 private void AvatarGeomAndBodyDestroy()
730 {
731 // Kill the Amotor
732 if (Amotor != IntPtr.Zero)
733 {
734 d.JointDestroy(Amotor);
735 Amotor = IntPtr.Zero;
736 }
737
738 if (Body != IntPtr.Zero)
739 {
740 //kill the body
741 d.BodyDestroy(Body);
742 Body = IntPtr.Zero;
743 }
744
745 //kill the Geometry
746 if (Shell != IntPtr.Zero)
747 {
748// _parent_scene.geom_name_map.Remove(Shell);
749 _parent_scene.actor_name_map.Remove(Shell);
750 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
751 d.GeomDestroy(Shell);
752 Shell = IntPtr.Zero;
753 }
754 }
755
756 /// <summary>
757 /// Called from Simulate
758 /// This is the avatar's movement control + PID Controller
759 /// </summary>
760 /// <param name="timeStep"></param>
761 public void Move(float timeStep, List<OdeCharacter> defects)
762 {
763 if (Body == IntPtr.Zero)
764 return;
765
766 d.Vector3 dtmp = d.BodyGetPosition(Body);
767 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
768
769 // the Amotor still lets avatar rotation to drift during colisions
770 // so force it back to identity
771
772 d.Quaternion qtmp;
773 qtmp.W = 1;
774 qtmp.X = 0;
775 qtmp.Y = 0;
776 qtmp.Z = 0;
777 d.BodySetQuaternion(Body, ref qtmp);
778
779 if (m_pidControllerActive == false)
780 {
781 _zeroPosition = localpos;
782 }
783
784 if (!localpos.IsFinite())
785 {
786 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
787 defects.Add(this);
788 // _parent_scene.RemoveCharacter(this);
789
790 // destroy avatar capsule and related ODE data
791 AvatarGeomAndBodyDestroy();
792 return;
793 }
794
795 // check outbounds forcing to be in world
796 bool fixbody = false;
797 if (localpos.X < 0.0f)
798 {
799 fixbody = true;
800 localpos.X = 0.1f;
801 }
802 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
803 {
804 fixbody = true;
805 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
806 }
807 if (localpos.Y < 0.0f)
808 {
809 fixbody = true;
810 localpos.Y = 0.1f;
811 }
812 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
813 {
814 fixbody = true;
815 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
816 }
817 if (fixbody)
818 {
819 m_freemove = false;
820 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
821 }
822
823 float breakfactor;
824
825 Vector3 vec = Vector3.Zero;
826 dtmp = d.BodyGetLinearVel(Body);
827 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
828 float velLengthSquared = vel.LengthSquared();
829
830 float movementdivisor = 1f;
831 //Ubit change divisions into multiplications below
832 if (!m_alwaysRun)
833 movementdivisor = 1 / walkDivisor;
834 else
835 movementdivisor = 1 / runDivisor;
836
837 //******************************************
838 // colide with land
839 d.AABB aabb;
840 d.GeomGetAABB(Shell, out aabb);
841 float chrminZ = aabb.MinZ;
842
843 Vector3 posch = localpos;
844
845 float ftmp;
846
847 if (flying)
848 {
849 ftmp = timeStep;
850 posch.X += vel.X * ftmp;
851 posch.Y += vel.Y * ftmp;
852 }
853
854 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
855 if (chrminZ < terrainheight)
856 {
857 float depth = terrainheight - chrminZ;
858 if (!flying)
859 {
860 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
861 }
862 else
863 vec.Z = depth * PID_P * 50;
864
865 if (depth < 0.1f)
866 {
867 m_colliderGroundfilter++;
868 if (m_colliderGroundfilter > 2)
869 {
870 m_iscolliding = true;
871 m_colliderfilter = 2;
872
873 if (m_colliderGroundfilter > 10)
874 {
875 m_colliderGroundfilter = 10;
876 m_freemove = false;
877 }
878
879 m_iscollidingGround = true;
880
881 ContactPoint contact = new ContactPoint();
882 contact.PenetrationDepth = depth;
883 contact.Position.X = localpos.X;
884 contact.Position.Y = localpos.Y;
885 contact.Position.Z = chrminZ;
886 contact.SurfaceNormal.X = 0f;
887 contact.SurfaceNormal.Y = 0f;
888 contact.SurfaceNormal.Z = -1f;
889 contact.RelativeSpeed = -vel.Z;
890 AddCollisionEvent(0, contact);
891
892 vec.Z *= 0.5f;
893 }
894 }
895
896 else
897 {
898 m_colliderGroundfilter = 0;
899 m_iscollidingGround = false;
900 }
901 }
902 else
903 {
904 m_colliderGroundfilter = 0;
905 m_iscollidingGround = false;
906 }
907
908 //******************************************
909
910 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
911
912 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
913 if (!tviszero)
914 m_freemove = false;
915
916 if (!m_freemove)
917 {
918
919 // if velocity is zero, use position control; otherwise, velocity control
920 if (tviszero && m_iscolliding)
921 {
922 // keep track of where we stopped. No more slippin' & slidin'
923 if (!_zeroFlag)
924 {
925 _zeroFlag = true;
926 _zeroPosition = localpos;
927 }
928 if (m_pidControllerActive)
929 {
930 // We only want to deactivate the PID Controller if we think we want to have our surrogate
931 // react to the physics scene by moving it's position.
932 // Avatar to Avatar collisions
933 // Prim to avatar collisions
934
935 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
936 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
937 if (flying)
938 {
939 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
940 }
941 }
942 //PidStatus = true;
943 }
944 else
945 {
946 m_pidControllerActive = true;
947 _zeroFlag = false;
948
949 if (m_iscolliding)
950 {
951 if (!flying)
952 {
953 if (_target_velocity.Z > 0.0f)
954 {
955 // We're colliding with something and we're not flying but we're moving
956 // This means we're walking or running. JUMPING
957 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
958 }
959 // We're standing on something
960 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
961 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
962 }
963 else
964 {
965 // We're flying and colliding with something
966 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
967 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
968 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
969 }
970 }
971 else // ie not colliding
972 {
973 if (flying) //(!m_iscolliding && flying)
974 {
975 // we're in mid air suspended
976 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
977 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
978 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
979 }
980
981 else
982 {
983 // we're not colliding and we're not flying so that means we're falling!
984 // m_iscolliding includes collisions with the ground.
985
986 // d.Vector3 pos = d.BodyGetPosition(Body);
987 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
988 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
989 }
990 }
991 }
992
993 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
994 {
995 breakfactor = 0.16f * m_mass;
996 vec.X -= breakfactor * vel.X;
997 vec.Y -= breakfactor * vel.Y;
998 vec.Z -= breakfactor * vel.Z;
999 }
1000 }
1001 else
1002 {
1003 breakfactor = m_mass;
1004 vec.X -= breakfactor * vel.X;
1005 vec.Y -= breakfactor * vel.Y;
1006 if (flying)
1007 vec.Z -= breakfactor * vel.Z;
1008 else
1009 vec.Z -= .5f* m_mass * vel.Z;
1010 }
1011
1012 if (flying)
1013 {
1014 vec.Z -= _parent_scene.gravityz * m_mass;
1015
1016 //Added for auto fly height. Kitto Flora
1017 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1018
1019 if (localpos.Z < target_altitude)
1020 {
1021 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1022 }
1023 // end add Kitto Flora
1024 }
1025
1026 if (vec.IsFinite())
1027 {
1028 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1029 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1030 }
1031 else
1032 {
1033 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1034 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1035 defects.Add(this);
1036 // _parent_scene.RemoveCharacter(this);
1037 // destroy avatar capsule and related ODE data
1038 AvatarGeomAndBodyDestroy();
1039 return;
1040 }
1041
1042 // update our local ideia of position velocity and aceleration
1043 _position = localpos;
1044 if (_zeroFlag)
1045 {
1046 _velocity = Vector3.Zero;
1047 _acceleration = Vector3.Zero;
1048 }
1049 else
1050 {
1051 _acceleration = _velocity; // previus velocity
1052 _velocity = vel;
1053 _acceleration = (vel - _acceleration) / timeStep;
1054 }
1055
1056 }
1057
1058 /// <summary>
1059 /// Updates the reported position and velocity.
1060 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1061 /// also outbounds checking
1062 /// copy and outbounds now done in move(..) at ode rate
1063 ///
1064 /// </summary>
1065 public void UpdatePositionAndVelocity()
1066 {
1067 return;
1068
1069// if (Body == IntPtr.Zero)
1070// return;
1071
1072 }
1073
1074 /// <summary>
1075 /// Cleanup the things we use in the scene.
1076 /// </summary>
1077 public void Destroy()
1078 {
1079 AddChange(changes.Remove, null);
1080 }
1081
1082 public override void CrossingFailure()
1083 {
1084 }
1085
1086 public override Vector3 PIDTarget { set { return; } }
1087 public override bool PIDActive { set { return; } }
1088 public override float PIDTau { set { return; } }
1089
1090 public override float PIDHoverHeight { set { return; } }
1091 public override bool PIDHoverActive { set { return; } }
1092 public override PIDHoverType PIDHoverType { set { return; } }
1093 public override float PIDHoverTau { set { return; } }
1094
1095 public override Quaternion APIDTarget { set { return; } }
1096
1097 public override bool APIDActive { set { return; } }
1098
1099 public override float APIDStrength { set { return; } }
1100
1101 public override float APIDDamping { set { return; } }
1102
1103
1104 public override void SubscribeEvents(int ms)
1105 {
1106 m_eventsubscription = ms;
1107 m_cureventsubscription = 0;
1108 if (CollisionEventsThisFrame == null)
1109 CollisionEventsThisFrame = new CollisionEventUpdate();
1110 SentEmptyCollisionsEvent = false;
1111 }
1112
1113 public override void UnSubscribeEvents()
1114 {
1115 if (CollisionEventsThisFrame != null)
1116 {
1117 CollisionEventsThisFrame.Clear();
1118 CollisionEventsThisFrame = null;
1119 }
1120 m_eventsubscription = 0;
1121 }
1122
1123 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1124 {
1125 if (CollisionEventsThisFrame == null)
1126 CollisionEventsThisFrame = new CollisionEventUpdate();
1127 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1128 _parent_scene.AddCollisionEventReporting(this);
1129 }
1130
1131 public void SendCollisions()
1132 {
1133 if (CollisionEventsThisFrame == null)
1134 return;
1135
1136 if (m_cureventsubscription < m_eventsubscription)
1137 return;
1138
1139 m_cureventsubscription = 0;
1140
1141 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1142
1143 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1144 {
1145 base.SendCollisionUpdate(CollisionEventsThisFrame);
1146
1147 if (ncolisions == 0)
1148 {
1149 SentEmptyCollisionsEvent = true;
1150 _parent_scene.RemoveCollisionEventReporting(this);
1151 }
1152 else
1153 {
1154 SentEmptyCollisionsEvent = false;
1155 CollisionEventsThisFrame.Clear();
1156 }
1157 }
1158 }
1159
1160 internal void AddCollisionFrameTime(int t)
1161 {
1162 // protect it from overflow crashing
1163 if (m_cureventsubscription < 50000)
1164 m_cureventsubscription += t;
1165 }
1166
1167 public override bool SubscribedEvents()
1168 {
1169 if (m_eventsubscription > 0)
1170 return true;
1171 return false;
1172 }
1173
1174 private void changePhysicsStatus(bool NewStatus)
1175 {
1176 if (NewStatus != m_isPhysical)
1177 {
1178 if (NewStatus)
1179 {
1180 // Create avatar capsule and related ODE data
1181 if ((Shell != IntPtr.Zero))
1182 {
1183 // a lost shell ?
1184 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1185 + (Shell != IntPtr.Zero ? "Shell " : "")
1186 + (Body != IntPtr.Zero ? "Body " : "")
1187 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1188 AvatarGeomAndBodyDestroy();
1189 }
1190
1191 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1192
1193 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1194 _parent_scene.AddCharacter(this);
1195 }
1196 else
1197 {
1198 _parent_scene.RemoveCollisionEventReporting(this);
1199 _parent_scene.RemoveCharacter(this);
1200 // destroy avatar capsule and related ODE data
1201 AvatarGeomAndBodyDestroy();
1202 }
1203 m_freemove = false;
1204 m_isPhysical = NewStatus;
1205 }
1206 }
1207
1208 private void changeAdd()
1209 {
1210 changePhysicsStatus(true);
1211 }
1212
1213 private void changeRemove()
1214 {
1215 changePhysicsStatus(false);
1216 }
1217
1218 private void changeShape(PrimitiveBaseShape arg)
1219 {
1220 }
1221
1222 private void changeSize(Vector3 Size)
1223 {
1224 if (Size.IsFinite())
1225 {
1226 float caplen = Size.Z;
1227
1228 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1229
1230 if (caplen != CAPSULE_LENGTH)
1231 {
1232 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1233 {
1234 AvatarGeomAndBodyDestroy();
1235
1236 float prevCapsule = CAPSULE_LENGTH;
1237 CAPSULE_LENGTH = caplen;
1238
1239 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1240 _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f);
1241
1242 Velocity = Vector3.Zero;
1243
1244 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1245 }
1246 else
1247 {
1248 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1249 + (Shell == IntPtr.Zero ? "Shell " : "")
1250 + (Body == IntPtr.Zero ? "Body " : "")
1251 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1252 }
1253 }
1254 m_freemove = false;
1255 m_pidControllerActive = true;
1256 }
1257 else
1258 {
1259 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1260 }
1261 }
1262
1263 private void changePosition( Vector3 newPos)
1264 {
1265 if (Body != IntPtr.Zero)
1266 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1267 _position = newPos;
1268 m_freemove = false;
1269 m_pidControllerActive = true;
1270 }
1271
1272 private void changeOrientation(Quaternion newOri)
1273 {
1274 }
1275
1276 private void changeVelocity(Vector3 newVel)
1277 {
1278 m_pidControllerActive = true;
1279 m_freemove = false;
1280 _target_velocity = newVel;
1281 }
1282
1283 private void changeSetTorque(Vector3 newTorque)
1284 {
1285 }
1286
1287 private void changeAddForce(Vector3 newForce)
1288 {
1289 }
1290
1291 private void changeAddAngularForce(Vector3 arg)
1292 {
1293 }
1294
1295 private void changeAngularLock(Vector3 arg)
1296 {
1297 }
1298
1299 private void changeFloatOnWater(bool arg)
1300 {
1301 }
1302
1303 private void changeVolumedetetion(bool arg)
1304 {
1305 }
1306
1307 private void changeSelectedStatus(bool arg)
1308 {
1309 }
1310
1311 private void changeDisable(bool arg)
1312 {
1313 }
1314
1315 private void changeBuilding(bool arg)
1316 {
1317 }
1318
1319 private void setFreeMove()
1320 {
1321 m_pidControllerActive = true;
1322 _zeroFlag = false;
1323 _target_velocity = Vector3.Zero;
1324 m_freemove = true;
1325 m_colliderfilter = -2;
1326 m_colliderObjectfilter = -2;
1327 m_colliderGroundfilter = -2;
1328
1329 m_iscolliding = false;
1330 m_iscollidingGround = false;
1331 m_iscollidingObj = false;
1332
1333 CollisionEventsThisFrame.Clear();
1334 }
1335
1336 private void changeForce(Vector3 newForce)
1337 {
1338 setFreeMove();
1339
1340 if (Body != IntPtr.Zero)
1341 {
1342 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1343 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1344 }
1345 }
1346
1347 // for now momentum is actually velocity
1348 private void changeMomentum(Vector3 newmomentum)
1349 {
1350 _velocity = newmomentum;
1351 setFreeMove();
1352
1353 if (Body != IntPtr.Zero)
1354 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1355 }
1356
1357 private void donullchange()
1358 {
1359 }
1360
1361 public bool DoAChange(changes what, object arg)
1362 {
1363 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1364 {
1365 return false;
1366 }
1367
1368 // nasty switch
1369 switch (what)
1370 {
1371 case changes.Add:
1372 changeAdd();
1373 break;
1374 case changes.Remove:
1375 changeRemove();
1376 break;
1377
1378 case changes.Position:
1379 changePosition((Vector3)arg);
1380 break;
1381
1382 case changes.Orientation:
1383 changeOrientation((Quaternion)arg);
1384 break;
1385
1386 case changes.PosOffset:
1387 donullchange();
1388 break;
1389
1390 case changes.OriOffset:
1391 donullchange();
1392 break;
1393
1394 case changes.Velocity:
1395 changeVelocity((Vector3)arg);
1396 break;
1397
1398 // case changes.Acceleration:
1399 // changeacceleration((Vector3)arg);
1400 // break;
1401 // case changes.AngVelocity:
1402 // changeangvelocity((Vector3)arg);
1403 // break;
1404
1405 case changes.Force:
1406 changeForce((Vector3)arg);
1407 break;
1408
1409 case changes.Torque:
1410 changeSetTorque((Vector3)arg);
1411 break;
1412
1413 case changes.AddForce:
1414 changeAddForce((Vector3)arg);
1415 break;
1416
1417 case changes.AddAngForce:
1418 changeAddAngularForce((Vector3)arg);
1419 break;
1420
1421 case changes.AngLock:
1422 changeAngularLock((Vector3)arg);
1423 break;
1424
1425 case changes.Size:
1426 changeSize((Vector3)arg);
1427 break;
1428
1429 case changes.Momentum:
1430 changeMomentum((Vector3)arg);
1431 break;
1432/* not in use for now
1433 case changes.Shape:
1434 changeShape((PrimitiveBaseShape)arg);
1435 break;
1436
1437 case changes.CollidesWater:
1438 changeFloatOnWater((bool)arg);
1439 break;
1440
1441 case changes.VolumeDtc:
1442 changeVolumedetetion((bool)arg);
1443 break;
1444
1445 case changes.Physical:
1446 changePhysicsStatus((bool)arg);
1447 break;
1448
1449 case changes.Selected:
1450 changeSelectedStatus((bool)arg);
1451 break;
1452
1453 case changes.disabled:
1454 changeDisable((bool)arg);
1455 break;
1456
1457 case changes.building:
1458 changeBuilding((bool)arg);
1459 break;
1460*/
1461 case changes.Null:
1462 donullchange();
1463 break;
1464
1465 default:
1466 donullchange();
1467 break;
1468 }
1469 return false;
1470 }
1471
1472 public void AddChange(changes what, object arg)
1473 {
1474 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1475 }
1476 }
1477}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..a7dda7a
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1087 @@
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
41// Extensive change Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager;
52
53namespace OpenSim.Region.Physics.OdePlugin
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private OdeScene _pParentScene;
64
65 // Vehicle properties
66 // WARNING this are working copies for internel use
67 // their values may not be the corresponding parameter
68
69 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71
72 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73
74 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75 // HOVER_TERRAIN_ONLY
76 // HOVER_GLOBAL_HEIGHT
77 // NO_DEFLECTION_UP
78 // HOVER_WATER_ONLY
79 // HOVER_UP_ONLY
80 // LIMIT_MOTOR_UP
81 // LIMIT_ROLL_ONLY
82 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83
84 // Linear properties
85 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87 private float m_linearMotorDecayTimescale = 120;
88 private float m_linearMotorTimescale = 1000;
89 private Vector3 m_linearMotorOffset = Vector3.Zero;
90
91 //Angular properties
92 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96
97 //Deflection properties
98 private float m_angularDeflectionEfficiency = 0;
99 private float m_angularDeflectionTimescale = 1000;
100 private float m_linearDeflectionEfficiency = 0;
101 private float m_linearDeflectionTimescale = 1000;
102
103 //Banking properties
104 private float m_bankingEfficiency = 0;
105 private float m_bankingMix = 0;
106 private float m_bankingTimescale = 1000;
107
108 //Hover and Buoyancy properties
109 private float m_VhoverHeight = 0f;
110 private float m_VhoverEfficiency = 0f;
111 private float m_VhoverTimescale = 1000f;
112 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116
117 //Attractor properties
118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120
121
122 // auxiliar
123 private float m_lmEfect = 0f; // current linear motor eficiency
124 private float m_lmDecay = 0f; // current linear decay
125
126 private float m_amEfect = 0; // current angular motor eficiency
127 private float m_amDecay = 0f; // current linear decay
128
129 private float m_ffactor = 1.0f;
130
131 private float m_timestep = 0.02f;
132 private float m_invtimestep = 50;
133
134
135 float m_ampwr;
136 float m_amdampX;
137 float m_amdampY;
138 float m_amdampZ;
139
140
141 public float FrictionFactor
142 {
143 get
144 {
145 return m_ffactor;
146 }
147 }
148
149
150 public ODEDynamics(OdePrim rootp)
151 {
152 rootPrim = rootp;
153 _pParentScene = rootPrim._parent_scene;
154 m_timestep = _pParentScene.ODE_STEPSIZE;
155 m_invtimestep = 1.0f / m_timestep;
156 }
157
158 public void DoSetVehicle(VehicleData vd)
159 {
160 m_type = vd.m_type;
161 m_flags = vd.m_flags;
162
163
164 // Linear properties
165 m_linearMotorDirection = vd.m_linearMotorDirection;
166
167 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
168 if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
169 if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
170 if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
171
172 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
173 if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
174 m_linearMotorDecayTimescale += 0.2f;
175 m_linearMotorDecayTimescale *= m_invtimestep;
176
177 m_linearMotorTimescale = vd.m_linearMotorTimescale;
178 if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
179
180 m_linearMotorOffset = vd.m_linearMotorOffset;
181
182 //Angular properties
183 m_angularMotorDirection = vd.m_angularMotorDirection;
184 m_angularMotorTimescale = vd.m_angularMotorTimescale;
185 if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
186
187 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
188 if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
189 m_angularMotorDecayTimescale *= m_invtimestep;
190
191 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
192 if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
193 if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
194 if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
195
196 //Deflection properties
197 m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
198 m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
199 if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
200
201 m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
202 m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
203 if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
204
205 //Banking properties
206 m_bankingEfficiency = vd.m_bankingEfficiency;
207 m_bankingMix = vd.m_bankingMix;
208 m_bankingTimescale = vd.m_bankingTimescale;
209 if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
210
211 //Hover and Buoyancy properties
212 m_VhoverHeight = vd.m_VhoverHeight;
213 m_VhoverEfficiency = vd.m_VhoverEfficiency;
214 m_VhoverTimescale = vd.m_VhoverTimescale;
215 if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
216
217 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
218
219 //Attractor properties
220 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
221 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
222 if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
223
224 // Axis
225 m_referenceFrame = vd.m_referenceFrame;
226
227 m_lmEfect = 0;
228 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
229 m_amEfect = 0;
230 m_ffactor = 1.0f;
231 }
232
233 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
234 {
235 float len;
236
237 switch (pParam)
238 {
239 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
240 if (pValue < 0f) pValue = 0f;
241 if (pValue > 1f) pValue = 1f;
242 m_angularDeflectionEfficiency = pValue;
243 break;
244 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
245 if (pValue < m_timestep) pValue = m_timestep;
246 m_angularDeflectionTimescale = pValue;
247 break;
248 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
249 if (pValue < m_timestep) pValue = m_timestep;
250 else if (pValue > 120) pValue = 120;
251 m_angularMotorDecayTimescale = pValue * m_invtimestep;
252 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
253 break;
254 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
255 if (pValue < m_timestep) pValue = m_timestep;
256 m_angularMotorTimescale = pValue;
257 break;
258 case Vehicle.BANKING_EFFICIENCY:
259 if (pValue < -1f) pValue = -1f;
260 if (pValue > 1f) pValue = 1f;
261 m_bankingEfficiency = pValue;
262 break;
263 case Vehicle.BANKING_MIX:
264 if (pValue < 0f) pValue = 0f;
265 if (pValue > 1f) pValue = 1f;
266 m_bankingMix = pValue;
267 break;
268 case Vehicle.BANKING_TIMESCALE:
269 if (pValue < m_timestep) pValue = m_timestep;
270 m_bankingTimescale = pValue;
271 break;
272 case Vehicle.BUOYANCY:
273 if (pValue < -1f) pValue = -1f;
274 if (pValue > 1f) pValue = 1f;
275 m_VehicleBuoyancy = pValue;
276 break;
277 case Vehicle.HOVER_EFFICIENCY:
278 if (pValue < 0f) pValue = 0f;
279 if (pValue > 1f) pValue = 1f;
280 m_VhoverEfficiency = pValue;
281 break;
282 case Vehicle.HOVER_HEIGHT:
283 m_VhoverHeight = pValue;
284 break;
285 case Vehicle.HOVER_TIMESCALE:
286 if (pValue < m_timestep) pValue = m_timestep;
287 m_VhoverTimescale = pValue;
288 break;
289 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
290 if (pValue < 0f) pValue = 0f;
291 if (pValue > 1f) pValue = 1f;
292 m_linearDeflectionEfficiency = pValue;
293 break;
294 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
295 if (pValue < m_timestep) pValue = m_timestep;
296 m_linearDeflectionTimescale = pValue;
297 break;
298 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
299 if (pValue < m_timestep) pValue = m_timestep;
300 else if (pValue > 120) pValue = 120;
301 m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
302 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
303 break;
304 case Vehicle.LINEAR_MOTOR_TIMESCALE:
305 if (pValue < m_timestep) pValue = m_timestep;
306 m_linearMotorTimescale = pValue;
307 break;
308 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
309 if (pValue < 0f) pValue = 0f;
310 if (pValue > 1f) pValue = 1f;
311 m_verticalAttractionEfficiency = pValue;
312 break;
313 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
314 if (pValue < m_timestep) pValue = m_timestep;
315 m_verticalAttractionTimescale = pValue;
316 break;
317
318 // These are vector properties but the engine lets you use a single float value to
319 // set all of the components to the same value
320 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
321 if (pValue < m_timestep) pValue = m_timestep;
322 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
323 break;
324 case Vehicle.ANGULAR_MOTOR_DIRECTION:
325 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
326 len = m_angularMotorDirection.Length();
327 if (len > 12.566f)
328 m_angularMotorDirection *= (12.566f / len);
329
330 m_amEfect = 1.0f ; // turn it on
331 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
332
333 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
334 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
335 d.BodyEnable(rootPrim.Body);
336 break;
337 case Vehicle.LINEAR_FRICTION_TIMESCALE:
338 if (pValue < m_timestep) pValue = m_timestep;
339 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
340 break;
341 case Vehicle.LINEAR_MOTOR_DIRECTION:
342 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
343 len = m_linearMotorDirection.Length();
344 if (len > 100.0f)
345 m_linearMotorDirection *= (100.0f / len);
346
347 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
348 m_lmEfect = 1.0f; // turn it on
349
350 m_ffactor = 0.0f;
351 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
352 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
353 d.BodyEnable(rootPrim.Body);
354 break;
355 case Vehicle.LINEAR_MOTOR_OFFSET:
356 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
357 len = m_linearMotorOffset.Length();
358 if (len > 100.0f)
359 m_linearMotorOffset *= (100.0f / len);
360 break;
361 }
362 }//end ProcessFloatVehicleParam
363
364 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
365 {
366 float len;
367
368 switch (pParam)
369 {
370 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
371 if (pValue.X < m_timestep) pValue.X = m_timestep;
372 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
373 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
374
375 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
376 break;
377 case Vehicle.ANGULAR_MOTOR_DIRECTION:
378 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
379 // Limit requested angular speed to 2 rps= 4 pi rads/sec
380 len = m_angularMotorDirection.Length();
381 if (len > 12.566f)
382 m_angularMotorDirection *= (12.566f / len);
383
384 m_amEfect = 1.0f; // turn it on
385 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
386
387 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
388 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
389 d.BodyEnable(rootPrim.Body);
390 break;
391 case Vehicle.LINEAR_FRICTION_TIMESCALE:
392 if (pValue.X < m_timestep) pValue.X = m_timestep;
393 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
394 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
395 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
396 break;
397 case Vehicle.LINEAR_MOTOR_DIRECTION:
398 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
399 len = m_linearMotorDirection.Length();
400 if (len > 100.0f)
401 m_linearMotorDirection *= (100.0f / len);
402
403 m_lmEfect = 1.0f; // turn it on
404 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
405
406 m_ffactor = 0.0f;
407 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
408 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
409 d.BodyEnable(rootPrim.Body);
410 break;
411 case Vehicle.LINEAR_MOTOR_OFFSET:
412 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
413 len = m_linearMotorOffset.Length();
414 if (len > 100.0f)
415 m_linearMotorOffset *= (100.0f / len);
416 break;
417 case Vehicle.BLOCK_EXIT:
418 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
419 break;
420 }
421 }//end ProcessVectorVehicleParam
422
423 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
424 {
425 switch (pParam)
426 {
427 case Vehicle.REFERENCE_FRAME:
428 // m_referenceFrame = Quaternion.Inverse(pValue);
429 m_referenceFrame = pValue;
430 break;
431 case Vehicle.ROLL_FRAME:
432 m_RollreferenceFrame = pValue;
433 break;
434 }
435 }//end ProcessRotationVehicleParam
436
437 internal void ProcessVehicleFlags(int pParam, bool remove)
438 {
439 if (remove)
440 {
441 m_flags &= ~((VehicleFlag)pParam);
442 }
443 else
444 {
445 m_flags |= (VehicleFlag)pParam;
446 }
447 }//end ProcessVehicleFlags
448
449 internal void ProcessTypeChange(Vehicle pType)
450 {
451 m_lmEfect = 0;
452
453 m_amEfect = 0;
454 m_ffactor = 1f;
455
456 m_linearMotorDirection = Vector3.Zero;
457 m_angularMotorDirection = Vector3.Zero;
458
459 m_BlockingEndPoint = Vector3.Zero;
460 m_RollreferenceFrame = Quaternion.Identity;
461 m_linearMotorOffset = Vector3.Zero;
462
463 m_referenceFrame = Quaternion.Identity;
464
465 // Set Defaults For Type
466 m_type = pType;
467 switch (pType)
468 {
469 case Vehicle.TYPE_NONE:
470 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
471 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
472 m_linearMotorTimescale = 1000;
473 m_linearMotorDecayTimescale = 120 * m_invtimestep;
474 m_angularMotorTimescale = 1000;
475 m_angularMotorDecayTimescale = 1000 * m_invtimestep;
476 m_VhoverHeight = 0;
477 m_VhoverEfficiency = 1;
478 m_VhoverTimescale = 1000;
479 m_VehicleBuoyancy = 0;
480 m_linearDeflectionEfficiency = 0;
481 m_linearDeflectionTimescale = 1000;
482 m_angularDeflectionEfficiency = 0;
483 m_angularDeflectionTimescale = 1000;
484 m_bankingEfficiency = 0;
485 m_bankingMix = 1;
486 m_bankingTimescale = 1000;
487 m_verticalAttractionEfficiency = 0;
488 m_verticalAttractionTimescale = 1000;
489
490 m_flags = (VehicleFlag)0;
491 break;
492
493 case Vehicle.TYPE_SLED:
494 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
495 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
496 m_linearMotorTimescale = 1000;
497 m_linearMotorDecayTimescale = 120 * m_invtimestep;
498 m_angularMotorTimescale = 1000;
499 m_angularMotorDecayTimescale = 120 * m_invtimestep;
500 m_VhoverHeight = 0;
501 m_VhoverEfficiency = 1;
502 m_VhoverTimescale = 10;
503 m_VehicleBuoyancy = 0;
504 m_linearDeflectionEfficiency = 1;
505 m_linearDeflectionTimescale = 1;
506 m_angularDeflectionEfficiency = 0;
507 m_angularDeflectionTimescale = 10;
508 m_verticalAttractionEfficiency = 1;
509 m_verticalAttractionTimescale = 1000;
510 m_bankingEfficiency = 0;
511 m_bankingMix = 1;
512 m_bankingTimescale = 10;
513 m_flags &=
514 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
515 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
516 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
517 VehicleFlag.LIMIT_ROLL_ONLY |
518 VehicleFlag.LIMIT_MOTOR_UP);
519 break;
520
521 case Vehicle.TYPE_CAR:
522 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
523 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
524 m_linearMotorTimescale = 1;
525 m_linearMotorDecayTimescale = 60 * m_invtimestep;
526 m_angularMotorTimescale = 1;
527 m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
528 m_VhoverHeight = 0;
529 m_VhoverEfficiency = 0;
530 m_VhoverTimescale = 1000;
531 m_VehicleBuoyancy = 0;
532 m_linearDeflectionEfficiency = 1;
533 m_linearDeflectionTimescale = 2;
534 m_angularDeflectionEfficiency = 0;
535 m_angularDeflectionTimescale = 10;
536 m_verticalAttractionEfficiency = 1f;
537 m_verticalAttractionTimescale = 10f;
538 m_bankingEfficiency = -0.2f;
539 m_bankingMix = 1;
540 m_bankingTimescale = 1;
541 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
542 VehicleFlag.HOVER_TERRAIN_ONLY |
543 VehicleFlag.HOVER_GLOBAL_HEIGHT);
544 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
545 VehicleFlag.LIMIT_ROLL_ONLY |
546 VehicleFlag.LIMIT_MOTOR_UP |
547 VehicleFlag.HOVER_UP_ONLY);
548 break;
549 case Vehicle.TYPE_BOAT:
550 m_linearFrictionTimescale = new Vector3(10, 3, 2);
551 m_angularFrictionTimescale = new Vector3(10, 10, 10);
552 m_linearMotorTimescale = 5;
553 m_linearMotorDecayTimescale = 60 * m_invtimestep;
554 m_angularMotorTimescale = 4;
555 m_angularMotorDecayTimescale = 4 * m_invtimestep;
556 m_VhoverHeight = 0;
557 m_VhoverEfficiency = 0.5f;
558 m_VhoverTimescale = 2;
559 m_VehicleBuoyancy = 1;
560 m_linearDeflectionEfficiency = 0.5f;
561 m_linearDeflectionTimescale = 3;
562 m_angularDeflectionEfficiency = 0.5f;
563 m_angularDeflectionTimescale = 5;
564 m_verticalAttractionEfficiency = 0.5f;
565 m_verticalAttractionTimescale = 5f;
566 m_bankingEfficiency = -0.3f;
567 m_bankingMix = 0.8f;
568 m_bankingTimescale = 1;
569 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
570 VehicleFlag.HOVER_GLOBAL_HEIGHT |
571 VehicleFlag.HOVER_UP_ONLY); // |
572// VehicleFlag.LIMIT_ROLL_ONLY);
573 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
574 VehicleFlag.LIMIT_MOTOR_UP |
575 VehicleFlag.HOVER_UP_ONLY | // new sl
576 VehicleFlag.HOVER_WATER_ONLY);
577 break;
578
579 case Vehicle.TYPE_AIRPLANE:
580 m_linearFrictionTimescale = new Vector3(200, 10, 5);
581 m_angularFrictionTimescale = new Vector3(20, 20, 20);
582 m_linearMotorTimescale = 2;
583 m_linearMotorDecayTimescale = 60 * m_invtimestep;
584 m_angularMotorTimescale = 4;
585 m_angularMotorDecayTimescale = 8 * m_invtimestep;
586 m_VhoverHeight = 0;
587 m_VhoverEfficiency = 0.5f;
588 m_VhoverTimescale = 1000;
589 m_VehicleBuoyancy = 0;
590 m_linearDeflectionEfficiency = 0.5f;
591 m_linearDeflectionTimescale = 0.5f;
592 m_angularDeflectionEfficiency = 1;
593 m_angularDeflectionTimescale = 2;
594 m_verticalAttractionEfficiency = 0.9f;
595 m_verticalAttractionTimescale = 2f;
596 m_bankingEfficiency = 1;
597 m_bankingMix = 0.7f;
598 m_bankingTimescale = 2;
599 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
600 VehicleFlag.HOVER_TERRAIN_ONLY |
601 VehicleFlag.HOVER_GLOBAL_HEIGHT |
602 VehicleFlag.HOVER_UP_ONLY |
603 VehicleFlag.NO_DEFLECTION_UP |
604 VehicleFlag.LIMIT_MOTOR_UP);
605 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
606 break;
607
608 case Vehicle.TYPE_BALLOON:
609 m_linearFrictionTimescale = new Vector3(5, 5, 5);
610 m_angularFrictionTimescale = new Vector3(10, 10, 10);
611 m_linearMotorTimescale = 5;
612 m_linearMotorDecayTimescale = 60 * m_invtimestep;
613 m_angularMotorTimescale = 6;
614 m_angularMotorDecayTimescale = 10 * m_invtimestep;
615 m_VhoverHeight = 5;
616 m_VhoverEfficiency = 0.8f;
617 m_VhoverTimescale = 10;
618 m_VehicleBuoyancy = 1;
619 m_linearDeflectionEfficiency = 0;
620 m_linearDeflectionTimescale = 5 * m_invtimestep;
621 m_angularDeflectionEfficiency = 0;
622 m_angularDeflectionTimescale = 5;
623 m_verticalAttractionEfficiency = 1f;
624 m_verticalAttractionTimescale = 1000f;
625 m_bankingEfficiency = 0;
626 m_bankingMix = 0.7f;
627 m_bankingTimescale = 5;
628 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
629 VehicleFlag.HOVER_TERRAIN_ONLY |
630 VehicleFlag.HOVER_UP_ONLY |
631 VehicleFlag.NO_DEFLECTION_UP |
632 VehicleFlag.LIMIT_MOTOR_UP | //);
633 VehicleFlag.LIMIT_ROLL_ONLY | // new sl
634 VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
635
636// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
637// VehicleFlag.HOVER_GLOBAL_HEIGHT);
638 break;
639
640 }
641
642 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
643 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
644
645 }//end SetDefaultsForType
646
647 internal void Stop()
648 {
649 m_lmEfect = 0;
650 m_lmDecay = 0f;
651 m_amEfect = 0;
652 m_amDecay = 0;
653 m_ffactor = 1f;
654 }
655
656 public static Vector3 Xrot(Quaternion rot)
657 {
658 Vector3 vec;
659 rot.Normalize(); // just in case
660 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
661 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
662 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
663 return vec;
664 }
665
666 public static Vector3 Zrot(Quaternion rot)
667 {
668 Vector3 vec;
669 rot.Normalize(); // just in case
670 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
671 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
672 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
673
674 return vec;
675 }
676
677 private const float pi = (float)Math.PI;
678 private const float halfpi = 0.5f * (float)Math.PI;
679 private const float twopi = 2.0f * pi;
680
681 public static Vector3 ubitRot2Euler(Quaternion rot)
682 {
683 // returns roll in X
684 // pitch in Y
685 // yaw in Z
686 Vector3 vec;
687
688 // assuming rot is normalised
689 // rot.Normalize();
690
691 float zX = rot.X * rot.Z + rot.Y * rot.W;
692
693 if (zX < -0.49999f)
694 {
695 vec.X = 0;
696 vec.Y = -halfpi;
697 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
698 }
699 else if (zX > 0.49999f)
700 {
701 vec.X = 0;
702 vec.Y = halfpi;
703 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
704 }
705 else
706 {
707 vec.Y = (float)Math.Asin(2 * zX);
708
709 float sqw = rot.W * rot.W;
710
711 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
712 float zZ = rot.Z * rot.Z + sqw - 0.5f;
713
714 vec.X = (float)Math.Atan2(minuszY, zZ);
715
716 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
717 float yY = rot.X * rot.X + sqw - 0.5f;
718 vec.Z = (float)Math.Atan2(yX, yY);
719 }
720 return vec;
721 }
722
723 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
724 {
725 // assuming rot is normalised
726 // rot.Normalize();
727
728 float zX = rot.X * rot.Z + rot.Y * rot.W;
729
730 if (zX < -0.49999f)
731 {
732 roll = 0;
733 pitch = -halfpi;
734 }
735 else if (zX > 0.49999f)
736 {
737 roll = 0;
738 pitch = halfpi;
739 }
740 else
741 {
742 pitch = (float)Math.Asin(2 * zX);
743
744 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
745 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
746
747 roll = (float)Math.Atan2(minuszY, zZ);
748 }
749 return ;
750 }
751
752 internal void Step()
753 {
754 IntPtr Body = rootPrim.Body;
755
756 d.Mass dmass;
757 d.BodyGetMass(Body, out dmass);
758
759 d.Quaternion rot = d.BodyGetQuaternion(Body);
760 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
761 Quaternion rotq = objrotq; // rotq = rotation of object
762 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
763 Quaternion irotq = Quaternion.Inverse(rotq);
764
765 d.Vector3 dvtmp;
766 Vector3 tmpV;
767 Vector3 curVel; // velocity in world
768 Vector3 curAngVel; // angular velocity in world
769 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
770 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
771 d.Vector3 dtorque = new d.Vector3();
772
773 dvtmp = d.BodyGetLinearVel(Body);
774 curVel.X = dvtmp.X;
775 curVel.Y = dvtmp.Y;
776 curVel.Z = dvtmp.Z;
777 Vector3 curLocalVel = curVel * irotq; // current velocity in local
778
779 dvtmp = d.BodyGetAngularVel(Body);
780 curAngVel.X = dvtmp.X;
781 curAngVel.Y = dvtmp.Y;
782 curAngVel.Z = dvtmp.Z;
783 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
784
785 float ldampZ = 0;
786
787 // linear motor
788 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
789 {
790 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
791 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
792 tmpV *= rotq; // to world
793
794 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
795 tmpV.Z = 0;
796
797 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
798 {
799 // have offset, do it now
800 tmpV *= dmass.mass;
801 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
802 }
803 else
804 {
805 force.X += tmpV.X;
806 force.Y += tmpV.Y;
807 force.Z += tmpV.Z;
808 }
809
810 m_lmEfect *= m_lmDecay;
811// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
812 m_ffactor = 0.0f;
813 }
814 else
815 {
816 m_lmEfect = 0;
817 m_ffactor = 1f;
818 }
819
820 // hover
821 if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
822 {
823 // d.Vector3 pos = d.BodyGetPosition(Body);
824 d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
825 pos.Z -= 0.21f; // minor offset that seems to be always there in sl
826
827 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
828 float perr;
829
830 // default to global but don't go underground
831 perr = m_VhoverHeight - pos.Z;
832
833 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
834 {
835 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
836 {
837 perr += _pParentScene.GetWaterLevel();
838 }
839 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
840 {
841 perr += t;
842 }
843 else
844 {
845 float w = _pParentScene.GetWaterLevel();
846 if (t > w)
847 perr += t;
848 else
849 perr += w;
850 }
851 }
852 else if (t > m_VhoverHeight)
853 perr = t - pos.Z; ;
854
855 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
856 {
857 ldampZ = m_VhoverEfficiency * m_invtimestep;
858
859 perr *= (1.0f + ldampZ) / m_VhoverTimescale;
860
861 // force.Z += perr - curVel.Z * tmp;
862 force.Z += perr;
863 ldampZ *= -curVel.Z;
864
865 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
866 }
867 else // no buoyancy
868 force.Z += _pParentScene.gravityz;
869 }
870 else
871 {
872 // default gravity and Buoyancy
873 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
874 }
875
876 // linear deflection
877 if (m_linearDeflectionEfficiency > 0)
878 {
879 float len = curVel.Length();
880 if (len > 0.01) // if moving
881 {
882 Vector3 atAxis;
883 atAxis = Xrot(rotq); // where are we pointing to
884 atAxis *= len; // make it same size as world velocity vector
885
886 tmpV = -atAxis; // oposite direction
887 atAxis -= curVel; // error to one direction
888 len = atAxis.LengthSquared();
889
890 tmpV -= curVel; // error to oposite
891 float lens = tmpV.LengthSquared();
892
893 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
894 {
895 if (len < lens)
896 tmpV = atAxis;
897
898 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
899 force.X += tmpV.X;
900 force.Y += tmpV.Y;
901 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
902 force.Z += tmpV.Z;
903 }
904 }
905 }
906
907 // linear friction/damping
908 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
909 {
910 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
911 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
912 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
913 tmpV *= rotq; // to world
914
915 if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
916 tmpV.Z = ldampZ;
917 force.X += tmpV.X;
918 force.Y += tmpV.Y;
919 force.Z += tmpV.Z;
920 }
921
922 // vertical atractor
923 if (m_verticalAttractionTimescale < 300)
924 {
925 float roll;
926 float pitch;
927
928
929
930 float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
931
932 float ftmp2;
933 ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
934 m_amdampX = ftmp2;
935
936 m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
937
938 GetRollPitch(irotq, out roll, out pitch);
939
940 if (roll > halfpi)
941 roll = pi - roll;
942 else if (roll < -halfpi)
943 roll = -pi - roll;
944
945 float effroll = pitch / halfpi;
946 effroll *= effroll;
947 effroll = 1 - effroll;
948 effroll *= roll;
949
950
951 torque.X += effroll * ftmp;
952
953 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
954 {
955 float effpitch = roll / halfpi;
956 effpitch *= effpitch;
957 effpitch = 1 - effpitch;
958 effpitch *= pitch;
959
960 torque.Y += effpitch * ftmp;
961 }
962
963 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
964 {
965
966 float broll = effroll;
967 /*
968 if (broll > halfpi)
969 broll = pi - broll;
970 else if (broll < -halfpi)
971 broll = -pi - broll;
972 */
973 broll *= m_bankingEfficiency;
974 if (m_bankingMix != 0)
975 {
976 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
977 if (vfact > 1.0f) vfact = 1.0f;
978
979 if (curLocalVel.X >= 0)
980 broll *= (1 + (vfact - 1) * m_bankingMix);
981 else
982 broll *= -(1 + (vfact - 1) * m_bankingMix);
983 }
984 // make z rot be in world Z not local as seems to be in sl
985
986 broll = broll / m_bankingTimescale;
987
988
989 tmpV = Zrot(irotq);
990 tmpV *= broll;
991
992 torque.X += tmpV.X;
993 torque.Y += tmpV.Y;
994 torque.Z += tmpV.Z;
995
996 m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
997 m_amdampY = m_amdampZ;
998
999 }
1000 else
1001 {
1002 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1003 m_amdampY = m_amdampX;
1004 }
1005 }
1006 else
1007 {
1008 m_ampwr = 1.0f;
1009 m_amdampX = 1 / m_angularFrictionTimescale.X;
1010 m_amdampY = 1 / m_angularFrictionTimescale.Y;
1011 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1012 }
1013
1014 // angular motor
1015 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
1016 {
1017 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
1018 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
1019 torque.X += tmpV.X * m_ampwr;
1020 torque.Y += tmpV.Y * m_ampwr;
1021 torque.Z += tmpV.Z;
1022
1023 m_amEfect *= m_amDecay;
1024 }
1025 else
1026 m_amEfect = 0;
1027
1028 // angular deflection
1029 if (m_angularDeflectionEfficiency > 0)
1030 {
1031 Vector3 dirv;
1032
1033 if (curLocalVel.X > 0.01f)
1034 dirv = curLocalVel;
1035 else if (curLocalVel.X < -0.01f)
1036 // use oposite
1037 dirv = -curLocalVel;
1038 else
1039 {
1040 // make it fall into small positive x case
1041 dirv.X = 0.01f;
1042 dirv.Y = curLocalVel.Y;
1043 dirv.Z = curLocalVel.Z;
1044 }
1045
1046 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1047
1048 if (Math.Abs(dirv.Z) > 0.01)
1049 {
1050 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
1051 }
1052
1053 if (Math.Abs(dirv.Y) > 0.01)
1054 {
1055 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
1056 }
1057 }
1058
1059 // angular friction
1060 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1061 {
1062 torque.X -= curLocalAngVel.X * m_amdampX;
1063 torque.Y -= curLocalAngVel.Y * m_amdampY;
1064 torque.Z -= curLocalAngVel.Z * m_amdampZ;
1065 }
1066
1067
1068
1069 if (force.X != 0 || force.Y != 0 || force.Z != 0)
1070 {
1071 force *= dmass.mass;
1072 d.BodyAddForce(Body, force.X, force.Y, force.Z);
1073 }
1074
1075 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1076 {
1077 torque *= m_referenceFrame; // to object frame
1078 dtorque.X = torque.X ;
1079 dtorque.Y = torque.Y;
1080 dtorque.Z = torque.Z;
1081
1082 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
1083 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1084 }
1085 }
1086 }
1087}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..6bf5be1
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,4032 @@
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/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
33/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
36 * ODEPrim.cs contains methods dealing with Prim editing, Prim
37 * characteristics and Kinetic motion.
38 * ODEDynamics.cs contains methods dealing with Prim Physical motion
39 * (dynamics) and the associated settings. Old Linear and angular
40 * motors for dynamic motion have been replace with MoveLinear()
41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
43 * switch between 'VEHICLE' parameter use and general dynamics
44 * settings use.
45 */
46
47//#define SPAM
48
49using System;
50using System.Collections.Generic;
51using System.Reflection;
52using System.Runtime.InteropServices;
53using System.Threading;
54using log4net;
55using OpenMetaverse;
56using OdeAPI;
57using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager;
59
60
61namespace OpenSim.Region.Physics.OdePlugin
62{
63 public class OdePrim : PhysicsActor
64 {
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66
67 private bool m_isphysical;
68 private bool m_fakeisphysical;
69 private bool m_isphantom;
70 private bool m_fakeisphantom;
71 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
73
74 protected bool m_building;
75 protected bool m_forcePosOrRotation;
76 private bool m_iscolliding;
77
78 internal bool m_isSelected;
79 private bool m_delaySelect;
80 private bool m_lastdoneSelected;
81 internal bool m_outbounds;
82
83 private Quaternion m_lastorientation = new Quaternion();
84 private Quaternion _orientation;
85
86 private Vector3 _position;
87 private Vector3 _velocity;
88 private Vector3 _torque;
89 private Vector3 m_lastVelocity;
90 private Vector3 m_lastposition;
91 private Vector3 m_rotationalVelocity;
92 private Vector3 _size;
93 private Vector3 _acceleration;
94 private Vector3 m_angularlock = Vector3.One;
95 private IntPtr Amotor = IntPtr.Zero;
96
97 private Vector3 m_force;
98 private Vector3 m_forceacc;
99 private Vector3 m_angularForceacc;
100
101 private float m_invTimeStep = 50.0f;
102 private float m_timeStep = .02f;
103
104 private Vector3 m_PIDTarget;
105 private float m_PIDTau;
106 private bool m_usePID;
107
108 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
109 // and are for non-VEHICLES only.
110
111 private float m_PIDHoverHeight;
112 private float m_PIDHoverTau;
113 private bool m_useHoverPID;
114 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
115 private float m_targetHoverHeight;
116 private float m_groundHeight;
117 private float m_waterHeight;
118 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
119
120 private int body_autodisable_frames = 5;
121 public int bodydisablecontrol = 0;
122
123
124 // Default we're a Geometry
125 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
126 // Default colide nonphysical don't try to colide with anything
127 private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
128
129 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
130 CollisionCategories.Character |
131 CollisionCategories.Land |
132 CollisionCategories.VolumeDtc);
133
134// private bool m_collidesLand = true;
135 private bool m_collidesWater;
136// public bool m_returnCollisions;
137
138 private bool m_NoColide; // for now only for internal use for bad meshs
139
140
141 // Default, Collide with Other Geometries, spaces and Bodies
142 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
143
144 public bool m_disabled;
145
146 private uint m_localID;
147
148 private IMesh m_mesh;
149 private object m_meshlock = new object();
150 private PrimitiveBaseShape _pbs;
151 public OdeScene _parent_scene;
152
153 /// <summary>
154 /// The physics space which contains prim geometry
155 /// </summary>
156 public IntPtr m_targetSpace = IntPtr.Zero;
157
158 public IntPtr prim_geom;
159 public IntPtr _triMeshData;
160
161 private PhysicsActor _parent;
162
163 private List<OdePrim> childrenPrim = new List<OdePrim>();
164
165
166// private bool m_throttleUpdates;
167// private int throttleCounter;
168 public float m_collisionscore;
169 private int m_colliderfilter = 0;
170
171 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
172
173 private float m_density = 10.000006836f; // Aluminum g/cm3;
174 private byte m_shapetype;
175 public bool _zeroFlag;
176 private bool m_lastUpdateSent;
177
178 public IntPtr Body = IntPtr.Zero;
179// public String Name { get; private set; }
180 private Vector3 _target_velocity;
181
182 public Vector3 primOOBsize; // prim real dimensions from mesh
183 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
184 public float primOOBradiusSQ;
185 public d.Mass primdMass; // prim inertia information on it's own referencial
186 float primMass; // prim own mass
187 float primVolume; // prim own volume;
188 float _mass; // object mass acording to case
189 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
190
191 public int givefakepos = 0;
192 private Vector3 fakepos;
193 public int givefakeori = 0;
194 private Quaternion fakeori;
195
196 private int m_eventsubscription;
197 private int m_cureventsubscription;
198 private CollisionEventUpdate CollisionEventsThisFrame = null;
199 private bool SentEmptyCollisionsEvent;
200
201 public volatile bool childPrim;
202
203 public ODEDynamics m_vehicle;
204
205 internal int m_material = (int)Material.Wood;
206 private float mu;
207 private float bounce;
208
209 /// <summary>
210 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
211 /// </summary>
212 public override bool IsPhysical // this is not reliable for internal use
213 {
214 get { return m_fakeisphysical; }
215 set
216 {
217 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
218 // and also to stop imediatly some updates
219 // but real change will only happen in taintprocessing
220
221 if (!value) // Zero the remembered last velocity
222 m_lastVelocity = Vector3.Zero;
223 AddChange(changes.Physical, value);
224 }
225 }
226
227 public override bool IsVolumeDtc
228 {
229 get { return m_fakeisVolumeDetect; }
230 set
231 {
232 m_fakeisVolumeDetect = value;
233 AddChange(changes.VolumeDtc, value);
234 }
235 }
236
237 public override bool Phantom // this is not reliable for internal use
238 {
239 get { return m_fakeisphantom; }
240 set
241 {
242 m_fakeisphantom = value;
243 AddChange(changes.Phantom, value);
244 }
245 }
246
247 public override bool Building // this is not reliable for internal use
248 {
249 get { return m_building; }
250 set
251 {
252 if (value)
253 m_building = true;
254 AddChange(changes.building, value);
255 }
256 }
257
258 public override void getContactData(ref ContactData cdata)
259 {
260 cdata.mu = mu;
261 cdata.bounce = bounce;
262
263 // cdata.softcolide = m_softcolide;
264 cdata.softcolide = false;
265
266 if (m_isphysical)
267 {
268 ODEDynamics veh;
269 if (_parent != null)
270 veh = ((OdePrim)_parent).m_vehicle;
271 else
272 veh = m_vehicle;
273
274 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
275 cdata.mu *= veh.FrictionFactor;
276// cdata.mu *= 0;
277 }
278 }
279
280 public override int PhysicsActorType
281 {
282 get { return (int)ActorTypes.Prim; }
283 set { return; }
284 }
285
286 public override bool SetAlwaysRun
287 {
288 get { return false; }
289 set { return; }
290 }
291
292 public override uint LocalID
293 {
294 get { return m_localID; }
295 set { m_localID = value; }
296 }
297
298 public override PhysicsActor ParentActor
299 {
300 get
301 {
302 if (childPrim)
303 return _parent;
304 else
305 return (PhysicsActor)this;
306 }
307 }
308
309 public override bool Grabbed
310 {
311 set { return; }
312 }
313
314 public override bool Selected
315 {
316 set
317 {
318 if (value)
319 m_isSelected = value; // if true set imediatly to stop moves etc
320 AddChange(changes.Selected, value);
321 }
322 }
323
324 public override bool Flying
325 {
326 // no flying prims for you
327 get { return false; }
328 set { }
329 }
330
331 public override bool IsColliding
332 {
333 get { return m_iscolliding; }
334 set
335 {
336 if (value)
337 {
338 m_colliderfilter += 2;
339 if (m_colliderfilter > 2)
340 m_colliderfilter = 2;
341 }
342 else
343 {
344 m_colliderfilter--;
345 if (m_colliderfilter < 0)
346 m_colliderfilter = 0;
347 }
348
349 if (m_colliderfilter == 0)
350 m_iscolliding = false;
351 else
352 m_iscolliding = true;
353 }
354 }
355
356 public override bool CollidingGround
357 {
358 get { return false; }
359 set { return; }
360 }
361
362 public override bool CollidingObj
363 {
364 get { return false; }
365 set { return; }
366 }
367
368
369 public override bool ThrottleUpdates {get;set;}
370/*
371 {
372 get { return m_throttleUpdates; }
373 set { m_throttleUpdates = value; }
374 }
375*/
376 public override bool Stopped
377 {
378 get { return _zeroFlag; }
379 }
380
381 public override Vector3 Position
382 {
383 get
384 {
385 if (givefakepos > 0)
386 return fakepos;
387 else
388 return _position;
389 }
390
391 set
392 {
393 fakepos = value;
394 givefakepos++;
395 AddChange(changes.Position, value);
396 }
397 }
398
399 public override Vector3 Size
400 {
401 get { return _size; }
402 set
403 {
404 if (value.IsFinite())
405 {
406 AddChange(changes.Size, value);
407 }
408 else
409 {
410 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
411 }
412 }
413 }
414
415 public override float Mass
416 {
417 get { return primMass; }
418 }
419
420 public override Vector3 Force
421 {
422 //get { return Vector3.Zero; }
423 get { return m_force; }
424 set
425 {
426 if (value.IsFinite())
427 {
428 AddChange(changes.Force, value);
429 }
430 else
431 {
432 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
433 }
434 }
435 }
436
437 public override void SetVolumeDetect(int param)
438 {
439 m_fakeisVolumeDetect = (param != 0);
440 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
441 }
442
443 public override Vector3 GeometricCenter
444 {
445 // this is not real geometric center but a average of positions relative to root prim acording to
446 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
447 // ignoring tortured prims details since sl also seems to ignore
448 // so no real use in doing it on physics
449 get
450 {
451 return Vector3.Zero;
452 }
453 }
454
455 public override Vector3 CenterOfMass
456 {
457 get
458 {
459 lock (_parent_scene.OdeLock)
460 {
461 d.Vector3 dtmp;
462 if (!childPrim && Body != IntPtr.Zero)
463 {
464 dtmp = d.BodyGetPosition(Body);
465 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
466 }
467 else if (prim_geom != IntPtr.Zero)
468 {
469 d.Quaternion dq;
470 d.GeomCopyQuaternion(prim_geom, out dq);
471 Quaternion q;
472 q.X = dq.X;
473 q.Y = dq.Y;
474 q.Z = dq.Z;
475 q.W = dq.W;
476
477 Vector3 Ptot = primOOBoffset * q;
478 dtmp = d.GeomGetPosition(prim_geom);
479 Ptot.X += dtmp.X;
480 Ptot.Y += dtmp.Y;
481 Ptot.Z += dtmp.Z;
482
483 // if(childPrim) we only know about physical linksets
484 return Ptot;
485/*
486 float tmass = _mass;
487 Ptot *= tmass;
488
489 float m;
490
491 foreach (OdePrim prm in childrenPrim)
492 {
493 m = prm._mass;
494 Ptot += prm.CenterOfMass * m;
495 tmass += m;
496 }
497
498 if (tmass == 0)
499 tmass = 0;
500 else
501 tmass = 1.0f / tmass;
502
503 Ptot *= tmass;
504 return Ptot;
505*/
506 }
507 else
508 return _position;
509 }
510 }
511 }
512
513 public override Vector3 OOBsize
514 {
515 get
516 {
517 return primOOBsize;
518 }
519 }
520
521 public override Vector3 OOBoffset
522 {
523 get
524 {
525 return primOOBoffset;
526 }
527 }
528
529 public override float OOBRadiusSQ
530 {
531 get
532 {
533 return primOOBradiusSQ;
534 }
535 }
536
537 public override PrimitiveBaseShape Shape
538 {
539 set
540 {
541/*
542 IMesh mesh = null;
543 if (_parent_scene.needsMeshing(value))
544 {
545 bool convex;
546 if (m_shapetype == 0)
547 convex = false;
548 else
549 convex = true;
550 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
551 }
552
553 if (mesh != null)
554 {
555 lock (m_meshlock)
556 m_mesh = mesh;
557 }
558*/
559 AddChange(changes.Shape, value);
560 }
561 }
562
563 public override byte PhysicsShapeType
564 {
565 get
566 {
567 return m_shapetype;
568 }
569 set
570 {
571 m_shapetype = value;
572 AddChange(changes.Shape, null);
573 }
574 }
575
576
577 public override Vector3 Velocity
578 {
579 get
580 {
581 if (_zeroFlag)
582 return Vector3.Zero;
583 return _velocity;
584 }
585 set
586 {
587 if (value.IsFinite())
588 {
589 AddChange(changes.Velocity, value);
590 }
591 else
592 {
593 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
594 }
595
596 }
597 }
598
599 public override Vector3 Torque
600 {
601 get
602 {
603 if (!IsPhysical || Body == IntPtr.Zero)
604 return Vector3.Zero;
605
606 return _torque;
607 }
608
609 set
610 {
611 if (value.IsFinite())
612 {
613 AddChange(changes.Torque, value);
614 }
615 else
616 {
617 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
618 }
619 }
620 }
621
622 public override float CollisionScore
623 {
624 get { return m_collisionscore; }
625 set { m_collisionscore = value; }
626 }
627
628 public override bool Kinematic
629 {
630 get { return false; }
631 set { }
632 }
633
634 public override Quaternion Orientation
635 {
636 get
637 {
638 if (givefakeori > 0)
639 return fakeori;
640 else
641
642 return _orientation;
643 }
644 set
645 {
646 if (QuaternionIsFinite(value))
647 {
648 fakeori = value;
649 givefakeori++;
650 AddChange(changes.Orientation, value);
651 }
652 else
653 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
654
655 }
656 }
657
658 public override Vector3 Acceleration
659 {
660 get { return _acceleration; }
661 set { }
662 }
663
664 public override Vector3 RotationalVelocity
665 {
666 get
667 {
668 Vector3 pv = Vector3.Zero;
669 if (_zeroFlag)
670 return pv;
671
672 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
673 return pv;
674
675 return m_rotationalVelocity;
676 }
677 set
678 {
679 if (value.IsFinite())
680 {
681 AddChange(changes.AngVelocity, value);
682 }
683 else
684 {
685 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
686 }
687 }
688 }
689
690 public override float Buoyancy
691 {
692 get { return m_buoyancy; }
693 set
694 {
695 m_buoyancy = value;
696 }
697 }
698
699 public override bool FloatOnWater
700 {
701 set
702 {
703 AddChange(changes.CollidesWater, value);
704 }
705 }
706
707 public override Vector3 PIDTarget
708 {
709 set
710 {
711 if (value.IsFinite())
712 {
713 m_PIDTarget = value;
714 }
715 else
716 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
717 }
718 }
719
720 public override bool PIDActive { set { m_usePID = value; } }
721 public override float PIDTau
722 {
723 set
724 {
725 if (value <= 0)
726 m_PIDTau = 0;
727 else
728 {
729 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
730 if (value < mint)
731 m_PIDTau = mint;
732 else
733 m_PIDTau = value;
734 }
735 }
736 }
737
738 public override float PIDHoverHeight
739 {
740 set
741 {
742 m_PIDHoverHeight = value;
743 if (value == 0)
744 m_useHoverPID = false;
745 }
746 }
747 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
748 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
749 public override float PIDHoverTau
750 {
751 set
752 {
753 if (value <= 0)
754 m_PIDHoverTau = 0;
755 else
756 {
757 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
758 if (value < mint)
759 m_PIDHoverTau = mint;
760 else
761 m_PIDHoverTau = value;
762 }
763 }
764 }
765
766 public override Quaternion APIDTarget { set { return; } }
767
768 public override bool APIDActive { set { return; } }
769
770 public override float APIDStrength { set { return; } }
771
772 public override float APIDDamping { set { return; } }
773
774 public override int VehicleType
775 {
776 // we may need to put a fake on this
777 get
778 {
779 if (m_vehicle == null)
780 return (int)Vehicle.TYPE_NONE;
781 else
782 return (int)m_vehicle.Type;
783 }
784 set
785 {
786 AddChange(changes.VehicleType, value);
787 }
788 }
789
790 public override void VehicleFloatParam(int param, float value)
791 {
792 strVehicleFloatParam fp = new strVehicleFloatParam();
793 fp.param = param;
794 fp.value = value;
795 AddChange(changes.VehicleFloatParam, fp);
796 }
797
798 public override void VehicleVectorParam(int param, Vector3 value)
799 {
800 strVehicleVectorParam fp = new strVehicleVectorParam();
801 fp.param = param;
802 fp.value = value;
803 AddChange(changes.VehicleVectorParam, fp);
804 }
805
806 public override void VehicleRotationParam(int param, Quaternion value)
807 {
808 strVehicleQuatParam fp = new strVehicleQuatParam();
809 fp.param = param;
810 fp.value = value;
811 AddChange(changes.VehicleRotationParam, fp);
812 }
813
814 public override void VehicleFlags(int param, bool value)
815 {
816 strVehicleBoolParam bp = new strVehicleBoolParam();
817 bp.param = param;
818 bp.value = value;
819 AddChange(changes.VehicleFlags, bp);
820 }
821
822 public override void SetVehicle(object vdata)
823 {
824 AddChange(changes.SetVehicle, vdata);
825 }
826 public void SetAcceleration(Vector3 accel)
827 {
828 _acceleration = accel;
829 }
830
831 public override void AddForce(Vector3 force, bool pushforce)
832 {
833 if (force.IsFinite())
834 {
835 if(pushforce)
836 AddChange(changes.AddForce, force);
837 else // a impulse
838 AddChange(changes.AddForce, force * m_invTimeStep);
839 }
840 else
841 {
842 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
843 }
844 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
845 }
846
847 public override void AddAngularForce(Vector3 force, bool pushforce)
848 {
849 if (force.IsFinite())
850 {
851// if(pushforce) for now applyrotationimpulse seems more happy applied as a force
852 AddChange(changes.AddAngForce, force);
853// else // a impulse
854// AddChange(changes.AddAngForce, force * m_invTimeStep);
855 }
856 else
857 {
858 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
859 }
860 }
861
862 public override void CrossingFailure()
863 {
864 if (m_outbounds)
865 {
866 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
867 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
868 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
869
870 m_lastposition = _position;
871 _velocity.X = 0;
872 _velocity.Y = 0;
873 _velocity.Z = 0;
874
875 m_lastVelocity = _velocity;
876 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
877 m_vehicle.Stop();
878
879 if(Body != IntPtr.Zero)
880 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
881 if (prim_geom != IntPtr.Zero)
882 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
883
884 m_outbounds = false;
885 changeDisable(false);
886 base.RequestPhysicsterseUpdate();
887 }
888 }
889
890 public override void SetMomentum(Vector3 momentum)
891 {
892 }
893
894 public override void SetMaterial(int pMaterial)
895 {
896 m_material = pMaterial;
897 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
898 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
899 }
900
901 public void setPrimForRemoval()
902 {
903 AddChange(changes.Remove, null);
904 }
905
906 public override void link(PhysicsActor obj)
907 {
908 AddChange(changes.Link, obj);
909 }
910
911 public override void delink()
912 {
913 AddChange(changes.DeLink, null);
914 }
915
916 public override void LockAngularMotion(Vector3 axis)
917 {
918 // reverse the zero/non zero values for ODE.
919 if (axis.IsFinite())
920 {
921 axis.X = (axis.X > 0) ? 1f : 0f;
922 axis.Y = (axis.Y > 0) ? 1f : 0f;
923 axis.Z = (axis.Z > 0) ? 1f : 0f;
924 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
925 AddChange(changes.AngLock, axis);
926 }
927 else
928 {
929 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
930 }
931 }
932
933 public override void SubscribeEvents(int ms)
934 {
935 m_eventsubscription = ms;
936 m_cureventsubscription = 0;
937 if (CollisionEventsThisFrame == null)
938 CollisionEventsThisFrame = new CollisionEventUpdate();
939 SentEmptyCollisionsEvent = false;
940 }
941
942 public override void UnSubscribeEvents()
943 {
944 if (CollisionEventsThisFrame != null)
945 {
946 CollisionEventsThisFrame.Clear();
947 CollisionEventsThisFrame = null;
948 }
949 m_eventsubscription = 0;
950 _parent_scene.RemoveCollisionEventReporting(this);
951 }
952
953 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
954 {
955 if (CollisionEventsThisFrame == null)
956 CollisionEventsThisFrame = new CollisionEventUpdate();
957// if(CollisionEventsThisFrame.Count < 32)
958 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
959 }
960
961 public void SendCollisions()
962 {
963 if (CollisionEventsThisFrame == null)
964 return;
965
966 if (m_cureventsubscription < m_eventsubscription)
967 return;
968
969 m_cureventsubscription = 0;
970
971 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
972
973 if (!SentEmptyCollisionsEvent || ncolisions > 0)
974 {
975 base.SendCollisionUpdate(CollisionEventsThisFrame);
976
977 if (ncolisions == 0)
978 {
979 SentEmptyCollisionsEvent = true;
980 _parent_scene.RemoveCollisionEventReporting(this);
981 }
982 else
983 {
984 SentEmptyCollisionsEvent = false;
985 CollisionEventsThisFrame.Clear();
986 }
987 }
988 }
989
990 internal void AddCollisionFrameTime(int t)
991 {
992 if (m_cureventsubscription < 50000)
993 m_cureventsubscription += t;
994 }
995
996 public override bool SubscribedEvents()
997 {
998 if (m_eventsubscription > 0)
999 return true;
1000 return false;
1001 }
1002
1003
1004 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
1005 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1006 {
1007 Name = primName;
1008 LocalID = plocalID;
1009
1010 m_vehicle = null;
1011
1012 if (!pos.IsFinite())
1013 {
1014 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1015 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1016 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1017 }
1018 _position = pos;
1019 givefakepos = 0;
1020
1021 m_timeStep = parent_scene.ODE_STEPSIZE;
1022 m_invTimeStep = 1f / m_timeStep;
1023
1024 m_density = parent_scene.geomDefaultDensity;
1025 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
1026 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
1027
1028 prim_geom = IntPtr.Zero;
1029 collide_geom = IntPtr.Zero;
1030 Body = IntPtr.Zero;
1031
1032 if (!size.IsFinite())
1033 {
1034 size = new Vector3(0.5f, 0.5f, 0.5f);
1035 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
1036 }
1037
1038 if (size.X <= 0) size.X = 0.01f;
1039 if (size.Y <= 0) size.Y = 0.01f;
1040 if (size.Z <= 0) size.Z = 0.01f;
1041
1042 _size = size;
1043
1044 if (!QuaternionIsFinite(rotation))
1045 {
1046 rotation = Quaternion.Identity;
1047 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
1048 }
1049
1050 _orientation = rotation;
1051 givefakeori = 0;
1052
1053 _pbs = pbs;
1054
1055 _parent_scene = parent_scene;
1056 m_targetSpace = IntPtr.Zero;
1057
1058 if (pos.Z < 0)
1059 {
1060 m_isphysical = false;
1061 }
1062 else
1063 {
1064 m_isphysical = pisPhysical;
1065 }
1066 m_fakeisphysical = m_isphysical;
1067
1068 m_isVolumeDetect = false;
1069 m_fakeisVolumeDetect = false;
1070
1071 m_force = Vector3.Zero;
1072
1073 m_iscolliding = false;
1074 m_colliderfilter = 0;
1075 m_NoColide = false;
1076
1077 hasOOBoffsetFromMesh = false;
1078 _triMeshData = IntPtr.Zero;
1079
1080 m_shapetype = _shapeType;
1081
1082 m_lastdoneSelected = false;
1083 m_isSelected = false;
1084 m_delaySelect = false;
1085
1086 m_isphantom = pisPhantom;
1087 m_fakeisphantom = pisPhantom;
1088
1089 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
1090 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
1091
1092 CalcPrimBodyData();
1093
1094 m_mesh = null;
1095 if (_parent_scene.needsMeshing(pbs))
1096 {
1097 bool convex;
1098 if (m_shapetype == 0)
1099 convex = false;
1100 else
1101 convex = true;
1102
1103 m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1104 }
1105
1106
1107 m_building = true; // control must set this to false when done
1108
1109 AddChange(changes.Add, null);
1110 }
1111
1112 private void resetCollisionAccounting()
1113 {
1114 m_collisionscore = 0;
1115 }
1116
1117 private void UpdateCollisionCatFlags()
1118 {
1119 if(m_isphysical && m_disabled)
1120 {
1121 m_collisionCategories = 0;
1122 m_collisionFlags = 0;
1123 }
1124
1125 else if (m_isSelected)
1126 {
1127 m_collisionCategories = CollisionCategories.Selected;
1128 m_collisionFlags = 0;
1129 }
1130
1131 else if (m_isVolumeDetect)
1132 {
1133 m_collisionCategories = CollisionCategories.VolumeDtc;
1134 if (m_isphysical)
1135 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1136 else
1137 m_collisionFlags = 0;
1138 }
1139 else if (m_isphantom)
1140 {
1141 m_collisionCategories = CollisionCategories.Phantom;
1142 if (m_isphysical)
1143 m_collisionFlags = CollisionCategories.Land;
1144 else
1145 m_collisionFlags = 0;
1146 }
1147 else
1148 {
1149 m_collisionCategories = CollisionCategories.Geom;
1150 if (m_isphysical)
1151 m_collisionFlags = m_default_collisionFlagsPhysical;
1152 else
1153 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1154 }
1155 }
1156
1157 private void ApplyCollisionCatFlags()
1158 {
1159 if (prim_geom != IntPtr.Zero)
1160 {
1161 if (!childPrim && childrenPrim.Count > 0)
1162 {
1163 foreach (OdePrim prm in childrenPrim)
1164 {
1165 if (m_isphysical && m_disabled)
1166 {
1167 prm.m_collisionCategories = 0;
1168 prm.m_collisionFlags = 0;
1169 }
1170 else
1171 {
1172 // preserve some
1173 if (prm.m_isSelected)
1174 {
1175 prm.m_collisionCategories = CollisionCategories.Selected;
1176 prm.m_collisionFlags = 0;
1177 }
1178 else if (prm.m_isVolumeDetect)
1179 {
1180 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1181 if (m_isphysical)
1182 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1183 else
1184 prm.m_collisionFlags = 0;
1185 }
1186 else if (prm.m_isphantom)
1187 {
1188 prm.m_collisionCategories = CollisionCategories.Phantom;
1189 if (m_isphysical)
1190 prm.m_collisionFlags = CollisionCategories.Land;
1191 else
1192 prm.m_collisionFlags = 0;
1193 }
1194 else
1195 {
1196 prm.m_collisionCategories = m_collisionCategories;
1197 prm.m_collisionFlags = m_collisionFlags;
1198 }
1199 }
1200
1201 if (prm.prim_geom != IntPtr.Zero)
1202 {
1203 if (prm.m_NoColide)
1204 {
1205 d.GeomSetCategoryBits(prm.prim_geom, 0);
1206 if (m_isphysical)
1207 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1208 else
1209 d.GeomSetCollideBits(prm.prim_geom, 0);
1210 }
1211 else
1212 {
1213 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1214 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1215 }
1216 }
1217 }
1218 }
1219
1220 if (m_NoColide)
1221 {
1222 d.GeomSetCategoryBits(prim_geom, 0);
1223 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1224 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1225 {
1226 d.GeomSetCategoryBits(collide_geom, 0);
1227 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
1228 }
1229 }
1230 else
1231 {
1232 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1233 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1234 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1235 {
1236 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1237 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1238 }
1239 }
1240 }
1241 }
1242
1243 private void createAMotor(Vector3 axis)
1244 {
1245 if (Body == IntPtr.Zero)
1246 return;
1247
1248 if (Amotor != IntPtr.Zero)
1249 {
1250 d.JointDestroy(Amotor);
1251 Amotor = IntPtr.Zero;
1252 }
1253
1254 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1255
1256 if (axisnum <= 0)
1257 return;
1258
1259 // stop it
1260 d.BodySetTorque(Body, 0, 0, 0);
1261 d.BodySetAngularVel(Body, 0, 0, 0);
1262
1263 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1264 d.JointAttach(Amotor, Body, IntPtr.Zero);
1265
1266 d.JointSetAMotorMode(Amotor, 0);
1267
1268 d.JointSetAMotorNumAxes(Amotor, axisnum);
1269
1270 // get current orientation to lock
1271
1272 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1273 Quaternion curr; // crap convertion between identical things
1274 curr.X = dcur.X;
1275 curr.Y = dcur.Y;
1276 curr.Z = dcur.Z;
1277 curr.W = dcur.W;
1278 Vector3 ax;
1279
1280 int i = 0;
1281 int j = 0;
1282 if (axis.X == 0)
1283 {
1284 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1285 // ODE should do this with axis relative to body 1 but seems to fail
1286 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1287 d.JointSetAMotorAngle(Amotor, 0, 0);
1288 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1289 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1290 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1291 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1292 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1294 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1295 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1296 i++;
1297 j = 256; // move to next axis set
1298 }
1299
1300 if (axis.Y == 0)
1301 {
1302 ax = (new Vector3(0, 1, 0)) * curr;
1303 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1304 d.JointSetAMotorAngle(Amotor, i, 0);
1305 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1306 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1307 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1308 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1309 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1311 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1312 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1313 i++;
1314 j += 256;
1315 }
1316
1317 if (axis.Z == 0)
1318 {
1319 ax = (new Vector3(0, 0, 1)) * curr;
1320 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1321 d.JointSetAMotorAngle(Amotor, i, 0);
1322 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1323 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1324 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1325 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1326 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1328 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1329 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1330 }
1331 }
1332
1333 private bool setMesh(OdeScene parent_scene)
1334 {
1335 IntPtr vertices, indices;
1336 int vertexCount, indexCount;
1337 int vertexStride, triStride;
1338
1339 if (Body != IntPtr.Zero)
1340 {
1341 if (childPrim)
1342 {
1343 if (_parent != null)
1344 {
1345 OdePrim parent = (OdePrim)_parent;
1346 parent.ChildDelink(this, false);
1347 }
1348 }
1349 else
1350 {
1351 DestroyBody();
1352 }
1353 }
1354
1355 IMesh mesh = null;
1356
1357
1358 lock (m_meshlock)
1359 {
1360 if (m_mesh == null)
1361 {
1362 bool convex;
1363 if (m_shapetype == 0)
1364 convex = false;
1365 else
1366 convex = true;
1367
1368 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1369 }
1370 else
1371 {
1372 mesh = m_mesh;
1373 }
1374
1375 if (mesh == null)
1376 {
1377 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1378 return false;
1379 }
1380
1381
1382 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1383 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1384
1385 if (vertexCount == 0 || indexCount == 0)
1386 {
1387 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1388 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1389 mesh.releaseSourceMeshData();
1390 return false;
1391 }
1392
1393 primOOBoffset = mesh.GetCentroid();
1394 hasOOBoffsetFromMesh = true;
1395
1396 mesh.releaseSourceMeshData();
1397 m_mesh = null;
1398 }
1399
1400 IntPtr geo = IntPtr.Zero;
1401
1402 try
1403 {
1404 _triMeshData = d.GeomTriMeshDataCreate();
1405
1406 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1407 d.GeomTriMeshDataPreprocess(_triMeshData);
1408
1409 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1410 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1411 }
1412
1413 catch (Exception e)
1414 {
1415 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1416 if (_triMeshData != IntPtr.Zero)
1417 {
1418 d.GeomTriMeshDataDestroy(_triMeshData);
1419 _triMeshData = IntPtr.Zero;
1420 }
1421 return false;
1422 }
1423
1424 SetGeom(geo);
1425 return true;
1426 }
1427
1428 private void SetGeom(IntPtr geom)
1429 {
1430 prim_geom = geom;
1431 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1432 if (prim_geom != IntPtr.Zero)
1433 {
1434
1435 if (m_NoColide)
1436 {
1437 d.GeomSetCategoryBits(prim_geom, 0);
1438 if (m_isphysical)
1439 {
1440 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1441 }
1442 else
1443 {
1444 d.GeomSetCollideBits(prim_geom, 0);
1445 d.GeomDisable(prim_geom);
1446 }
1447 }
1448 else
1449 {
1450 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1451 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1452 }
1453
1454 CalcPrimBodyData();
1455
1456 _parent_scene.actor_name_map[prim_geom] = this;
1457
1458 }
1459 else
1460 m_log.Warn("Setting bad Geom");
1461 }
1462
1463
1464 /// <summary>
1465 /// Create a geometry for the given mesh in the given target space.
1466 /// </summary>
1467 /// <param name="m_targetSpace"></param>
1468 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1469 private void CreateGeom()
1470 {
1471 if (_triMeshData != IntPtr.Zero)
1472 {
1473 d.GeomTriMeshDataDestroy(_triMeshData);
1474 _triMeshData = IntPtr.Zero;
1475 }
1476
1477 bool haveMesh = false;
1478 hasOOBoffsetFromMesh = false;
1479 m_NoColide = false;
1480
1481 if (_parent_scene.needsMeshing(_pbs))
1482 {
1483 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1484 if (!haveMesh)
1485 m_NoColide = true;
1486 }
1487
1488 if (!haveMesh)
1489 {
1490 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1491 && _size.X == _size.Y && _size.Y == _size.Z)
1492 { // it's a sphere
1493 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1494 try
1495 {
1496 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1497 }
1498 catch (Exception e)
1499 {
1500 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1501 return;
1502 }
1503 }
1504 else
1505 {// do it as a box
1506 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1507 try
1508 {
1509 //Console.WriteLine(" CreateGeom 4");
1510 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1511 }
1512 catch (Exception e)
1513 {
1514 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1515 return;
1516 }
1517 }
1518 }
1519 }
1520
1521 /// <summary>
1522 /// Set a new geometry for this prim.
1523 /// </summary>
1524 /// <param name="geom"></param>
1525 private void RemoveGeom()
1526 {
1527 if (prim_geom != IntPtr.Zero)
1528 {
1529// _parent_scene.geom_name_map.Remove(prim_geom);
1530 _parent_scene.actor_name_map.Remove(prim_geom);
1531 try
1532 {
1533 d.GeomDestroy(prim_geom);
1534 if (_triMeshData != IntPtr.Zero)
1535 {
1536 d.GeomTriMeshDataDestroy(_triMeshData);
1537 _triMeshData = IntPtr.Zero;
1538 }
1539 }
1540 // catch (System.AccessViolationException)
1541 catch (Exception e)
1542 {
1543 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1544 }
1545
1546 prim_geom = IntPtr.Zero;
1547 collide_geom = IntPtr.Zero;
1548 }
1549 else
1550 {
1551 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1552 }
1553 Body = IntPtr.Zero;
1554 hasOOBoffsetFromMesh = false;
1555 }
1556/*
1557 private void ChildSetGeom(OdePrim odePrim)
1558 {
1559 // well..
1560 DestroyBody();
1561 MakeBody();
1562 }
1563*/
1564 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1565 // should only be called for non physical prims unless they are becoming non physical
1566 private void SetInStaticSpace(OdePrim prim)
1567 {
1568 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1569 prim.m_targetSpace = targetSpace;
1570 collide_geom = IntPtr.Zero;
1571 }
1572
1573 public void enableBodySoft()
1574 {
1575 m_disabled = false;
1576 if (!childPrim && !m_isSelected)
1577 {
1578 if (m_isphysical && Body != IntPtr.Zero)
1579 {
1580 UpdateCollisionCatFlags();
1581 ApplyCollisionCatFlags();
1582
1583 d.BodyEnable(Body);
1584 }
1585 }
1586 resetCollisionAccounting();
1587 }
1588
1589 private void disableBodySoft()
1590 {
1591 m_disabled = true;
1592 if (!childPrim)
1593 {
1594 if (m_isphysical && Body != IntPtr.Zero)
1595 {
1596 if (m_isSelected)
1597 m_collisionFlags = CollisionCategories.Selected;
1598 else
1599 m_collisionCategories = 0;
1600 m_collisionFlags = 0;
1601 ApplyCollisionCatFlags();
1602 d.BodyDisable(Body);
1603 }
1604 }
1605 }
1606
1607 private void MakeBody()
1608 {
1609 if (!m_isphysical) // only physical get bodies
1610 return;
1611
1612 if (childPrim) // child prims don't get bodies;
1613 return;
1614
1615 if (m_building)
1616 return;
1617
1618 if (prim_geom == IntPtr.Zero)
1619 {
1620 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1621 return;
1622 }
1623
1624 if (Body != IntPtr.Zero)
1625 {
1626// d.BodyDestroy(Body);
1627// Body = IntPtr.Zero;
1628 // do a more complet destruction
1629 DestroyBody();
1630 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1631 }
1632
1633 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1634 {
1635 d.GeomSetBody(prim_geom, IntPtr.Zero);
1636 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1637 }
1638
1639 d.Matrix3 mymat = new d.Matrix3();
1640 d.Quaternion myrot = new d.Quaternion();
1641 d.Mass objdmass = new d.Mass { };
1642
1643 Body = d.BodyCreate(_parent_scene.world);
1644
1645 objdmass = primdMass;
1646
1647 // rotate inertia
1648 myrot.X = _orientation.X;
1649 myrot.Y = _orientation.Y;
1650 myrot.Z = _orientation.Z;
1651 myrot.W = _orientation.W;
1652
1653 d.RfromQ(out mymat, ref myrot);
1654 d.MassRotate(ref objdmass, ref mymat);
1655
1656 // set the body rotation
1657 d.BodySetRotation(Body, ref mymat);
1658
1659 // recompute full object inertia if needed
1660 if (childrenPrim.Count > 0)
1661 {
1662 d.Matrix3 mat = new d.Matrix3();
1663 d.Quaternion quat = new d.Quaternion();
1664 d.Mass tmpdmass = new d.Mass { };
1665 Vector3 rcm;
1666
1667 rcm.X = _position.X;
1668 rcm.Y = _position.Y;
1669 rcm.Z = _position.Z;
1670
1671 lock (childrenPrim)
1672 {
1673 foreach (OdePrim prm in childrenPrim)
1674 {
1675 if (prm.prim_geom == IntPtr.Zero)
1676 {
1677 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1678 continue;
1679 }
1680
1681 tmpdmass = prm.primdMass;
1682
1683 // apply prim current rotation to inertia
1684 quat.X = prm._orientation.X;
1685 quat.Y = prm._orientation.Y;
1686 quat.Z = prm._orientation.Z;
1687 quat.W = prm._orientation.W;
1688 d.RfromQ(out mat, ref quat);
1689 d.MassRotate(ref tmpdmass, ref mat);
1690
1691 Vector3 ppos = prm._position;
1692 ppos.X -= rcm.X;
1693 ppos.Y -= rcm.Y;
1694 ppos.Z -= rcm.Z;
1695 // refer inertia to root prim center of mass position
1696 d.MassTranslate(ref tmpdmass,
1697 ppos.X,
1698 ppos.Y,
1699 ppos.Z);
1700
1701 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1702 // fix prim colision cats
1703
1704 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1705 {
1706 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1707 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1708 }
1709
1710 d.GeomClearOffset(prm.prim_geom);
1711 d.GeomSetBody(prm.prim_geom, Body);
1712 prm.Body = Body;
1713 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1714 }
1715 }
1716 }
1717
1718 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1719 // associate root geom with body
1720 d.GeomSetBody(prim_geom, Body);
1721
1722 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1723 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1724
1725 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1726 myrot.X = -myrot.X;
1727 myrot.Y = -myrot.Y;
1728 myrot.Z = -myrot.Z;
1729
1730 d.RfromQ(out mymat, ref myrot);
1731 d.MassRotate(ref objdmass, ref mymat);
1732
1733 d.BodySetMass(Body, ref objdmass);
1734 _mass = objdmass.mass;
1735
1736 // disconnect from world gravity so we can apply buoyancy
1737 d.BodySetGravityMode(Body, false);
1738
1739 d.BodySetAutoDisableFlag(Body, true);
1740 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1741 d.BodySetDamping(Body, .005f, .005f);
1742
1743 if (m_targetSpace != IntPtr.Zero)
1744 {
1745 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1746 if (d.SpaceQuery(m_targetSpace, prim_geom))
1747 d.SpaceRemove(m_targetSpace, prim_geom);
1748 }
1749
1750 if (childrenPrim.Count == 0)
1751 {
1752 collide_geom = prim_geom;
1753 m_targetSpace = _parent_scene.ActiveSpace;
1754 }
1755 else
1756 {
1757 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1758 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1759 d.SpaceSetSublevel(m_targetSpace, 3);
1760 d.SpaceSetCleanup(m_targetSpace, false);
1761
1762 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1763 CollisionCategories.Geom |
1764 CollisionCategories.Phantom |
1765 CollisionCategories.VolumeDtc
1766 ));
1767 d.GeomSetCollideBits(m_targetSpace, 0);
1768 collide_geom = m_targetSpace;
1769 }
1770
1771 d.SpaceAdd(m_targetSpace, prim_geom);
1772
1773 if (m_delaySelect)
1774 {
1775 m_isSelected = true;
1776 m_delaySelect = false;
1777 }
1778
1779 m_collisionscore = 0;
1780
1781 UpdateCollisionCatFlags();
1782 ApplyCollisionCatFlags();
1783
1784 _parent_scene.addActivePrim(this);
1785
1786 lock (childrenPrim)
1787 {
1788 foreach (OdePrim prm in childrenPrim)
1789 {
1790 if (prm.prim_geom == IntPtr.Zero)
1791 continue;
1792
1793 Vector3 ppos = prm._position;
1794 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1795
1796 if (prm.m_targetSpace != m_targetSpace)
1797 {
1798 if (prm.m_targetSpace != IntPtr.Zero)
1799 {
1800 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1801 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1802 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1803 }
1804 prm.m_targetSpace = m_targetSpace;
1805 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1806 }
1807
1808 prm.m_collisionscore = 0;
1809
1810 if(!m_disabled)
1811 prm.m_disabled = false;
1812
1813 _parent_scene.addActivePrim(prm);
1814 }
1815 }
1816
1817 // The body doesn't already have a finite rotation mode set here
1818 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1819 {
1820 createAMotor(m_angularlock);
1821 }
1822
1823
1824 if (m_isSelected || m_disabled)
1825 {
1826 d.BodyDisable(Body);
1827 }
1828 else
1829 {
1830 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1831 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1832 }
1833 _parent_scene.addActiveGroups(this);
1834 }
1835
1836 private void DestroyBody()
1837 {
1838 if (Body != IntPtr.Zero)
1839 {
1840 _parent_scene.remActivePrim(this);
1841
1842 collide_geom = IntPtr.Zero;
1843
1844 if (m_disabled)
1845 m_collisionCategories = 0;
1846 else if (m_isSelected)
1847 m_collisionCategories = CollisionCategories.Selected;
1848 else if (m_isVolumeDetect)
1849 m_collisionCategories = CollisionCategories.VolumeDtc;
1850 else if (m_isphantom)
1851 m_collisionCategories = CollisionCategories.Phantom;
1852 else
1853 m_collisionCategories = CollisionCategories.Geom;
1854
1855 m_collisionFlags = 0;
1856
1857 if (prim_geom != IntPtr.Zero)
1858 {
1859 if (m_NoColide)
1860 {
1861 d.GeomSetCategoryBits(prim_geom, 0);
1862 d.GeomSetCollideBits(prim_geom, 0);
1863 }
1864 else
1865 {
1866 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1867 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1868 }
1869 UpdateDataFromGeom();
1870 d.GeomSetBody(prim_geom, IntPtr.Zero);
1871 SetInStaticSpace(this);
1872 }
1873
1874 if (!childPrim)
1875 {
1876 lock (childrenPrim)
1877 {
1878 foreach (OdePrim prm in childrenPrim)
1879 {
1880 _parent_scene.remActivePrim(prm);
1881
1882 if (prm.m_isSelected)
1883 prm.m_collisionCategories = CollisionCategories.Selected;
1884 else if (prm.m_isVolumeDetect)
1885 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1886 else if (prm.m_isphantom)
1887 prm.m_collisionCategories = CollisionCategories.Phantom;
1888 else
1889 prm.m_collisionCategories = CollisionCategories.Geom;
1890
1891 prm.m_collisionFlags = 0;
1892
1893 if (prm.prim_geom != IntPtr.Zero)
1894 {
1895 if (prm.m_NoColide)
1896 {
1897 d.GeomSetCategoryBits(prm.prim_geom, 0);
1898 d.GeomSetCollideBits(prm.prim_geom, 0);
1899 }
1900 else
1901 {
1902 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1903 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1904 }
1905 prm.UpdateDataFromGeom();
1906 SetInStaticSpace(prm);
1907 }
1908 prm.Body = IntPtr.Zero;
1909 prm._mass = prm.primMass;
1910 prm.m_collisionscore = 0;
1911 }
1912 }
1913 if (Amotor != IntPtr.Zero)
1914 {
1915 d.JointDestroy(Amotor);
1916 Amotor = IntPtr.Zero;
1917 }
1918 _parent_scene.remActiveGroup(this);
1919 d.BodyDestroy(Body);
1920 }
1921 Body = IntPtr.Zero;
1922 }
1923 _mass = primMass;
1924 m_collisionscore = 0;
1925 }
1926
1927 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1928 {
1929 d.Matrix3 mat = new d.Matrix3();
1930 d.Quaternion quat = new d.Quaternion();
1931
1932 d.Mass tmpdmass = new d.Mass { };
1933 d.Mass objdmass = new d.Mass { };
1934
1935 d.BodyGetMass(Body, out tmpdmass);
1936 objdmass = tmpdmass;
1937
1938 d.Vector3 dobjpos;
1939 d.Vector3 thispos;
1940
1941 // get current object position and rotation
1942 dobjpos = d.BodyGetPosition(Body);
1943
1944 // get prim own inertia in its local frame
1945 tmpdmass = primdMass;
1946
1947 // transform to object frame
1948 mat = d.GeomGetOffsetRotation(prim_geom);
1949 d.MassRotate(ref tmpdmass, ref mat);
1950
1951 thispos = d.GeomGetOffsetPosition(prim_geom);
1952 d.MassTranslate(ref tmpdmass,
1953 thispos.X,
1954 thispos.Y,
1955 thispos.Z);
1956
1957 // subtract current prim inertia from object
1958 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1959
1960 // back prim own inertia
1961 tmpdmass = primdMass;
1962
1963 // update to new position and orientation
1964 _position = NewPos;
1965 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1966 _orientation = newrot;
1967 quat.X = newrot.X;
1968 quat.Y = newrot.Y;
1969 quat.Z = newrot.Z;
1970 quat.W = newrot.W;
1971 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1972
1973 mat = d.GeomGetOffsetRotation(prim_geom);
1974 d.MassRotate(ref tmpdmass, ref mat);
1975
1976 thispos = d.GeomGetOffsetPosition(prim_geom);
1977 d.MassTranslate(ref tmpdmass,
1978 thispos.X,
1979 thispos.Y,
1980 thispos.Z);
1981
1982 d.MassAdd(ref objdmass, ref tmpdmass);
1983
1984 // fix all positions
1985 IntPtr g = d.BodyGetFirstGeom(Body);
1986 while (g != IntPtr.Zero)
1987 {
1988 thispos = d.GeomGetOffsetPosition(g);
1989 thispos.X -= objdmass.c.X;
1990 thispos.Y -= objdmass.c.Y;
1991 thispos.Z -= objdmass.c.Z;
1992 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1993 g = d.dBodyGetNextGeom(g);
1994 }
1995 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1996
1997 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1998 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1999 d.BodySetMass(Body, ref objdmass);
2000 _mass = objdmass.mass;
2001 }
2002
2003
2004
2005 private void FixInertia(Vector3 NewPos)
2006 {
2007 d.Matrix3 primmat = new d.Matrix3();
2008 d.Mass tmpdmass = new d.Mass { };
2009 d.Mass objdmass = new d.Mass { };
2010 d.Mass primmass = new d.Mass { };
2011
2012 d.Vector3 dobjpos;
2013 d.Vector3 thispos;
2014
2015 d.BodyGetMass(Body, out objdmass);
2016
2017 // get prim own inertia in its local frame
2018 primmass = primdMass;
2019 // transform to object frame
2020 primmat = d.GeomGetOffsetRotation(prim_geom);
2021 d.MassRotate(ref primmass, ref primmat);
2022
2023 tmpdmass = primmass;
2024
2025 thispos = d.GeomGetOffsetPosition(prim_geom);
2026 d.MassTranslate(ref tmpdmass,
2027 thispos.X,
2028 thispos.Y,
2029 thispos.Z);
2030
2031 // subtract current prim inertia from object
2032 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2033
2034 // update to new position
2035 _position = NewPos;
2036 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2037
2038 thispos = d.GeomGetOffsetPosition(prim_geom);
2039 d.MassTranslate(ref primmass,
2040 thispos.X,
2041 thispos.Y,
2042 thispos.Z);
2043
2044 d.MassAdd(ref objdmass, ref primmass);
2045
2046 // fix all positions
2047 IntPtr g = d.BodyGetFirstGeom(Body);
2048 while (g != IntPtr.Zero)
2049 {
2050 thispos = d.GeomGetOffsetPosition(g);
2051 thispos.X -= objdmass.c.X;
2052 thispos.Y -= objdmass.c.Y;
2053 thispos.Z -= objdmass.c.Z;
2054 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2055 g = d.dBodyGetNextGeom(g);
2056 }
2057
2058 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2059
2060 // get current object position and rotation
2061 dobjpos = d.BodyGetPosition(Body);
2062
2063 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2064 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2065 d.BodySetMass(Body, ref objdmass);
2066 _mass = objdmass.mass;
2067 }
2068
2069 private void FixInertia(Quaternion newrot)
2070 {
2071 d.Matrix3 mat = new d.Matrix3();
2072 d.Quaternion quat = new d.Quaternion();
2073
2074 d.Mass tmpdmass = new d.Mass { };
2075 d.Mass objdmass = new d.Mass { };
2076 d.Vector3 dobjpos;
2077 d.Vector3 thispos;
2078
2079 d.BodyGetMass(Body, out objdmass);
2080
2081 // get prim own inertia in its local frame
2082 tmpdmass = primdMass;
2083 mat = d.GeomGetOffsetRotation(prim_geom);
2084 d.MassRotate(ref tmpdmass, ref mat);
2085 // transform to object frame
2086 thispos = d.GeomGetOffsetPosition(prim_geom);
2087 d.MassTranslate(ref tmpdmass,
2088 thispos.X,
2089 thispos.Y,
2090 thispos.Z);
2091
2092 // subtract current prim inertia from object
2093 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2094
2095 // update to new orientation
2096 _orientation = newrot;
2097 quat.X = newrot.X;
2098 quat.Y = newrot.Y;
2099 quat.Z = newrot.Z;
2100 quat.W = newrot.W;
2101 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2102
2103 tmpdmass = primdMass;
2104 mat = d.GeomGetOffsetRotation(prim_geom);
2105 d.MassRotate(ref tmpdmass, ref mat);
2106 d.MassTranslate(ref tmpdmass,
2107 thispos.X,
2108 thispos.Y,
2109 thispos.Z);
2110
2111 d.MassAdd(ref objdmass, ref tmpdmass);
2112
2113 // fix all positions
2114 IntPtr g = d.BodyGetFirstGeom(Body);
2115 while (g != IntPtr.Zero)
2116 {
2117 thispos = d.GeomGetOffsetPosition(g);
2118 thispos.X -= objdmass.c.X;
2119 thispos.Y -= objdmass.c.Y;
2120 thispos.Z -= objdmass.c.Z;
2121 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2122 g = d.dBodyGetNextGeom(g);
2123 }
2124
2125 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2126 // get current object position and rotation
2127 dobjpos = d.BodyGetPosition(Body);
2128
2129 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2130 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2131 d.BodySetMass(Body, ref objdmass);
2132 _mass = objdmass.mass;
2133 }
2134
2135
2136 #region Mass Calculation
2137
2138 private float CalculatePrimVolume()
2139 {
2140 float volume = _size.X * _size.Y * _size.Z; // default
2141 float tmp;
2142
2143 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
2144 float hollowVolume = hollowAmount * hollowAmount;
2145
2146 switch (_pbs.ProfileShape)
2147 {
2148 case ProfileShape.Square:
2149 // default box
2150
2151 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2152 {
2153 if (hollowAmount > 0.0)
2154 {
2155 switch (_pbs.HollowShape)
2156 {
2157 case HollowShape.Square:
2158 case HollowShape.Same:
2159 break;
2160
2161 case HollowShape.Circle:
2162
2163 hollowVolume *= 0.78539816339f;
2164 break;
2165
2166 case HollowShape.Triangle:
2167
2168 hollowVolume *= (0.5f * .5f);
2169 break;
2170
2171 default:
2172 hollowVolume = 0;
2173 break;
2174 }
2175 volume *= (1.0f - hollowVolume);
2176 }
2177 }
2178
2179 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2180 {
2181 //a tube
2182
2183 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
2184 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
2185 volume -= volume * tmp * tmp;
2186
2187 if (hollowAmount > 0.0)
2188 {
2189 hollowVolume *= hollowAmount;
2190
2191 switch (_pbs.HollowShape)
2192 {
2193 case HollowShape.Square:
2194 case HollowShape.Same:
2195 break;
2196
2197 case HollowShape.Circle:
2198 hollowVolume *= 0.78539816339f;
2199 break;
2200
2201 case HollowShape.Triangle:
2202 hollowVolume *= 0.5f * 0.5f;
2203 break;
2204 default:
2205 hollowVolume = 0;
2206 break;
2207 }
2208 volume *= (1.0f - hollowVolume);
2209 }
2210 }
2211
2212 break;
2213
2214 case ProfileShape.Circle:
2215
2216 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2217 {
2218 volume *= 0.78539816339f; // elipse base
2219
2220 if (hollowAmount > 0.0)
2221 {
2222 switch (_pbs.HollowShape)
2223 {
2224 case HollowShape.Same:
2225 case HollowShape.Circle:
2226 break;
2227
2228 case HollowShape.Square:
2229 hollowVolume *= 0.5f * 2.5984480504799f;
2230 break;
2231
2232 case HollowShape.Triangle:
2233 hollowVolume *= .5f * 1.27323954473516f;
2234 break;
2235
2236 default:
2237 hollowVolume = 0;
2238 break;
2239 }
2240 volume *= (1.0f - hollowVolume);
2241 }
2242 }
2243
2244 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2245 {
2246 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
2247 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2248 volume *= (1.0f - tmp * tmp);
2249
2250 if (hollowAmount > 0.0)
2251 {
2252
2253 // calculate the hollow volume by it's shape compared to the prim shape
2254 hollowVolume *= hollowAmount;
2255
2256 switch (_pbs.HollowShape)
2257 {
2258 case HollowShape.Same:
2259 case HollowShape.Circle:
2260 break;
2261
2262 case HollowShape.Square:
2263 hollowVolume *= 0.5f * 2.5984480504799f;
2264 break;
2265
2266 case HollowShape.Triangle:
2267 hollowVolume *= .5f * 1.27323954473516f;
2268 break;
2269
2270 default:
2271 hollowVolume = 0;
2272 break;
2273 }
2274 volume *= (1.0f - hollowVolume);
2275 }
2276 }
2277 break;
2278
2279 case ProfileShape.HalfCircle:
2280 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2281 {
2282 volume *= 0.5236f;
2283
2284 if (hollowAmount > 0.0)
2285 {
2286 hollowVolume *= hollowAmount;
2287
2288 switch (_pbs.HollowShape)
2289 {
2290 case HollowShape.Circle:
2291 case HollowShape.Triangle: // diference in sl is minor and odd
2292 case HollowShape.Same:
2293 break;
2294
2295 case HollowShape.Square:
2296 hollowVolume *= 0.909f;
2297 break;
2298
2299 // case HollowShape.Triangle:
2300 // hollowVolume *= .827f;
2301 // break;
2302 default:
2303 hollowVolume = 0;
2304 break;
2305 }
2306 volume *= (1.0f - hollowVolume);
2307 }
2308
2309 }
2310 break;
2311
2312 case ProfileShape.EquilateralTriangle:
2313
2314 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2315 {
2316 volume *= 0.32475953f;
2317
2318 if (hollowAmount > 0.0)
2319 {
2320
2321 // calculate the hollow volume by it's shape compared to the prim shape
2322 switch (_pbs.HollowShape)
2323 {
2324 case HollowShape.Same:
2325 case HollowShape.Triangle:
2326 hollowVolume *= .25f;
2327 break;
2328
2329 case HollowShape.Square:
2330 hollowVolume *= 0.499849f * 3.07920140172638f;
2331 break;
2332
2333 case HollowShape.Circle:
2334 // Hollow shape is a perfect cyllinder in respect to the cube's scale
2335 // Cyllinder hollow volume calculation
2336
2337 hollowVolume *= 0.1963495f * 3.07920140172638f;
2338 break;
2339
2340 default:
2341 hollowVolume = 0;
2342 break;
2343 }
2344 volume *= (1.0f - hollowVolume);
2345 }
2346 }
2347 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2348 {
2349 volume *= 0.32475953f;
2350 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
2351 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2352 volume *= (1.0f - tmp * tmp);
2353
2354 if (hollowAmount > 0.0)
2355 {
2356
2357 hollowVolume *= hollowAmount;
2358
2359 switch (_pbs.HollowShape)
2360 {
2361 case HollowShape.Same:
2362 case HollowShape.Triangle:
2363 hollowVolume *= .25f;
2364 break;
2365
2366 case HollowShape.Square:
2367 hollowVolume *= 0.499849f * 3.07920140172638f;
2368 break;
2369
2370 case HollowShape.Circle:
2371
2372 hollowVolume *= 0.1963495f * 3.07920140172638f;
2373 break;
2374
2375 default:
2376 hollowVolume = 0;
2377 break;
2378 }
2379 volume *= (1.0f - hollowVolume);
2380 }
2381 }
2382 break;
2383
2384 default:
2385 break;
2386 }
2387
2388 float taperX1;
2389 float taperY1;
2390 float taperX;
2391 float taperY;
2392 float pathBegin;
2393 float pathEnd;
2394 float profileBegin;
2395 float profileEnd;
2396
2397 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
2398 {
2399 taperX1 = _pbs.PathScaleX * 0.01f;
2400 if (taperX1 > 1.0f)
2401 taperX1 = 2.0f - taperX1;
2402 taperX = 1.0f - taperX1;
2403
2404 taperY1 = _pbs.PathScaleY * 0.01f;
2405 if (taperY1 > 1.0f)
2406 taperY1 = 2.0f - taperY1;
2407 taperY = 1.0f - taperY1;
2408 }
2409 else
2410 {
2411 taperX = _pbs.PathTaperX * 0.01f;
2412 if (taperX < 0.0f)
2413 taperX = -taperX;
2414 taperX1 = 1.0f - taperX;
2415
2416 taperY = _pbs.PathTaperY * 0.01f;
2417 if (taperY < 0.0f)
2418 taperY = -taperY;
2419 taperY1 = 1.0f - taperY;
2420 }
2421
2422 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
2423
2424 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
2425 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
2426 volume *= (pathEnd - pathBegin);
2427
2428 // this is crude aproximation
2429 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
2430 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
2431 volume *= (profileEnd - profileBegin);
2432
2433 return volume;
2434 }
2435
2436
2437 private void CalcPrimBodyData()
2438 {
2439 float volume;
2440
2441 if (prim_geom == IntPtr.Zero)
2442 {
2443 // Ubit let's have a initial basic OOB
2444 primOOBsize.X = _size.X;
2445 primOOBsize.Y = _size.Y;
2446 primOOBsize.Z = _size.Z;
2447 primOOBoffset = Vector3.Zero;
2448 }
2449 else
2450 {
2451 d.AABB AABB;
2452 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
2453
2454 primOOBsize.X = (AABB.MaxX - AABB.MinX);
2455 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
2456 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
2457 if (!hasOOBoffsetFromMesh)
2458 {
2459 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
2460 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
2461 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
2462 }
2463 }
2464
2465 // also its own inertia and mass
2466 // keep using basic shape mass for now
2467 volume = CalculatePrimVolume();
2468
2469 primVolume = volume;
2470 primMass = m_density * volume;
2471
2472 if (primMass <= 0)
2473 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2474 if (primMass > _parent_scene.maximumMassObject)
2475 primMass = _parent_scene.maximumMassObject;
2476
2477 _mass = primMass; // just in case
2478
2479 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
2480
2481 d.MassTranslate(ref primdMass,
2482 primOOBoffset.X,
2483 primOOBoffset.Y,
2484 primOOBoffset.Z);
2485
2486 primOOBsize *= 0.5f; // let obb size be a corner coords
2487 primOOBradiusSQ = primOOBsize.LengthSquared();
2488 }
2489
2490
2491 #endregion
2492
2493
2494 /// <summary>
2495 /// Add a child prim to this parent prim.
2496 /// </summary>
2497 /// <param name="prim">Child prim</param>
2498 // I'm the parent
2499 // prim is the child
2500 public void ParentPrim(OdePrim prim)
2501 {
2502 //Console.WriteLine("ParentPrim " + m_primName);
2503 if (this.m_localID != prim.m_localID)
2504 {
2505 DestroyBody(); // for now we need to rebuil entire object on link change
2506
2507 lock (childrenPrim)
2508 {
2509 // adopt the prim
2510 if (!childrenPrim.Contains(prim))
2511 childrenPrim.Add(prim);
2512
2513 // see if this prim has kids and adopt them also
2514 // should not happen for now
2515 foreach (OdePrim prm in prim.childrenPrim)
2516 {
2517 if (!childrenPrim.Contains(prm))
2518 {
2519 if (prm.Body != IntPtr.Zero)
2520 {
2521 if (prm.prim_geom != IntPtr.Zero)
2522 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2523 if (prm.Body != prim.Body)
2524 prm.DestroyBody(); // don't loose bodies around
2525 prm.Body = IntPtr.Zero;
2526 }
2527
2528 childrenPrim.Add(prm);
2529 prm._parent = this;
2530 }
2531 }
2532 }
2533 //Remove old children from the prim
2534 prim.childrenPrim.Clear();
2535
2536 if (prim.Body != IntPtr.Zero)
2537 {
2538 if (prim.prim_geom != IntPtr.Zero)
2539 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2540 prim.DestroyBody(); // don't loose bodies around
2541 prim.Body = IntPtr.Zero;
2542 }
2543
2544 prim.childPrim = true;
2545 prim._parent = this;
2546
2547 MakeBody(); // full nasty reconstruction
2548 }
2549 }
2550
2551 private void UpdateChildsfromgeom()
2552 {
2553 if (childrenPrim.Count > 0)
2554 {
2555 foreach (OdePrim prm in childrenPrim)
2556 prm.UpdateDataFromGeom();
2557 }
2558 }
2559
2560 private void UpdateDataFromGeom()
2561 {
2562 if (prim_geom != IntPtr.Zero)
2563 {
2564 d.Quaternion qtmp;
2565 d.GeomCopyQuaternion(prim_geom, out qtmp);
2566 _orientation.X = qtmp.X;
2567 _orientation.Y = qtmp.Y;
2568 _orientation.Z = qtmp.Z;
2569 _orientation.W = qtmp.W;
2570
2571 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2572 _position.X = lpos.X;
2573 _position.Y = lpos.Y;
2574 _position.Z = lpos.Z;
2575 }
2576 }
2577
2578 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2579 {
2580 // Okay, we have a delinked child.. destroy all body and remake
2581 if (odePrim != this && !childrenPrim.Contains(odePrim))
2582 return;
2583
2584 DestroyBody();
2585
2586 if (odePrim == this) // delinking the root prim
2587 {
2588 OdePrim newroot = null;
2589 lock (childrenPrim)
2590 {
2591 if (childrenPrim.Count > 0)
2592 {
2593 newroot = childrenPrim[0];
2594 childrenPrim.RemoveAt(0);
2595 foreach (OdePrim prm in childrenPrim)
2596 {
2597 newroot.childrenPrim.Add(prm);
2598 }
2599 childrenPrim.Clear();
2600 }
2601 if (newroot != null)
2602 {
2603 newroot.childPrim = false;
2604 newroot._parent = null;
2605 if (remakebodies)
2606 newroot.MakeBody();
2607 }
2608 }
2609 }
2610
2611 else
2612 {
2613 lock (childrenPrim)
2614 {
2615 childrenPrim.Remove(odePrim);
2616 odePrim.childPrim = false;
2617 odePrim._parent = null;
2618 // odePrim.UpdateDataFromGeom();
2619 if (remakebodies)
2620 odePrim.MakeBody();
2621 }
2622 }
2623 if (remakebodies)
2624 MakeBody();
2625 }
2626
2627 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2628 {
2629 // Okay, we have a delinked child.. destroy all body and remake
2630 if (odePrim != this && !childrenPrim.Contains(odePrim))
2631 return;
2632
2633 DestroyBody();
2634
2635 if (odePrim == this)
2636 {
2637 OdePrim newroot = null;
2638 lock (childrenPrim)
2639 {
2640 if (childrenPrim.Count > 0)
2641 {
2642 newroot = childrenPrim[0];
2643 childrenPrim.RemoveAt(0);
2644 foreach (OdePrim prm in childrenPrim)
2645 {
2646 newroot.childrenPrim.Add(prm);
2647 }
2648 childrenPrim.Clear();
2649 }
2650 if (newroot != null)
2651 {
2652 newroot.childPrim = false;
2653 newroot._parent = null;
2654 newroot.MakeBody();
2655 }
2656 }
2657 if (reMakeBody)
2658 MakeBody();
2659 return;
2660 }
2661 else
2662 {
2663 lock (childrenPrim)
2664 {
2665 childrenPrim.Remove(odePrim);
2666 odePrim.childPrim = false;
2667 odePrim._parent = null;
2668 if (reMakeBody)
2669 odePrim.MakeBody();
2670 }
2671 }
2672 MakeBody();
2673 }
2674
2675 #region changes
2676
2677 private void changeadd()
2678 {
2679 CreateGeom();
2680
2681 if (prim_geom != IntPtr.Zero)
2682 {
2683 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2684 d.Quaternion myrot = new d.Quaternion();
2685 myrot.X = _orientation.X;
2686 myrot.Y = _orientation.Y;
2687 myrot.Z = _orientation.Z;
2688 myrot.W = _orientation.W;
2689 d.GeomSetQuaternion(prim_geom, ref myrot);
2690
2691 if (!m_isphysical)
2692 {
2693 SetInStaticSpace(this);
2694 UpdateCollisionCatFlags();
2695 ApplyCollisionCatFlags();
2696 }
2697 else
2698 MakeBody();
2699 }
2700 }
2701
2702 private void changeAngularLock(Vector3 newLock)
2703 {
2704 // do we have a Physical object?
2705 if (Body != IntPtr.Zero)
2706 {
2707 //Check that we have a Parent
2708 //If we have a parent then we're not authorative here
2709 if (_parent == null)
2710 {
2711 if (!newLock.ApproxEquals(Vector3.One, 0f))
2712 {
2713 createAMotor(newLock);
2714 }
2715 else
2716 {
2717 if (Amotor != IntPtr.Zero)
2718 {
2719 d.JointDestroy(Amotor);
2720 Amotor = IntPtr.Zero;
2721 }
2722 }
2723 }
2724 }
2725 // Store this for later in case we get turned into a separate body
2726 m_angularlock = newLock;
2727 }
2728
2729 private void changeLink(OdePrim NewParent)
2730 {
2731 if (_parent == null && NewParent != null)
2732 {
2733 NewParent.ParentPrim(this);
2734 }
2735 else if (_parent != null)
2736 {
2737 if (_parent is OdePrim)
2738 {
2739 if (NewParent != _parent)
2740 {
2741 (_parent as OdePrim).ChildDelink(this, false); // for now...
2742 childPrim = false;
2743
2744 if (NewParent != null)
2745 {
2746 NewParent.ParentPrim(this);
2747 }
2748 }
2749 }
2750 }
2751 _parent = NewParent;
2752 }
2753
2754
2755 private void Stop()
2756 {
2757 if (!childPrim)
2758 {
2759 m_force = Vector3.Zero;
2760 m_forceacc = Vector3.Zero;
2761 m_angularForceacc = Vector3.Zero;
2762 _torque = Vector3.Zero;
2763 _velocity = Vector3.Zero;
2764 _acceleration = Vector3.Zero;
2765 m_rotationalVelocity = Vector3.Zero;
2766 _target_velocity = Vector3.Zero;
2767 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2768 m_vehicle.Stop();
2769 }
2770
2771 if (Body != IntPtr.Zero)
2772 {
2773 d.BodySetForce(Body, 0f, 0f, 0f);
2774 d.BodySetTorque(Body, 0f, 0f, 0f);
2775 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2776 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2777 }
2778 }
2779
2780 private void changePhantomStatus(bool newval)
2781 {
2782 m_isphantom = newval;
2783
2784 UpdateCollisionCatFlags();
2785 ApplyCollisionCatFlags();
2786 }
2787
2788/* not in use
2789 internal void ChildSelectedChange(bool childSelect)
2790 {
2791 if(childPrim)
2792 return;
2793
2794 if (childSelect == m_isSelected)
2795 return;
2796
2797 if (childSelect)
2798 {
2799 DoSelectedStatus(true);
2800 }
2801
2802 else
2803 {
2804 foreach (OdePrim prm in childrenPrim)
2805 {
2806 if (prm.m_isSelected)
2807 return;
2808 }
2809 DoSelectedStatus(false);
2810 }
2811 }
2812*/
2813 private void changeSelectedStatus(bool newval)
2814 {
2815 if (m_lastdoneSelected == newval)
2816 return;
2817
2818 m_lastdoneSelected = newval;
2819 DoSelectedStatus(newval);
2820 }
2821
2822 private void CheckDelaySelect()
2823 {
2824 if (m_delaySelect)
2825 {
2826 DoSelectedStatus(m_isSelected);
2827 }
2828 }
2829
2830 private void DoSelectedStatus(bool newval)
2831 {
2832 m_isSelected = newval;
2833 Stop();
2834
2835 if (newval)
2836 {
2837 if (!childPrim && Body != IntPtr.Zero)
2838 d.BodyDisable(Body);
2839
2840 if (m_delaySelect || m_isphysical)
2841 {
2842 m_collisionCategories = CollisionCategories.Selected;
2843 m_collisionFlags = 0;
2844
2845 if (!childPrim)
2846 {
2847 foreach (OdePrim prm in childrenPrim)
2848 {
2849 prm.m_collisionCategories = m_collisionCategories;
2850 prm.m_collisionFlags = m_collisionFlags;
2851
2852 if (prm.prim_geom != null)
2853 {
2854
2855 if (prm.m_NoColide)
2856 {
2857 d.GeomSetCategoryBits(prm.prim_geom, 0);
2858 d.GeomSetCollideBits(prm.prim_geom, 0);
2859 }
2860 else
2861 {
2862 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2863 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2864 }
2865 }
2866 prm.m_delaySelect = false;
2867 }
2868 }
2869// else if (_parent != null)
2870// ((OdePrim)_parent).ChildSelectedChange(true);
2871
2872
2873 if (prim_geom != null)
2874 {
2875 if (m_NoColide)
2876 {
2877 d.GeomSetCategoryBits(prim_geom, 0);
2878 d.GeomSetCollideBits(prim_geom, 0);
2879 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2880 {
2881 d.GeomSetCategoryBits(collide_geom, 0);
2882 d.GeomSetCollideBits(collide_geom, 0);
2883 }
2884
2885 }
2886 else
2887 {
2888 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2889 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2890 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2891 {
2892 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
2893 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2894 }
2895 }
2896 }
2897
2898 m_delaySelect = false;
2899 }
2900 else if(!m_isphysical)
2901 {
2902 m_delaySelect = true;
2903 }
2904 }
2905 else
2906 {
2907 if (!childPrim)
2908 {
2909 if (Body != IntPtr.Zero && !m_disabled)
2910 d.BodyEnable(Body);
2911 }
2912// else if (_parent != null)
2913// ((OdePrim)_parent).ChildSelectedChange(false);
2914
2915 UpdateCollisionCatFlags();
2916 ApplyCollisionCatFlags();
2917
2918 m_delaySelect = false;
2919 }
2920
2921 resetCollisionAccounting();
2922 }
2923
2924 private void changePosition(Vector3 newPos)
2925 {
2926 CheckDelaySelect();
2927 if (m_isphysical)
2928 {
2929 if (childPrim) // inertia is messed, must rebuild
2930 {
2931 if (m_building)
2932 {
2933 _position = newPos;
2934 }
2935
2936 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2937 {
2938 FixInertia(newPos);
2939 if (!d.BodyIsEnabled(Body))
2940 d.BodyEnable(Body);
2941 }
2942 }
2943 else
2944 {
2945 if (_position != newPos)
2946 {
2947 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2948 _position = newPos;
2949 }
2950 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2951 d.BodyEnable(Body);
2952 }
2953 }
2954 else
2955 {
2956 if (prim_geom != IntPtr.Zero)
2957 {
2958 if (newPos != _position)
2959 {
2960 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2961 _position = newPos;
2962
2963 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2964 }
2965 }
2966 }
2967 givefakepos--;
2968 if (givefakepos < 0)
2969 givefakepos = 0;
2970// changeSelectedStatus();
2971 resetCollisionAccounting();
2972 }
2973
2974 private void changeOrientation(Quaternion newOri)
2975 {
2976 CheckDelaySelect();
2977 if (m_isphysical)
2978 {
2979 if (childPrim) // inertia is messed, must rebuild
2980 {
2981 if (m_building)
2982 {
2983 _orientation = newOri;
2984 }
2985/*
2986 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2987 {
2988 FixInertia(_position, newOri);
2989 if (!d.BodyIsEnabled(Body))
2990 d.BodyEnable(Body);
2991 }
2992*/
2993 }
2994 else
2995 {
2996 if (newOri != _orientation)
2997 {
2998 d.Quaternion myrot = new d.Quaternion();
2999 myrot.X = newOri.X;
3000 myrot.Y = newOri.Y;
3001 myrot.Z = newOri.Z;
3002 myrot.W = newOri.W;
3003 d.GeomSetQuaternion(prim_geom, ref myrot);
3004 _orientation = newOri;
3005 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
3006 createAMotor(m_angularlock);
3007 }
3008 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3009 d.BodyEnable(Body);
3010 }
3011 }
3012 else
3013 {
3014 if (prim_geom != IntPtr.Zero)
3015 {
3016 if (newOri != _orientation)
3017 {
3018 d.Quaternion myrot = new d.Quaternion();
3019 myrot.X = newOri.X;
3020 myrot.Y = newOri.Y;
3021 myrot.Z = newOri.Z;
3022 myrot.W = newOri.W;
3023 d.GeomSetQuaternion(prim_geom, ref myrot);
3024 _orientation = newOri;
3025 }
3026 }
3027 }
3028 givefakeori--;
3029 if (givefakeori < 0)
3030 givefakeori = 0;
3031 resetCollisionAccounting();
3032 }
3033
3034 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
3035 {
3036 CheckDelaySelect();
3037 if (m_isphysical)
3038 {
3039 if (childPrim && m_building) // inertia is messed, must rebuild
3040 {
3041 _position = newPos;
3042 _orientation = newOri;
3043 }
3044 else
3045 {
3046 if (newOri != _orientation)
3047 {
3048 d.Quaternion myrot = new d.Quaternion();
3049 myrot.X = newOri.X;
3050 myrot.Y = newOri.Y;
3051 myrot.Z = newOri.Z;
3052 myrot.W = newOri.W;
3053 d.GeomSetQuaternion(prim_geom, ref myrot);
3054 _orientation = newOri;
3055 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
3056 createAMotor(m_angularlock);
3057 }
3058 if (_position != newPos)
3059 {
3060 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3061 _position = newPos;
3062 }
3063 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3064 d.BodyEnable(Body);
3065 }
3066 }
3067 else
3068 {
3069 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
3070 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
3071
3072 if (prim_geom != IntPtr.Zero)
3073 {
3074 if (newOri != _orientation)
3075 {
3076 d.Quaternion myrot = new d.Quaternion();
3077 myrot.X = newOri.X;
3078 myrot.Y = newOri.Y;
3079 myrot.Z = newOri.Z;
3080 myrot.W = newOri.W;
3081 d.GeomSetQuaternion(prim_geom, ref myrot);
3082 _orientation = newOri;
3083 }
3084
3085 if (newPos != _position)
3086 {
3087 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3088 _position = newPos;
3089
3090 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
3091 }
3092 }
3093 }
3094 givefakepos--;
3095 if (givefakepos < 0)
3096 givefakepos = 0;
3097 givefakeori--;
3098 if (givefakeori < 0)
3099 givefakeori = 0;
3100 resetCollisionAccounting();
3101 }
3102
3103
3104 private void changeDisable(bool disable)
3105 {
3106 if (disable)
3107 {
3108 if (!m_disabled)
3109 disableBodySoft();
3110 }
3111 else
3112 {
3113 if (m_disabled)
3114 enableBodySoft();
3115 }
3116 }
3117
3118 private void changePhysicsStatus(bool NewStatus)
3119 {
3120 CheckDelaySelect();
3121
3122 m_isphysical = NewStatus;
3123
3124 if (!childPrim)
3125 {
3126 if (NewStatus)
3127 {
3128 if (Body == IntPtr.Zero)
3129 MakeBody();
3130 }
3131 else
3132 {
3133 if (Body != IntPtr.Zero)
3134 {
3135 DestroyBody();
3136 }
3137 Stop();
3138 }
3139 }
3140
3141 resetCollisionAccounting();
3142 }
3143
3144 private void changeprimsizeshape()
3145 {
3146 CheckDelaySelect();
3147
3148 OdePrim parent = (OdePrim)_parent;
3149
3150 bool chp = childPrim;
3151
3152 if (chp)
3153 {
3154 if (parent != null)
3155 {
3156 parent.DestroyBody();
3157 }
3158 }
3159 else
3160 {
3161 DestroyBody();
3162 }
3163
3164 RemoveGeom();
3165
3166 // we don't need to do space calculation because the client sends a position update also.
3167 if (_size.X <= 0)
3168 _size.X = 0.01f;
3169 if (_size.Y <= 0)
3170 _size.Y = 0.01f;
3171 if (_size.Z <= 0)
3172 _size.Z = 0.01f;
3173 // Construction of new prim
3174
3175 CreateGeom();
3176
3177 if (prim_geom != IntPtr.Zero)
3178 {
3179 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3180 d.Quaternion myrot = new d.Quaternion();
3181 myrot.X = _orientation.X;
3182 myrot.Y = _orientation.Y;
3183 myrot.Z = _orientation.Z;
3184 myrot.W = _orientation.W;
3185 d.GeomSetQuaternion(prim_geom, ref myrot);
3186 }
3187
3188 if (m_isphysical)
3189 {
3190 if (chp)
3191 {
3192 if (parent != null)
3193 {
3194 parent.MakeBody();
3195 }
3196 }
3197 else
3198 MakeBody();
3199 }
3200
3201 else
3202 {
3203 UpdateCollisionCatFlags();
3204 ApplyCollisionCatFlags();
3205 }
3206
3207 resetCollisionAccounting();
3208 }
3209
3210 private void changeSize(Vector3 newSize)
3211 {
3212 _size = newSize;
3213 changeprimsizeshape();
3214 }
3215
3216 private void changeShape(PrimitiveBaseShape newShape)
3217 {
3218 if(newShape != null)
3219 _pbs = newShape;
3220 changeprimsizeshape();
3221 }
3222
3223 private void changeFloatOnWater(bool newval)
3224 {
3225 m_collidesWater = newval;
3226
3227 UpdateCollisionCatFlags();
3228 ApplyCollisionCatFlags();
3229 }
3230
3231 private void changeSetTorque(Vector3 newtorque)
3232 {
3233 if (!m_isSelected)
3234 {
3235 if (m_isphysical && Body != IntPtr.Zero)
3236 {
3237 if (m_disabled)
3238 enableBodySoft();
3239 else if (!d.BodyIsEnabled(Body))
3240 d.BodyEnable(Body);
3241
3242 }
3243 _torque = newtorque;
3244 }
3245 }
3246
3247 private void changeForce(Vector3 force)
3248 {
3249 m_force = force;
3250 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3251 d.BodyEnable(Body);
3252 }
3253
3254
3255 private void changeAddForce(Vector3 theforce)
3256 {
3257 m_forceacc += theforce;
3258 if (!m_isSelected)
3259 {
3260 lock (this)
3261 {
3262 //m_log.Info("[PHYSICS]: dequeing forcelist");
3263 if (m_isphysical && Body != IntPtr.Zero)
3264 {
3265 if (m_disabled)
3266 enableBodySoft();
3267 else if (!d.BodyIsEnabled(Body))
3268 d.BodyEnable(Body);
3269 }
3270 }
3271
3272 m_collisionscore = 0;
3273 }
3274 }
3275
3276 // actually angular impulse
3277 private void changeAddAngularImpulse(Vector3 aimpulse)
3278 {
3279 m_angularForceacc += aimpulse * m_invTimeStep;
3280 if (!m_isSelected)
3281 {
3282 lock (this)
3283 {
3284 if (m_isphysical && Body != IntPtr.Zero)
3285 {
3286 if (m_disabled)
3287 enableBodySoft();
3288 else if (!d.BodyIsEnabled(Body))
3289 d.BodyEnable(Body);
3290 }
3291 }
3292 m_collisionscore = 0;
3293 }
3294 }
3295
3296 private void changevelocity(Vector3 newVel)
3297 {
3298 float len = newVel.LengthSquared();
3299 if (len > 100000.0f) // limit to 100m/s
3300 {
3301 len = 100.0f / (float)Math.Sqrt(len);
3302 newVel *= len;
3303 }
3304
3305 if (!m_isSelected)
3306 {
3307 if (Body != IntPtr.Zero)
3308 {
3309 if (m_disabled)
3310 enableBodySoft();
3311 else if (!d.BodyIsEnabled(Body))
3312 d.BodyEnable(Body);
3313
3314 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3315 }
3316 //resetCollisionAccounting();
3317 }
3318 _velocity = newVel;
3319 }
3320
3321
3322 private void changeangvelocity(Vector3 newAngVel)
3323 {
3324 float len = newAngVel.LengthSquared();
3325 if (len > 144.0f) // limit to 12rad/s
3326 {
3327 len = 12.0f / (float)Math.Sqrt(len);
3328 newAngVel *= len;
3329 }
3330
3331 if (!m_isSelected)
3332 {
3333 if (Body != IntPtr.Zero)
3334 {
3335 if (m_disabled)
3336 enableBodySoft();
3337 else if (!d.BodyIsEnabled(Body))
3338 d.BodyEnable(Body);
3339
3340
3341 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
3342 }
3343 //resetCollisionAccounting();
3344 }
3345 m_rotationalVelocity = newAngVel;
3346 }
3347
3348 private void changeVolumedetetion(bool newVolDtc)
3349 {
3350 m_isVolumeDetect = newVolDtc;
3351 m_fakeisVolumeDetect = newVolDtc;
3352 UpdateCollisionCatFlags();
3353 ApplyCollisionCatFlags();
3354 }
3355
3356 protected void changeBuilding(bool newbuilding)
3357 {
3358 // Check if we need to do anything
3359 if (newbuilding == m_building)
3360 return;
3361
3362 if ((bool)newbuilding)
3363 {
3364 m_building = true;
3365 if (!childPrim)
3366 DestroyBody();
3367 }
3368 else
3369 {
3370 m_building = false;
3371 CheckDelaySelect();
3372 if (!childPrim)
3373 MakeBody();
3374 }
3375 if (!childPrim && childrenPrim.Count > 0)
3376 {
3377 foreach (OdePrim prm in childrenPrim)
3378 prm.changeBuilding(m_building); // call directly
3379 }
3380 }
3381
3382 public void changeSetVehicle(VehicleData vdata)
3383 {
3384 if (m_vehicle == null)
3385 m_vehicle = new ODEDynamics(this);
3386 m_vehicle.DoSetVehicle(vdata);
3387 }
3388 private void changeVehicleType(int value)
3389 {
3390 if (value == (int)Vehicle.TYPE_NONE)
3391 {
3392 if (m_vehicle != null)
3393 m_vehicle = null;
3394 }
3395 else
3396 {
3397 if (m_vehicle == null)
3398 m_vehicle = new ODEDynamics(this);
3399
3400 m_vehicle.ProcessTypeChange((Vehicle)value);
3401 }
3402 }
3403
3404 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3405 {
3406 if (m_vehicle == null)
3407 return;
3408
3409 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3410 }
3411
3412 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3413 {
3414 if (m_vehicle == null)
3415 return;
3416 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3417 }
3418
3419 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3420 {
3421 if (m_vehicle == null)
3422 return;
3423 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3424 }
3425
3426 private void changeVehicleFlags(strVehicleBoolParam bp)
3427 {
3428 if (m_vehicle == null)
3429 return;
3430 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3431 }
3432
3433 #endregion
3434
3435 public void Move()
3436 {
3437 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3438 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3439 {
3440 if (!d.BodyIsEnabled(Body))
3441 {
3442 // let vehicles sleep
3443 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3444 return;
3445
3446 if (++bodydisablecontrol < 20)
3447 return;
3448
3449
3450 d.BodyEnable(Body);
3451 }
3452
3453 bodydisablecontrol = 0;
3454
3455 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3456
3457 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3458 {
3459 // 'VEHICLES' are dealt with in ODEDynamics.cs
3460 m_vehicle.Step();
3461 return;
3462 }
3463
3464 float fx = 0;
3465 float fy = 0;
3466 float fz = 0;
3467
3468 float m_mass = _mass;
3469
3470 if (m_usePID && m_PIDTau > 0)
3471 {
3472 // for now position error
3473 _target_velocity =
3474 new Vector3(
3475 (m_PIDTarget.X - lpos.X),
3476 (m_PIDTarget.Y - lpos.Y),
3477 (m_PIDTarget.Z - lpos.Z)
3478 );
3479
3480 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3481 {
3482 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3483 d.BodySetLinearVel(Body, 0, 0, 0);
3484 return;
3485 }
3486 else
3487 {
3488 _zeroFlag = false;
3489
3490 float tmp = 1 / m_PIDTau;
3491 _target_velocity *= tmp;
3492
3493 // apply limits
3494 tmp = _target_velocity.Length();
3495 if (tmp > 50.0f)
3496 {
3497 tmp = 50 / tmp;
3498 _target_velocity *= tmp;
3499 }
3500 else if (tmp < 0.05f)
3501 {
3502 tmp = 0.05f / tmp;
3503 _target_velocity *= tmp;
3504 }
3505
3506 d.Vector3 vel = d.BodyGetLinearVel(Body);
3507 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3508 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3509 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3510// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3511 }
3512 } // end if (m_usePID)
3513
3514 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3515 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3516 {
3517
3518 // Non-Vehicles have a limited set of Hover options.
3519 // determine what our target height really is based on HoverType
3520
3521 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3522
3523 switch (m_PIDHoverType)
3524 {
3525 case PIDHoverType.Ground:
3526 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3527 break;
3528
3529 case PIDHoverType.GroundAndWater:
3530 m_waterHeight = _parent_scene.GetWaterLevel();
3531 if (m_groundHeight > m_waterHeight)
3532 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3533 else
3534 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3535 break;
3536 } // end switch (m_PIDHoverType)
3537
3538 // don't go underground unless volumedetector
3539
3540 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3541 {
3542 d.Vector3 vel = d.BodyGetLinearVel(Body);
3543
3544 fz = (m_targetHoverHeight - lpos.Z);
3545
3546 // if error is zero, use position control; otherwise, velocity control
3547 if (Math.Abs(fz) < 0.01f)
3548 {
3549 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3550 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3551 }
3552 else
3553 {
3554 _zeroFlag = false;
3555 fz /= m_PIDHoverTau;
3556
3557 float tmp = Math.Abs(fz);
3558 if (tmp > 50)
3559 fz = 50 * Math.Sign(fz);
3560 else if (tmp < 0.1)
3561 fz = 0.1f * Math.Sign(fz);
3562
3563 fz = ((fz - vel.Z) * m_invTimeStep);
3564 }
3565 }
3566 }
3567 else
3568 {
3569 float b = (1.0f - m_buoyancy);
3570 fx = _parent_scene.gravityx * b;
3571 fy = _parent_scene.gravityy * b;
3572 fz = _parent_scene.gravityz * b;
3573 }
3574
3575 fx *= m_mass;
3576 fy *= m_mass;
3577 fz *= m_mass;
3578
3579 // constant force
3580 fx += m_force.X;
3581 fy += m_force.Y;
3582 fz += m_force.Z;
3583
3584 fx += m_forceacc.X;
3585 fy += m_forceacc.Y;
3586 fz += m_forceacc.Z;
3587
3588 m_forceacc = Vector3.Zero;
3589
3590 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3591 if (fx != 0 || fy != 0 || fz != 0)
3592 {
3593 d.BodyAddForce(Body, fx, fy, fz);
3594 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3595 }
3596
3597 Vector3 trq;
3598
3599 trq = _torque;
3600 trq += m_angularForceacc;
3601 m_angularForceacc = Vector3.Zero;
3602 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3603 {
3604 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3605 }
3606 }
3607 else
3608 { // is not physical, or is not a body or is selected
3609 // _zeroPosition = d.BodyGetPosition(Body);
3610 return;
3611 //Console.WriteLine("Nothing " + Name);
3612
3613 }
3614 }
3615
3616 public void UpdatePositionAndVelocity()
3617 {
3618 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3619 {
3620 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3621 {
3622 bool lastZeroFlag = _zeroFlag;
3623
3624 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
3625
3626 // check outside region
3627 if (lpos.Z < -100 || lpos.Z > 100000f)
3628 {
3629 m_outbounds = true;
3630
3631 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3632 _acceleration.X = 0;
3633 _acceleration.Y = 0;
3634 _acceleration.Z = 0;
3635
3636 _velocity.X = 0;
3637 _velocity.Y = 0;
3638 _velocity.Z = 0;
3639 m_rotationalVelocity.X = 0;
3640 m_rotationalVelocity.Y = 0;
3641 m_rotationalVelocity.Z = 0;
3642
3643 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3644 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3645 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3646 m_lastposition = _position;
3647 m_lastorientation = _orientation;
3648
3649 base.RequestPhysicsterseUpdate();
3650
3651// throttleCounter = 0;
3652 _zeroFlag = true;
3653
3654 disableBodySoft(); // disable it and colisions
3655 base.RaiseOutOfBounds(_position);
3656 return;
3657 }
3658
3659 if (lpos.X < 0f)
3660 {
3661 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3662 m_outbounds = true;
3663 }
3664 else if (lpos.X > _parent_scene.WorldExtents.X)
3665 {
3666 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3667 m_outbounds = true;
3668 }
3669 if (lpos.Y < 0f)
3670 {
3671 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3672 m_outbounds = true;
3673 }
3674 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3675 {
3676 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3677 m_outbounds = true;
3678 }
3679
3680 if (m_outbounds)
3681 {
3682 m_lastposition = _position;
3683 m_lastorientation = _orientation;
3684
3685 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3686 m_rotationalVelocity.X = dtmp.X;
3687 m_rotationalVelocity.Y = dtmp.Y;
3688 m_rotationalVelocity.Z = dtmp.Z;
3689
3690 dtmp = d.BodyGetLinearVel(Body);
3691 _velocity.X = dtmp.X;
3692 _velocity.Y = dtmp.Y;
3693 _velocity.Z = dtmp.Z;
3694
3695 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3696 d.BodySetAngularVel(Body, 0, 0, 0);
3697 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3698 disableBodySoft(); // stop collisions
3699 UnSubscribeEvents();
3700
3701 base.RequestPhysicsterseUpdate();
3702 return;
3703 }
3704
3705 d.Quaternion ori;
3706 d.GeomCopyQuaternion(prim_geom, out ori);
3707
3708 // decide if moving
3709 // use positions since this are integrated quantities
3710 // tolerance values depende a lot on simulation noise...
3711 // use simple math.abs since we dont need to be exact
3712
3713 if (
3714 (Math.Abs(_position.X - lpos.X) < 0.001f)
3715 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3716 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3717 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3718 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3719 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3720 )
3721 {
3722 _zeroFlag = true;
3723 }
3724 else
3725 _zeroFlag = false;
3726
3727 // update velocities and aceleration
3728 if (!(_zeroFlag && lastZeroFlag))
3729 {
3730 d.Vector3 vel = d.BodyGetLinearVel(Body);
3731
3732 _acceleration = _velocity;
3733
3734 if ((Math.Abs(vel.X) < 0.001f) &&
3735 (Math.Abs(vel.Y) < 0.001f) &&
3736 (Math.Abs(vel.Z) < 0.001f))
3737 {
3738 _velocity = Vector3.Zero;
3739 float t = -m_invTimeStep;
3740 _acceleration = _acceleration * t;
3741 }
3742 else
3743 {
3744 _velocity.X = vel.X;
3745 _velocity.Y = vel.Y;
3746 _velocity.Z = vel.Z;
3747 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3748 }
3749
3750 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3751 (Math.Abs(_acceleration.Y) < 0.01f) &&
3752 (Math.Abs(_acceleration.Z) < 0.01f))
3753 {
3754 _acceleration = Vector3.Zero;
3755 }
3756
3757 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3758 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3759 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3760 )
3761 {
3762 m_rotationalVelocity = Vector3.Zero;
3763 }
3764 else
3765 {
3766 vel = d.BodyGetAngularVel(Body);
3767 m_rotationalVelocity.X = vel.X;
3768 m_rotationalVelocity.Y = vel.Y;
3769 m_rotationalVelocity.Z = vel.Z;
3770 }
3771 }
3772
3773 if (_zeroFlag)
3774 {
3775 if (lastZeroFlag)
3776 {
3777 _velocity = Vector3.Zero;
3778 _acceleration = Vector3.Zero;
3779 m_rotationalVelocity = Vector3.Zero;
3780 }
3781
3782 if (!m_lastUpdateSent)
3783 {
3784 base.RequestPhysicsterseUpdate();
3785 if (lastZeroFlag)
3786 m_lastUpdateSent = true;
3787 }
3788 return;
3789 }
3790
3791 _position.X = lpos.X;
3792 _position.Y = lpos.Y;
3793 _position.Z = lpos.Z;
3794
3795 _orientation.X = ori.X;
3796 _orientation.Y = ori.Y;
3797 _orientation.Z = ori.Z;
3798 _orientation.W = ori.W;
3799 base.RequestPhysicsterseUpdate();
3800 m_lastUpdateSent = false;
3801 }
3802 }
3803 }
3804
3805 internal static bool QuaternionIsFinite(Quaternion q)
3806 {
3807 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3808 return false;
3809 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3810 return false;
3811 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3812 return false;
3813 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3814 return false;
3815 return true;
3816 }
3817
3818 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
3819 {
3820 // assumes object center of mass is zero
3821 float smass = part.mass;
3822 theobj.mass -= smass;
3823
3824 smass *= 1.0f / (theobj.mass); ;
3825
3826 theobj.c.X -= part.c.X * smass;
3827 theobj.c.Y -= part.c.Y * smass;
3828 theobj.c.Z -= part.c.Z * smass;
3829
3830 theobj.I.M00 -= part.I.M00;
3831 theobj.I.M01 -= part.I.M01;
3832 theobj.I.M02 -= part.I.M02;
3833 theobj.I.M10 -= part.I.M10;
3834 theobj.I.M11 -= part.I.M11;
3835 theobj.I.M12 -= part.I.M12;
3836 theobj.I.M20 -= part.I.M20;
3837 theobj.I.M21 -= part.I.M21;
3838 theobj.I.M22 -= part.I.M22;
3839 }
3840
3841 private void donullchange()
3842 {
3843 }
3844
3845 public bool DoAChange(changes what, object arg)
3846 {
3847 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3848 {
3849 return false;
3850 }
3851
3852 // nasty switch
3853 switch (what)
3854 {
3855 case changes.Add:
3856 changeadd();
3857 break;
3858 case changes.Remove:
3859 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3860 //When we return true, it destroys all of the prims in the linkset anyway
3861 if (_parent != null)
3862 {
3863 OdePrim parent = (OdePrim)_parent;
3864 parent.ChildRemove(this, false);
3865 }
3866 else
3867 ChildRemove(this, false);
3868
3869 m_vehicle = null;
3870 RemoveGeom();
3871 m_targetSpace = IntPtr.Zero;
3872 UnSubscribeEvents();
3873 return true;
3874
3875 case changes.Link:
3876 OdePrim tmp = (OdePrim)arg;
3877 changeLink(tmp);
3878 break;
3879
3880 case changes.DeLink:
3881 changeLink(null);
3882 break;
3883
3884 case changes.Position:
3885 changePosition((Vector3)arg);
3886 break;
3887
3888 case changes.Orientation:
3889 changeOrientation((Quaternion)arg);
3890 break;
3891
3892 case changes.PosOffset:
3893 donullchange();
3894 break;
3895
3896 case changes.OriOffset:
3897 donullchange();
3898 break;
3899
3900 case changes.Velocity:
3901 changevelocity((Vector3)arg);
3902 break;
3903
3904// case changes.Acceleration:
3905// changeacceleration((Vector3)arg);
3906// break;
3907
3908 case changes.AngVelocity:
3909 changeangvelocity((Vector3)arg);
3910 break;
3911
3912 case changes.Force:
3913 changeForce((Vector3)arg);
3914 break;
3915
3916 case changes.Torque:
3917 changeSetTorque((Vector3)arg);
3918 break;
3919
3920 case changes.AddForce:
3921 changeAddForce((Vector3)arg);
3922 break;
3923
3924 case changes.AddAngForce:
3925 changeAddAngularImpulse((Vector3)arg);
3926 break;
3927
3928 case changes.AngLock:
3929 changeAngularLock((Vector3)arg);
3930 break;
3931
3932 case changes.Size:
3933 changeSize((Vector3)arg);
3934 break;
3935
3936 case changes.Shape:
3937 changeShape((PrimitiveBaseShape)arg);
3938 break;
3939
3940 case changes.CollidesWater:
3941 changeFloatOnWater((bool)arg);
3942 break;
3943
3944 case changes.VolumeDtc:
3945 changeVolumedetetion((bool)arg);
3946 break;
3947
3948 case changes.Phantom:
3949 changePhantomStatus((bool)arg);
3950 break;
3951
3952 case changes.Physical:
3953 changePhysicsStatus((bool)arg);
3954 break;
3955
3956 case changes.Selected:
3957 changeSelectedStatus((bool)arg);
3958 break;
3959
3960 case changes.disabled:
3961 changeDisable((bool)arg);
3962 break;
3963
3964 case changes.building:
3965 changeBuilding((bool)arg);
3966 break;
3967
3968 case changes.VehicleType:
3969 changeVehicleType((int)arg);
3970 break;
3971
3972 case changes.VehicleFlags:
3973 changeVehicleFlags((strVehicleBoolParam) arg);
3974 break;
3975
3976 case changes.VehicleFloatParam:
3977 changeVehicleFloatParam((strVehicleFloatParam) arg);
3978 break;
3979
3980 case changes.VehicleVectorParam:
3981 changeVehicleVectorParam((strVehicleVectorParam) arg);
3982 break;
3983
3984 case changes.VehicleRotationParam:
3985 changeVehicleRotationParam((strVehicleQuatParam) arg);
3986 break;
3987
3988 case changes.SetVehicle:
3989 changeSetVehicle((VehicleData) arg);
3990 break;
3991 case changes.Null:
3992 donullchange();
3993 break;
3994
3995 default:
3996 donullchange();
3997 break;
3998 }
3999 return false;
4000 }
4001
4002 public void AddChange(changes what, object arg)
4003 {
4004 _parent_scene.AddChange((PhysicsActor) this, what, arg);
4005 }
4006
4007
4008 private struct strVehicleBoolParam
4009 {
4010 public int param;
4011 public bool value;
4012 }
4013
4014 private struct strVehicleFloatParam
4015 {
4016 public int param;
4017 public float value;
4018 }
4019
4020 private struct strVehicleQuatParam
4021 {
4022 public int param;
4023 public Quaternion value;
4024 }
4025
4026 private struct strVehicleVectorParam
4027 {
4028 public int param;
4029 public Vector3 value;
4030 }
4031 }
4032}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..21fe9c0
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,624 @@
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 OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// Processes raycast requests as ODE is in a state to be able to do them.
43 /// This ensures that it's thread safe and there will be no conflicts.
44 /// Requests get returned by a different thread then they were requested by.
45 /// </summary>
46 public class ODERayCastRequestManager
47 {
48 /// <summary>
49 /// Pending ray requests
50 /// </summary>
51 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
52
53 /// <summary>
54 /// Scene that created this object.
55 /// </summary>
56 private OdeScene m_scene;
57
58 IntPtr ray; // the ray. we only need one for our lifetime
59
60 private const int ColisionContactGeomsPerTest = 5;
61 private const int DefaultMaxCount = 25;
62 private const int MaxTimePerCallMS = 30;
63
64 /// <summary>
65 /// ODE near callback delegate
66 /// </summary>
67 private d.NearCallback nearCallback;
68 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
69 private List<ContactResult> m_contactResults = new List<ContactResult>();
70 private RayFilterFlags CurrentRayFilter;
71 private int CurrentMaxCount;
72
73 public ODERayCastRequestManager(OdeScene pScene)
74 {
75 m_scene = pScene;
76 nearCallback = near;
77 ray = d.CreateRay(IntPtr.Zero, 1.0f);
78 d.GeomSetCategoryBits(ray,0);
79 }
80
81 /// <summary>
82 /// Queues request for a raycast to all world
83 /// </summary>
84 /// <param name="position">Origin of Ray</param>
85 /// <param name="direction">Ray direction</param>
86 /// <param name="length">Ray length</param>
87 /// <param name="retMethod">Return method to send the results</param>
88 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
89 {
90 ODERayRequest req = new ODERayRequest();
91 req.geom = IntPtr.Zero;
92 req.callbackMethod = retMethod;
93 req.Count = DefaultMaxCount;
94 req.length = length;
95 req.Normal = direction;
96 req.Origin = position;
97 req.filter = RayFilterFlags.AllPrims;
98
99 m_PendingRequests.Enqueue(req);
100 }
101
102 /// <summary>
103 /// Queues request for a raycast to particular part
104 /// </summary>
105 /// <param name="position">Origin of Ray</param>
106 /// <param name="direction">Ray direction</param>
107 /// <param name="length">Ray length</param>
108 /// <param name="retMethod">Return method to send the results</param>
109 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
110 {
111 ODERayRequest req = new ODERayRequest();
112 req.geom = geom;
113 req.callbackMethod = retMethod;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117 req.Count = DefaultMaxCount;
118 req.filter = RayFilterFlags.AllPrims;
119
120 m_PendingRequests.Enqueue(req);
121 }
122
123 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
124 {
125 ODERayRequest req = new ODERayRequest();
126 req.geom = IntPtr.Zero;
127 req.callbackMethod = retMethod;
128 req.Count = DefaultMaxCount;
129 req.length = length;
130 req.Normal = direction;
131 req.Origin = position;
132 req.filter = RayFilterFlags.AllPrims;
133
134 m_PendingRequests.Enqueue(req);
135 }
136
137 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
138 {
139 ODERayRequest req = new ODERayRequest();
140 req.geom = geom;
141 req.callbackMethod = retMethod;
142 req.length = length;
143 req.Normal = direction;
144 req.Origin = position;
145 req.Count = DefaultMaxCount;
146 req.filter = RayFilterFlags.AllPrims;
147
148 m_PendingRequests.Enqueue(req);
149 }
150
151 /// <summary>
152 /// Queues a raycast
153 /// </summary>
154 /// <param name="position">Origin of Ray</param>
155 /// <param name="direction">Ray normal</param>
156 /// <param name="length">Ray length</param>
157 /// <param name="count"></param>
158 /// <param name="retMethod">Return method to send the results</param>
159 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
160 {
161 ODERayRequest req = new ODERayRequest();
162 req.geom = IntPtr.Zero;
163 req.callbackMethod = retMethod;
164 req.length = length;
165 req.Normal = direction;
166 req.Origin = position;
167 req.Count = count;
168 req.filter = RayFilterFlags.AllPrims;
169
170 m_PendingRequests.Enqueue(req);
171 }
172
173
174 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod)
175 {
176 ODERayRequest req = new ODERayRequest();
177 req.geom = IntPtr.Zero;
178 req.callbackMethod = retMethod;
179 req.length = length;
180 req.Normal = direction;
181 req.Origin = position;
182 req.Count = count;
183 req.filter = filter;
184
185 m_PendingRequests.Enqueue(req);
186 }
187
188 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
189 {
190 ODERayRequest req = new ODERayRequest();
191 req.geom = geom;
192 req.callbackMethod = retMethod;
193 req.length = length;
194 req.Normal = direction;
195 req.Origin = position;
196 req.Count = count;
197 req.filter = RayFilterFlags.AllPrims;
198
199 m_PendingRequests.Enqueue(req);
200 }
201
202 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
203 {
204 ODERayRequest req = new ODERayRequest();
205 req.geom = IntPtr.Zero;
206 req.callbackMethod = retMethod;
207 req.length = length;
208 req.Normal = direction;
209 req.Origin = position;
210 req.Count = count;
211 req.filter = RayFilterFlags.AllPrims;
212
213 m_PendingRequests.Enqueue(req);
214 }
215
216 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
217 {
218 ODERayRequest req = new ODERayRequest();
219 req.geom = geom;
220 req.callbackMethod = retMethod;
221 req.length = length;
222 req.Normal = direction;
223 req.Origin = position;
224 req.Count = count;
225 req.filter = RayFilterFlags.AllPrims;
226
227 m_PendingRequests.Enqueue(req);
228 }
229
230 /// <summary>
231 /// Process all queued raycast requests
232 /// </summary>
233 /// <returns>Time in MS the raycasts took to process.</returns>
234 public int ProcessQueuedRequests()
235 {
236
237 if (m_PendingRequests.Count <= 0)
238 return 0;
239
240 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
241 // oops something got wrong or scene isn't ready still
242 {
243 m_PendingRequests.Clear();
244 return 0;
245 }
246
247 int time = Util.EnvironmentTickCount();
248
249 ODERayRequest req;
250 int closestHit;
251 int backfacecull;
252 CollisionCategories catflags;
253
254 while (m_PendingRequests.Dequeue(out req))
255 {
256 if (req.callbackMethod != null)
257 {
258 CurrentRayFilter = req.filter;
259 CurrentMaxCount = req.Count;
260
261 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
262 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
263
264 d.GeomRaySetLength(ray, req.length);
265 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
266 d.GeomRaySetParams(ray, 0, backfacecull);
267 d.GeomRaySetClosestHit(ray, closestHit);
268
269 if (req.callbackMethod is RaycastCallback)
270 // if we only want one get only one per colision pair saving memory
271 CurrentRayFilter |= RayFilterFlags.ClosestHit;
272
273 if (req.geom == IntPtr.Zero)
274 {
275 // translate ray filter to colision flags
276 catflags = 0;
277 if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
278 catflags |= CollisionCategories.VolumeDtc;
279 if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
280 catflags |= CollisionCategories.Phantom;
281 if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
282 catflags |= CollisionCategories.Character;
283 if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
284 catflags |= CollisionCategories.Geom;
285 if ((CurrentRayFilter & RayFilterFlags.land) != 0)
286 catflags |= CollisionCategories.Land;
287 if ((CurrentRayFilter & RayFilterFlags.water) != 0)
288 catflags |= CollisionCategories.Water;
289
290 if (catflags != 0)
291 doSpaceRay(req);
292 }
293 else
294 {
295 // if we select a geom don't use filters
296 d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
297 doGeomRay(req);
298 }
299 }
300
301 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
302 break;
303 }
304
305 lock (m_contactResults)
306 m_contactResults.Clear();
307
308 return Util.EnvironmentTickCountSubtract(time);
309 }
310 /// <summary>
311 /// Method that actually initiates the raycast with spaces
312 /// </summary>
313 /// <param name="req"></param>
314 ///
315
316 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
317 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
318
319 private void doSpaceRay(ODERayRequest req)
320 {
321 // Collide tests
322 if ((CurrentRayFilter & FilterActiveSpace) != 0)
323 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
324 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
325 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
326
327 if (req.callbackMethod is RaycastCallback)
328 {
329 // Define default results
330 bool hitYN = false;
331 uint hitConsumerID = 0;
332 float distance = float.MaxValue;
333 Vector3 closestcontact = Vector3.Zero;
334 Vector3 snormal = Vector3.Zero;
335
336 // Find closest contact and object.
337 lock (m_contactResults)
338 {
339 foreach (ContactResult cResult in m_contactResults)
340 {
341 if(cResult.Depth < distance)
342 {
343 closestcontact = cResult.Pos;
344 hitConsumerID = cResult.ConsumerID;
345 distance = cResult.Depth;
346 snormal = cResult.Normal;
347 }
348 }
349 m_contactResults.Clear();
350 }
351
352 if (distance > 0 && distance < float.MaxValue)
353 hitYN = true;
354 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
355 }
356 else
357 {
358 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
359 lock (m_PendingRequests)
360 {
361 cresult.AddRange(m_contactResults);
362 m_contactResults.Clear();
363 }
364 ((RayCallback)req.callbackMethod)(cresult);
365 }
366 }
367
368 /// <summary>
369 /// Method that actually initiates the raycast with a geom
370 /// </summary>
371 /// <param name="req"></param>
372 private void doGeomRay(ODERayRequest req)
373 {
374 // Collide test
375 d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
376
377 if (req.callbackMethod is RaycastCallback)
378 {
379 // Define default results
380 bool hitYN = false;
381 uint hitConsumerID = 0;
382 float distance = float.MaxValue;
383 Vector3 closestcontact = Vector3.Zero;
384 Vector3 snormal = Vector3.Zero;
385
386 // Find closest contact and object.
387 lock (m_contactResults)
388 {
389 foreach (ContactResult cResult in m_contactResults)
390 {
391 if(cResult.Depth < distance )
392 {
393 closestcontact = cResult.Pos;
394 hitConsumerID = cResult.ConsumerID;
395 distance = cResult.Depth;
396 snormal = cResult.Normal;
397 }
398 }
399 m_contactResults.Clear();
400 }
401
402 if (distance > 0 && distance < float.MaxValue)
403 hitYN = true;
404
405 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
406 }
407 else
408 {
409 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
410 lock (m_PendingRequests)
411 {
412 cresult.AddRange(m_contactResults);
413 m_contactResults.Clear();
414 }
415 ((RayCallback)req.callbackMethod)(cresult);
416 }
417 }
418
419 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
420 {
421 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
422 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
423 return false;
424
425 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
426 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
427 return true;
428 }
429
430 // This is the standard Near. g1 is the ray
431 private void near(IntPtr space, IntPtr g1, IntPtr g2)
432 {
433 if (g2 == IntPtr.Zero || g1 == g2)
434 return;
435
436 if (m_contactResults.Count >= CurrentMaxCount)
437 return;
438
439 if (d.GeomIsSpace(g2))
440 {
441 try
442 {
443 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
444 }
445 catch (Exception e)
446 {
447 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
448 }
449 return;
450 }
451
452 int count = 0;
453 try
454 {
455 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
456 }
457 catch (Exception e)
458 {
459 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
460 return;
461 }
462
463 if (count == 0)
464 return;
465
466 uint ID = 0;
467 PhysicsActor p2 = null;
468
469 m_scene.actor_name_map.TryGetValue(g2, out p2);
470
471 if (p2 == null)
472 {
473 /*
474 string name;
475
476 if (!m_scene.geom_name_map.TryGetValue(g2, out name))
477 return;
478
479 if (name == "Terrain")
480 {
481 // land colision
482 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
483 return;
484 }
485 else if (name == "Water")
486 {
487 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
488 return;
489 }
490 else
491 return;
492 */
493 return;
494 }
495 else
496 {
497 switch (p2.PhysicsActorType)
498 {
499 case (int)ActorTypes.Prim:
500
501 RayFilterFlags thisFlags;
502
503 if (p2.IsPhysical)
504 thisFlags = RayFilterFlags.physical;
505 else
506 thisFlags = RayFilterFlags.nonphysical;
507
508 if (p2.Phantom)
509 thisFlags |= RayFilterFlags.phantom;
510
511 if (p2.IsVolumeDtc)
512 thisFlags |= RayFilterFlags.volumedtc;
513
514 if ((thisFlags & CurrentRayFilter) == 0)
515 return;
516
517 ID = ((OdePrim)p2).LocalID;
518 break;
519
520 case (int)ActorTypes.Agent:
521
522 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
523 return;
524 else
525 ID = ((OdeCharacter)p2).LocalID;
526 break;
527
528 case (int)ActorTypes.Ground:
529
530 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
531 return;
532 break;
533
534 case (int)ActorTypes.Water:
535
536 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
537 return;
538 break;
539
540 default:
541 return;
542 break;
543 }
544 }
545
546 d.ContactGeom curcontact = new d.ContactGeom();
547
548 // closestHit for now only works for meshs, so must do it for others
549 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
550 {
551 // Loop all contacts, build results.
552 for (int i = 0; i < count; i++)
553 {
554 if (!GetCurContactGeom(i, ref curcontact))
555 break;
556
557 ContactResult collisionresult = new ContactResult();
558 collisionresult.ConsumerID = ID;
559 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
560 collisionresult.Depth = curcontact.depth;
561 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
562 curcontact.normal.Z);
563 lock (m_contactResults)
564 {
565 m_contactResults.Add(collisionresult);
566 if (m_contactResults.Count >= CurrentMaxCount)
567 return;
568 }
569 }
570 }
571 else
572 {
573 // keep only closest contact
574 ContactResult collisionresult = new ContactResult();
575 collisionresult.ConsumerID = ID;
576 collisionresult.Depth = float.MaxValue;
577
578 for (int i = 0; i < count; i++)
579 {
580 if (!GetCurContactGeom(i, ref curcontact))
581 break;
582
583 if (curcontact.depth < collisionresult.Depth)
584 {
585 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
586 collisionresult.Depth = curcontact.depth;
587 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
588 curcontact.normal.Z);
589 }
590 }
591
592 if (collisionresult.Depth != float.MaxValue)
593 {
594 lock (m_contactResults)
595 m_contactResults.Add(collisionresult);
596 }
597 }
598 }
599
600 /// <summary>
601 /// Dereference the creator scene so that it can be garbage collected if needed.
602 /// </summary>
603 internal void Dispose()
604 {
605 m_scene = null;
606 if (ray != IntPtr.Zero)
607 {
608 d.GeomDestroy(ray);
609 ray = IntPtr.Zero;
610 }
611 }
612 }
613
614 public struct ODERayRequest
615 {
616 public IntPtr geom;
617 public Vector3 Origin;
618 public Vector3 Normal;
619 public int Count;
620 public float length;
621 public object callbackMethod;
622 public RayFilterFlags filter;
623 }
624} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
new file mode 100644
index 0000000..403a4ce
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,2014 @@
1/*
2 * based on:
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 * changes by opensim team;
39 * changes by Aurora team http://www.aurora-sim.org/
40
41 * Revision/fixs by Ubit Umarov
42 */
43
44using System;
45using System.Runtime.InteropServices;
46using System.Security;
47
48namespace OdeAPI
49{
50//#if dDOUBLE
51// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
52// at least we save same memory and memory access time, FPU performance on intel usually is similar
53// using dReal = System.Double;
54//#else
55 using dReal = System.Single;
56//#endif
57
58 public static class d
59 {
60 public static dReal Infinity = dReal.MaxValue;
61 public static int NTotalBodies = 0;
62 public static int NTotalGeoms = 0;
63
64 public const uint CONTACTS_UNIMPORTANT = 0x80000000;
65
66 #region Flags and Enumerations
67
68 [Flags]
69 public enum AllocateODEDataFlags : uint
70 {
71 BasicData = 0,
72 CollisionData = 0x00000001,
73 All = ~0u
74 }
75
76 [Flags]
77 public enum IniteODEFlags : uint
78 {
79 dInitFlagManualThreadCleanup = 0x00000001
80 }
81
82 [Flags]
83 public enum ContactFlags : int
84 {
85 Mu2 = 0x001,
86 FDir1 = 0x002,
87 Bounce = 0x004,
88 SoftERP = 0x008,
89 SoftCFM = 0x010,
90 Motion1 = 0x020,
91 Motion2 = 0x040,
92 MotionN = 0x080,
93 Slip1 = 0x100,
94 Slip2 = 0x200,
95 Approx0 = 0x0000,
96 Approx1_1 = 0x1000,
97 Approx1_2 = 0x2000,
98 Approx1 = 0x3000
99 }
100
101 public enum GeomClassID : int
102 {
103 SphereClass,
104 BoxClass,
105 CapsuleClass,
106 CylinderClass,
107 PlaneClass,
108 RayClass,
109 ConvexClass,
110 GeomTransformClass,
111 TriMeshClass,
112 HeightfieldClass,
113 FirstSpaceClass,
114 SimpleSpaceClass = FirstSpaceClass,
115 HashSpaceClass,
116 QuadTreeSpaceClass,
117 LastSpaceClass = QuadTreeSpaceClass,
118 UbitTerrainClass,
119 FirstUserClass,
120 LastUserClass = FirstUserClass + MaxUserClasses - 1,
121 NumClasses,
122 MaxUserClasses = 5
123 }
124
125 public enum JointType : int
126 {
127 None,
128 Ball,
129 Hinge,
130 Slider,
131 Contact,
132 Universal,
133 Hinge2,
134 Fixed,
135 Null,
136 AMotor,
137 LMotor,
138 Plane2D
139 }
140
141 public enum JointParam : int
142 {
143 LoStop,
144 HiStop,
145 Vel,
146 FMax,
147 FudgeFactor,
148 Bounce,
149 CFM,
150 StopERP,
151 StopCFM,
152 SuspensionERP,
153 SuspensionCFM,
154 LoStop2 = 256,
155 HiStop2,
156 Vel2,
157 FMax2,
158 FudgeFactor2,
159 Bounce2,
160 CFM2,
161 StopERP2,
162 StopCFM2,
163 SuspensionERP2,
164 SuspensionCFM2,
165 LoStop3 = 512,
166 HiStop3,
167 Vel3,
168 FMax3,
169 FudgeFactor3,
170 Bounce3,
171 CFM3,
172 StopERP3,
173 StopCFM3,
174 SuspensionERP3,
175 SuspensionCFM3
176 }
177
178 public enum dSweepAndPruneAxis : int
179 {
180 XYZ = ((0)|(1<<2)|(2<<4)),
181 XZY = ((0)|(2<<2)|(1<<4)),
182 YXZ = ((1)|(0<<2)|(2<<4)),
183 YZX = ((1)|(2<<2)|(0<<4)),
184 ZXY = ((2)|(0<<2)|(1<<4)),
185 ZYX = ((2)|(1<<2)|(0<<4))
186 }
187
188 #endregion
189
190 #region Callbacks
191
192 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
193 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
194
195 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
196 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
197
198 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
199 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
200
201 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
202 public delegate ColliderFn GetColliderFnFn(int num);
203
204 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
205 public delegate void GeomDtorFn(IntPtr o);
206
207 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
208 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
209
210 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
211 public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z);
212
213 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
214 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
215
216 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
217 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
218
219 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
220 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
221
222 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
223 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
224
225 #endregion
226
227 #region Structs
228
229 [StructLayout(LayoutKind.Sequential)]
230 public struct AABB
231 {
232 public dReal MinX, MaxX;
233 public dReal MinY, MaxY;
234 public dReal MinZ, MaxZ;
235 }
236
237
238 [StructLayout(LayoutKind.Sequential)]
239 public struct Contact
240 {
241 public SurfaceParameters surface;
242 public ContactGeom geom;
243 public Vector3 fdir1;
244 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
245 }
246
247
248 [StructLayout(LayoutKind.Sequential)]
249 public struct ContactGeom
250 {
251
252 public Vector3 pos;
253 public Vector3 normal;
254 public dReal depth;
255 public IntPtr g1;
256 public IntPtr g2;
257 public int side1;
258 public int side2;
259 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
260 }
261
262 [StructLayout(LayoutKind.Sequential)]
263 public struct GeomClass
264 {
265 public int bytes;
266 public GetColliderFnFn collider;
267 public GetAABBFn aabb;
268 public AABBTestFn aabb_test;
269 public GeomDtorFn dtor;
270 }
271
272
273 [StructLayout(LayoutKind.Sequential)]
274 public struct JointFeedback
275 {
276 public Vector3 f1;
277 public Vector3 t1;
278 public Vector3 f2;
279 public Vector3 t2;
280 }
281
282
283 [StructLayout(LayoutKind.Sequential)]
284 public struct Mass
285 {
286 public dReal mass;
287 public Vector4 c;
288 public Matrix3 I;
289 }
290
291
292 [StructLayout(LayoutKind.Sequential)]
293 public struct Matrix3
294 {
295 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
296 {
297 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
298 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
299 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
300 }
301 public dReal M00, M10, M20;
302 private dReal _m30;
303 public dReal M01, M11, M21;
304 private dReal _m31;
305 public dReal M02, M12, M22;
306 private dReal _m32;
307 }
308
309 [StructLayout(LayoutKind.Sequential)]
310 public struct Matrix4
311 {
312 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
313 dReal m01, dReal m11, dReal m21, dReal m31,
314 dReal m02, dReal m12, dReal m22, dReal m32,
315 dReal m03, dReal m13, dReal m23, dReal m33)
316 {
317 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
318 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
319 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
320 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
321 }
322 public dReal M00, M10, M20, M30;
323 public dReal M01, M11, M21, M31;
324 public dReal M02, M12, M22, M32;
325 public dReal M03, M13, M23, M33;
326 }
327
328 [StructLayout(LayoutKind.Sequential)]
329 public struct Quaternion
330 {
331 public dReal W, X, Y, Z;
332 }
333
334
335 [StructLayout(LayoutKind.Sequential)]
336 public struct SurfaceParameters
337 {
338 public ContactFlags mode;
339 public dReal mu;
340 public dReal mu2;
341 public dReal bounce;
342 public dReal bounce_vel;
343 public dReal soft_erp;
344 public dReal soft_cfm;
345 public dReal motion1;
346 public dReal motion2;
347 public dReal motionN;
348 public dReal slip1;
349 public dReal slip2;
350 }
351
352
353 [StructLayout(LayoutKind.Sequential)]
354 public struct Vector3
355 {
356 public Vector3(dReal x, dReal y, dReal z)
357 {
358 X = x; Y = y; Z = z; _w = 0.0f;
359 }
360 public dReal X, Y, Z;
361 private dReal _w;
362 }
363
364
365 [StructLayout(LayoutKind.Sequential)]
366 public struct Vector4
367 {
368 public Vector4(dReal x, dReal y, dReal z, dReal w)
369 {
370 X = x; Y = y; Z = z; W = w;
371 }
372 public dReal X, Y, Z, W;
373 }
374
375 #endregion
376
377 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
378 public static extern int AllocateODEDataForThread(uint ODEInitFlags);
379
380 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
381 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
382
383 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
384 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
385
386 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
387 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
388
389 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
390 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
391
392 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
393 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
394
395 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
396 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
397
398 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
399 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
400
401 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
402 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
403
404 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
405 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
406
407 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
408 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
409
410 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
411 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
412
413 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
414 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
415
416 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
417 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
418
419 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
420 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
421
422 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
423 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
424
425 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
426 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
427
428 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
429 public static extern IntPtr BodyiCreate(IntPtr world);
430 public static IntPtr BodyCreate(IntPtr world)
431 {
432 NTotalBodies++;
433 return BodyiCreate(world);
434 }
435
436 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
437 public static extern void BodyiDestroy(IntPtr body);
438 public static void BodyDestroy(IntPtr body)
439 {
440 NTotalBodies--;
441 BodyiDestroy(body);
442 }
443
444 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
445 public static extern void BodyDisable(IntPtr body);
446
447 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
448 public static extern void BodyEnable(IntPtr body);
449
450 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
451 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
452
453 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
454 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
455
456 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
457 public static extern void BodyGetAutoDisableDefaults(IntPtr body);
458
459 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
460 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
461
462 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
463 public static extern int BodyGetAutoDisableSteps(IntPtr body);
464
465 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
466 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
467
468 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
469 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
470 public static Vector3 BodyGetAngularVel(IntPtr body)
471 {
472 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
473 }
474
475 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
476 public static extern IntPtr BodyGetData(IntPtr body);
477
478 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
479 public static extern int BodyGetFiniteRotationMode(IntPtr body);
480
481 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
482 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
483
484 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
485 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
486 public static Vector3 BodyGetForce(IntPtr body)
487 {
488 unsafe { return *(BodyGetForceUnsafe(body)); }
489 }
490
491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
492 public static extern bool BodyGetGravityMode(IntPtr body);
493
494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
495 public static extern int BodyGetGyroscopicMode(IntPtr body);
496
497 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
498 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
499
500 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
501 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
502 public static Vector3 BodyGetLinearVel(IntPtr body)
503 {
504 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
505 }
506
507 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
508 public static extern void BodyGetMass(IntPtr body, out Mass mass);
509
510 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
511 public static extern int BodyGetNumJoints(IntPtr body);
512
513 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
514 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
515
516 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
517 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
518 public static Vector3 BodyGetPosition(IntPtr body)
519 {
520 unsafe { return *(BodyGetPositionUnsafe(body)); }
521 }
522
523 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
524 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
525
526 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
527 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
528 public static Quaternion BodyGetQuaternion(IntPtr body)
529 {
530 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
531 }
532
533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
534 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
535
536 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
537 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
538
539 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
540 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
541 public static Matrix3 BodyGetRotation(IntPtr body)
542 {
543 unsafe { return *(BodyGetRotationUnsafe(body)); }
544 }
545
546 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
547 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
548 public static Vector3 BodyGetTorque(IntPtr body)
549 {
550 unsafe { return *(BodyGetTorqueUnsafe(body)); }
551 }
552
553 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
554 public static extern IntPtr BodyGetWorld(IntPtr body);
555
556 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
557 public static extern IntPtr BodyGetFirstGeom(IntPtr body);
558
559 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
560 public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
561
562
563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
564 public static extern bool BodyIsEnabled(IntPtr body);
565
566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
567 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
568
569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
570 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
571
572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
573 public static extern void BodySetAutoDisableDefaults(IntPtr body);
574
575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
576 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
577
578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
579 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
580
581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
582 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
583
584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
585 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
586
587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
588 public static extern void BodySetData(IntPtr body, IntPtr data);
589
590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
591 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
592
593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
594 public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
595
596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
597 public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
598
599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
600 public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
601
602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
603 public static extern dReal BodyGetLinearDamping(IntPtr body);
604
605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
606 public static extern dReal BodyGetAngularDamping(IntPtr body);
607
608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
609 public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
610
611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
612 public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
613
614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
615 public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
616
617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
618 public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
619
620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
621 public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
622
623 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
624 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
625
626 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
627 public static extern void BodySetGravityMode(IntPtr body, bool mode);
628
629 /// <summary>
630 /// Sets the Gyroscopic term status on the body specified.
631 /// </summary>
632 /// <param name="body">Pointer to body</param>
633 /// <param name="enabled">NonZero enabled, Zero disabled</param>
634 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
635 public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
636
637 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
638 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
639
640 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
641 public static extern void BodySetMass(IntPtr body, ref Mass mass);
642
643 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
644 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
645
646 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
647 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
648
649 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
650 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
651
652 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
653 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
654
655 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
656 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
657
658 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
659 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
660
661 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
662 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
663
664 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
665 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
666
667 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
668 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
669 ref Vector3 side1, ref Vector3 p2,
670 ref Matrix3 R2, ref Vector3 side2,
671 ref Vector3 normal, out dReal depth, out int return_code,
672 int maxc, out ContactGeom contact, int skip);
673
674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
675 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
676 ref Vector3 side1, ref Vector3 _p2,
677 ref Matrix3 R2, ref Vector3 side2);
678
679 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
680 public static extern void CleanupODEAllDataForThread();
681
682 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
683 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
684 ref Vector3 b1, ref Vector3 b2,
685 ref Vector3 cp1, ref Vector3 cp2);
686
687 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
688 public static extern void CloseODE();
689
690 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
691 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
692 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
693 public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
694
695 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
696 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
697
698 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
699 public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
700 public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
701 {
702 NTotalGeoms++;
703 return CreateiBox(space, lx, ly, lz);
704 }
705
706 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
707 public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
708 public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
709 {
710 NTotalGeoms++;
711 return CreateiCapsule(space, radius, length);
712 }
713
714 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
715 public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
716 public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
717 {
718 NTotalGeoms++;
719 return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
720 }
721
722 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
723 public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
724 public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
725 {
726 NTotalGeoms++;
727 return CreateiCylinder(space, radius, length);
728 }
729
730 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
731 public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
732 public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
733 {
734 NTotalGeoms++;
735 return CreateiHeightfield(space, data, bPlaceable);
736 }
737
738 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity]
739 public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable);
740 public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable)
741 {
742 NTotalGeoms++;
743 return CreateiUbitTerrain(space, data, bPlaceable);
744 }
745
746
747
748
749
750 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
751 public static extern IntPtr CreateiGeom(int classnum);
752 public static IntPtr CreateGeom(int classnum)
753 {
754 NTotalGeoms++;
755 return CreateiGeom(classnum);
756 }
757
758 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
759 public static extern int CreateGeomClass(ref GeomClass classptr);
760
761 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
762 public static extern IntPtr CreateGeomTransform(IntPtr space);
763
764 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
765 public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
766 public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
767 {
768 NTotalGeoms++;
769 return CreateiPlane(space, a, b, c, d);
770 }
771
772 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
773 public static extern IntPtr CreateiRay(IntPtr space, dReal length);
774 public static IntPtr CreateRay(IntPtr space, dReal length)
775 {
776 NTotalGeoms++;
777 return CreateiRay(space, length);
778 }
779
780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
781 public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
782 public static IntPtr CreateSphere(IntPtr space, dReal radius)
783 {
784 NTotalGeoms++;
785 return CreateiSphere(space, radius);
786 }
787
788 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
789 public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
790 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
791 public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
792 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
793 {
794 NTotalGeoms++;
795 return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
796 }
797 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
798 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
799
800 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
801 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
802
803 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
804 public static extern int FactorCholesky(ref dReal A00, int n);
805
806 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
807 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
808
809 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
810 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
811
812 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
813 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
814
815 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
816 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
817
818 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
819 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
820
821 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
822 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
823
824 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
825 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
826
827 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
828 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
829
830 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
831 public static extern void GeomClearOffset(IntPtr geom);
832
833 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
834 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
835
836 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
837 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
838
839 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
840 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
841
842 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
843 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
844
845 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
846 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
847
848 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
849 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
850
851 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
852 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
853
854 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
855 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
856
857 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
858 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
859
860 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
861 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
862
863 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
864 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
865
866 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
867 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
868
869 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
870 public static extern void GeomiDestroy(IntPtr geom);
871 public static void GeomDestroy(IntPtr geom)
872 {
873 NTotalGeoms--;
874 GeomiDestroy(geom);
875 }
876
877
878 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
879 public static extern void GeomDisable(IntPtr geom);
880
881 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
882 public static extern void GeomEnable(IntPtr geom);
883
884 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
885 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
886
887 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
888 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
889
890 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
891 public static extern IntPtr GeomGetBody(IntPtr geom);
892
893 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
894 public static extern uint GeomGetCategoryBits(IntPtr geom);
895
896 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
897 public static extern IntPtr GeomGetClassData(IntPtr geom);
898
899 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
900 public static extern uint GeomGetCollideBits(IntPtr geom);
901
902 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
903 public static extern GeomClassID GeomGetClass(IntPtr geom);
904
905 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
906 public static extern IntPtr GeomGetData(IntPtr geom);
907
908 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
909 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
910 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
911 {
912 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
913 }
914
915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
916 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
917 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
918 {
919 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
920 }
921
922 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
923 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
924 public static Vector3 GeomGetPosition(IntPtr geom)
925 {
926 unsafe { return *(GeomGetPositionUnsafe(geom)); }
927 }
928
929 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
930 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
931
932 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
933 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
934
935 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
936 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
937 public static Matrix3 GeomGetRotation(IntPtr geom)
938 {
939 unsafe { return *(GeomGetRotationUnsafe(geom)); }
940 }
941
942 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
943 public static extern IntPtr GeomGetSpace(IntPtr geom);
944
945 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
946 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
947 dReal width, dReal depth, int widthSamples, int depthSamples,
948 dReal scale, dReal offset, dReal thickness, int bWrap);
949
950 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
951 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
952 dReal width, dReal depth, int widthSamples, int depthSamples,
953 dReal scale, dReal offset, dReal thickness, int bWrap);
954
955 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
956 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
957 dReal width, dReal depth, int widthSamples, int depthSamples,
958 dReal scale, dReal offset, dReal thickness, int bWrap);
959
960 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
961 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
962 dReal width, dReal depth, int widthSamples, int depthSamples,
963 dReal scale, dReal offset, dReal thickness, int bWrap);
964
965 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
966 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
967 dReal width, dReal depth, int widthSamples, int depthSamples,
968 dReal scale, dReal offset, dReal thickness, int bWrap);
969
970 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
971 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
972 dReal width, dReal depth, int widthSamples, int depthSamples,
973 dReal scale, dReal offset, dReal thickness, int bWrap);
974
975 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
976 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
977 dReal width, dReal depth, int widthSamples, int depthSamples,
978 dReal scale, dReal offset, dReal thickness, int bWrap);
979
980 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
981 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
982 dReal width, dReal depth, int widthSamples, int depthSamples,
983 dReal scale, dReal offset, dReal thickness, int bWrap);
984
985
986
987 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
988 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
989 dReal width, dReal depth, int widthSamples, int depthSamples,
990 dReal scale, dReal offset, dReal thickness, int bWrap);
991
992 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
993 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
994 dReal width, dReal depth, int widthSamples, int depthSamples,
995 dReal scale, dReal offset, dReal thickness, int bWrap);
996
997 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
998 public static extern IntPtr GeomHeightfieldDataCreate();
999
1000 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
1001 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
1002
1003 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
1004 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1005
1006 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1007 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
1008
1009 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1010 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
1011
1012
1013 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1014 public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
1015 dReal sampleSize, int widthSamples, int depthSamples,
1016 dReal offset, dReal thickness, int bWrap);
1017
1018 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1019 public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
1020 dReal sampleSize, int widthSamples, int depthSamples,
1021 dReal thickness, int bWrap);
1022
1023 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
1024 public static extern IntPtr GeomUbitTerrainDataCreate();
1025
1026 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
1027 public static extern void GeomUbitTerrainDataDestroy(IntPtr d);
1028
1029 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
1030 public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1031
1032 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1033 public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g);
1034
1035 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1036 public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d);
1037
1038
1039 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
1040 public static extern bool GeomIsEnabled(IntPtr geom);
1041
1042 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
1043 public static extern bool GeomIsOffset(IntPtr geom);
1044
1045 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
1046 public static extern bool GeomIsSpace(IntPtr geom);
1047
1048 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1049 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
1050
1051 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1052 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
1053
1054 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
1055 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1056
1057 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
1058 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
1059
1060 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1061 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
1062
1063 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1064 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
1065
1066 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
1067 public static extern int GeomRayGetClosestHit(IntPtr ray);
1068
1069 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
1070 public static extern dReal GeomRayGetLength(IntPtr ray);
1071
1072 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
1073 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
1074
1075 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
1076 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
1077
1078 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
1079 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
1080
1081 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
1082 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
1083
1084 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
1085 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
1086
1087 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
1088 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
1089
1090 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
1091 public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
1092
1093 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1094 public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
1095
1096 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
1097 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
1098
1099 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
1100 public static extern void GeomSetData(IntPtr geom, IntPtr data);
1101
1102 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
1103 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1104
1105 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1106 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
1107
1108 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1109 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
1110
1111 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1112 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
1113
1114 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1115 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
1116
1117 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
1118 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
1119
1120 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1121 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
1122
1123 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1124 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
1125
1126 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1127 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
1128
1129 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1130 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
1131
1132 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
1133 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1134
1135 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1136 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
1137
1138 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1139 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
1140
1141 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1142 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
1143
1144 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1145 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
1146
1147 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
1148 public static extern dReal GeomSphereGetRadius(IntPtr geom);
1149
1150 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
1151 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1152
1153 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
1154 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
1155
1156 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
1157 public static extern int GeomTransformGetCleanup(IntPtr geom);
1158
1159 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
1160 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
1161
1162 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
1163 public static extern int GeomTransformGetInfo(IntPtr geom);
1164
1165 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
1166 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
1167
1168 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
1169 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
1170
1171 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
1172 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
1173
1174 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1175 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1176 double[] vertices, int vertexStride, int vertexCount,
1177 int[] indices, int indexCount, int triStride);
1178
1179 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1180 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1181 IntPtr vertices, int vertexStride, int vertexCount,
1182 IntPtr indices, int indexCount, int triStride);
1183
1184 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1185 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
1186 double[] vertices, int vertexStride, int vertexCount,
1187 int[] indices, int indexCount, int triStride,
1188 double[] normals);
1189
1190 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1191 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1192 IntPtr vertices, int vertexStride, int vertexCount,
1193 IntPtr indices, int indexCount, int triStride,
1194 IntPtr normals);
1195
1196 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1197 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1198 dReal[] vertices, int vertexStride, int vertexCount,
1199 int[] indices, int indexCount, int triStride);
1200
1201 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1202 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1203 IntPtr vertices, int vertexStride, int vertexCount,
1204 IntPtr indices, int indexCount, int triStride);
1205
1206 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1207 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1208 dReal[] vertices, int vertexStride, int vertexCount,
1209 int[] indices, int indexCount, int triStride,
1210 dReal[] normals);
1211
1212 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1213 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1214 IntPtr vertices, int vertexStride, int vertexCount,
1215 IntPtr indices, int indexCount, int triStride,
1216 IntPtr normals);
1217
1218 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1219 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1220 float[] vertices, int vertexStride, int vertexCount,
1221 int[] indices, int indexCount, int triStride);
1222
1223 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1224 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1225 IntPtr vertices, int vertexStride, int vertexCount,
1226 IntPtr indices, int indexCount, int triStride);
1227
1228 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1229 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1230 float[] vertices, int vertexStride, int vertexCount,
1231 int[] indices, int indexCount, int triStride,
1232 float[] normals);
1233
1234 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1235 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1236 IntPtr vertices, int vertexStride, int vertexCount,
1237 IntPtr indices, int indexCount, int triStride,
1238 IntPtr normals);
1239
1240 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1241 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1242
1243 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1244 public static extern IntPtr GeomTriMeshDataCreate();
1245
1246 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1247 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1248
1249 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1250 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1251
1252 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1253 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1254
1255 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1256 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1257
1258 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1259 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1260
1261 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1262 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1263
1264 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1265 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1266
1267 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1268 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1269
1270 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1271 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1272
1273 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1274 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1275 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1276 {
1277 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1278 }
1279
1280 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1281 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1282
1283 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1284 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1285
1286 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1287 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1288
1289 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1290 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1291
1292 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1293 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1294
1295 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1296 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1297
1298 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1299 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1300
1301 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1302 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1303
1304 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1305 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1306
1307 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1308 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1309
1310 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1311 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1312
1313 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1314 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1315
1316 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
1317 public static extern IntPtr iGetConfiguration();
1318
1319 public static string GetConfiguration()
1320 {
1321 IntPtr ptr = iGetConfiguration();
1322 string s = Marshal.PtrToStringAnsi(ptr);
1323 return s;
1324 }
1325
1326 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1327 public static extern IntPtr HashSpaceCreate(IntPtr space);
1328
1329 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1330 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1331
1332 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1333 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1334
1335 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1336 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1337
1338 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1339 public static extern void InitODE();
1340
1341 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
1342 public static extern int InitODE2(uint ODEInitFlags);
1343
1344 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1345 public static extern int IsPositiveDefinite(ref dReal A, int n);
1346
1347 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1348 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1349
1350 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1351 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1352
1353 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1354 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1355
1356 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1357 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1358
1359 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1360 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1361
1362 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1363 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1364
1365 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1366 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1367
1368 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1369 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1370
1371 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1372 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1373
1374 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1375 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1376
1377 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1378 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1379 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1380 public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
1381
1382 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1383 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1384
1385 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1386 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1387
1388 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1389 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1390
1391 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1392 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1393
1394 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1395 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1396
1397 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1398 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1399
1400 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1401 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1402
1403 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1404 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1405
1406 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1407 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1408
1409 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1410 public static extern void JointDestroy(IntPtr j);
1411
1412 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1413 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1414
1415 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1416 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1417
1418 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1419 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1420
1421 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1422 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1423
1424 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1425 public static extern int JointGetAMotorMode(IntPtr j);
1426
1427 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1428 public static extern int JointGetAMotorNumAxes(IntPtr j);
1429
1430 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1431 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1432
1433 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1434 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1435
1436 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1437 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1438
1439 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1440 public static extern IntPtr JointGetBody(IntPtr j);
1441
1442 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1443 public static extern IntPtr JointGetData(IntPtr j);
1444
1445 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1446 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1447 public static JointFeedback JointGetFeedback(IntPtr j)
1448 {
1449 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1450 }
1451
1452 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1453 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1454
1455 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1456 public static extern dReal JointGetHingeAngle(IntPtr j);
1457
1458 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1459 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1460
1461 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1462 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1463
1464 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1465 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1466
1467 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1468 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1469
1470 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1471 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1472
1473 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1474 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1475
1476 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1477 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1478
1479 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1480 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1481
1482 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1483 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1484
1485 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1486 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1487
1488 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1489 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1490
1491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1492 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1493
1494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1495 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1496
1497 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1498 public static extern int JointGetLMotorNumAxes(IntPtr j);
1499
1500 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1501 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1502
1503 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1504 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1505
1506 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1507 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1508
1509 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1510 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1511
1512 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1513 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1514
1515 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1516 public static extern dReal JointGetPRPosition(IntPtr j);
1517
1518 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1519 public static extern dReal JointGetPRPositionRate(IntPtr j);
1520
1521 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1522 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1523
1524 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1525 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1526
1527 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1528 public static extern dReal JointGetSliderPosition(IntPtr j);
1529
1530 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1531 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1532
1533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1534 public static extern JointType JointGetType(IntPtr j);
1535
1536 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1537 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1538
1539 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1540 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1541
1542 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1543 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1544
1545 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1546 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1547
1548 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1549 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1550
1551 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1552 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1553
1554 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1555 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1556
1557 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1558 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1559
1560 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1561 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1562
1563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1564 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1565
1566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1567 public static extern IntPtr JointGroupCreate(int max_size);
1568
1569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1570 public static extern void JointGroupDestroy(IntPtr group);
1571
1572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1573 public static extern void JointGroupEmpty(IntPtr group);
1574
1575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1576 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1577
1578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1579 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1580
1581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1582 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1583
1584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1585 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1586
1587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1588 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1589
1590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1591 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1592
1593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1594 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1595
1596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1597 public static extern void JointSetData(IntPtr j, IntPtr data);
1598
1599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1600 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1601
1602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1603 public static extern void JointSetFixed(IntPtr j);
1604
1605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1606 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1607
1608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1609 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1610
1611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1612 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1613
1614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1615 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1616
1617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1618 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1619
1620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1621 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1622
1623 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1624 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1625
1626 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1627 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1628
1629 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1630 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1631
1632 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1633 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1634
1635 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1636 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1637
1638 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1639 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1640
1641 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1642 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1643
1644 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1645 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1646
1647 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1648 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1649
1650 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1651 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1652
1653 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1654 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1655
1656 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1657 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1658
1659 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1660 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1661
1662 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1663 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1664
1665 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1666 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1667
1668 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1669 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1670
1671 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1672 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1673
1674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1675 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1676
1677 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1678 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1679
1680 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1681 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1682
1683 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1684 public static extern void MassAdd(ref Mass a, ref Mass b);
1685
1686 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1687 public static extern void MassAdjust(ref Mass m, dReal newmass);
1688
1689 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1690 public static extern bool MassCheck(ref Mass m);
1691
1692 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1693 public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
1694
1695 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1696 public static extern void MassRotate(ref Mass mass, ref dReal M00);
1697
1698 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1699 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1700
1701 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1702 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1703
1704 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1705 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1706
1707 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1708 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1709
1710 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1711 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1712
1713 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1714 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1715
1716 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1717 public static extern void MassSetParameters(out Mass mass, dReal themass,
1718 dReal cgx, dReal cgy, dReal cgz,
1719 dReal i11, dReal i22, dReal i33,
1720 dReal i12, dReal i13, dReal i23);
1721
1722 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1723 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1724
1725 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1726 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1727
1728 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1729 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1730
1731 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1732 public static extern void MassSetZero(out Mass mass);
1733
1734 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1735 public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
1736
1737 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1738 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1739
1740 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1741 private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
1742 public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
1743 {
1744 MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
1745 }
1746
1747 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1748 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1749
1750 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1751 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1752
1753 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1754 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1755
1756 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1757 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1758
1759 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1760 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1761
1762 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1763 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1764
1765 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1766 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1767
1768 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1769 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1770
1771 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1772 public static extern void QSetIdentity(out Quaternion q);
1773
1774 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1775 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1776
1777 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1778 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1779
1780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1781 public static extern dReal RandReal();
1782
1783 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1784 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1785
1786 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1787 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1788
1789 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1790 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1791
1792 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1793 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1794
1795 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1796 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1797
1798 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1799 public static extern void RSetIdentity(out Matrix3 R);
1800
1801 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1802 public static extern void SetValue(out dReal a, int n);
1803
1804 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1805 public static extern void SetZero(out dReal a, int n);
1806
1807 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1808 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1809
1810 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1811 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1812
1813 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1814 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1815
1816 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1817 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1818
1819 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1820 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1821
1822 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1823 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1824
1825 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
1826 public static extern bool SpaceLockQuery(IntPtr space);
1827
1828 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1829 public static extern void SpaceClean(IntPtr space);
1830
1831 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1832 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1833
1834 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1835 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1836
1837 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1838 public static extern void SpaceDestroy(IntPtr space);
1839
1840 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1841 public static extern bool SpaceGetCleanup(IntPtr space);
1842
1843 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1844 public static extern int SpaceGetNumGeoms(IntPtr space);
1845
1846 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1847 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1848
1849 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
1850 public static extern int SpaceGetSublevel(IntPtr space);
1851
1852 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1853 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1854
1855 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1856 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1857
1858 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1859 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1860
1861 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
1862 public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
1863
1864 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
1865 public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
1866
1867 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1868 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1869
1870 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1871 public static extern IntPtr WorldCreate();
1872
1873 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1874 public static extern void WorldDestroy(IntPtr world);
1875
1876 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1877 public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
1878
1879 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1880 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1881
1882 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1883 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1884
1885 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1886 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1887
1888 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1889 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1890
1891 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1892 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1893
1894 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1895 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1896
1897 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1898 public static extern dReal WorldGetCFM(IntPtr world);
1899
1900 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1901 public static extern dReal WorldGetERP(IntPtr world);
1902
1903 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1904 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1905
1906 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1907 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1908
1909 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1910 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1911
1912 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1913 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1914
1915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
1916 public static extern dReal WorldGetAngularDamping(IntPtr world);
1917
1918 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1919 public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
1920
1921 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
1922 public static extern dReal WorldGetLinearDamping(IntPtr world);
1923
1924 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1925 public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
1926
1927 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1928 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1929
1930 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1931 public static extern dReal WorldGetQuickStepW(IntPtr world);
1932
1933 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1934 public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
1935
1936 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1937 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1938
1939 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1940 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1941
1942 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1943 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1944
1945 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
1946 public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
1947
1948 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1949 public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
1950
1951 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1952 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1953
1954 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1955 public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
1956
1957 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1958 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1959
1960 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1961 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1962
1963 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1964 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1965
1966 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1967 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1968
1969 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1970 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1971
1972 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1973 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1974
1975 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1976 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1977
1978 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1979 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1980
1981 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
1982 public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
1983
1984 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1985 public static extern void WorldSetERP(IntPtr world, dReal erp);
1986
1987 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1988 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
1989
1990 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
1991 public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
1992
1993 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1994 public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
1995
1996 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1997 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
1998
1999 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
2000 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
2001
2002 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
2003 public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
2004
2005 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
2006 public static extern void WorldStep(IntPtr world, dReal stepsize);
2007
2008 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
2009 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
2010
2011 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
2012 public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
2013 }
2014}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..d32188e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
@@ -0,0 +1,90 @@
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 OdeAPI;
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 if (Util.IsWindows())
56 Util.LoadArchSpecificWindowsDll("ode.dll");
57
58 if (d.InitODE2(0) != 0)
59 {
60 if (d.AllocateODEDataForThread(~0U) == 0)
61 {
62 d.CloseODE();
63 return false;
64 }
65 return true;
66 }
67 return false;
68 }
69
70 public PhysicsScene GetScene(String sceneIdentifier)
71 {
72 if (m_scene == null)
73 {
74
75 m_scene = new OdeScene(sceneIdentifier);
76 }
77 return (m_scene);
78 }
79
80 public string GetName()
81 {
82 return ("UbitODE");
83 }
84
85 public void Dispose()
86 {
87 d.CloseODE();
88 }
89 }
90} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
new file mode 100644
index 0000000..f3ac3ca
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2816 @@
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 SPAM
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using OdeAPI;
40using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43
44namespace OpenSim.Region.Physics.OdePlugin
45{
46 public enum StatusIndicators : int
47 {
48 Generic = 0,
49 Start = 1,
50 End = 2
51 }
52
53 public struct sCollisionData
54 {
55 public uint ColliderLocalId;
56 public uint CollidedWithLocalId;
57 public int NumberOfCollisions;
58 public int CollisionType;
59 public int StatusIndicator;
60 public int lastframe;
61 }
62
63 // colision flags of things others can colide with
64 // rays, sensors, probes removed since can't be colided with
65 // The top space where things are placed provided further selection
66 // ie physical are in active space nonphysical in static
67 // this should be exclusive as possible
68
69 [Flags]
70 public enum CollisionCategories : uint
71 {
72 Disabled = 0,
73 //by 'things' types
74 Space = 0x01,
75 Geom = 0x02, // aka prim/part
76 Character = 0x04,
77 Land = 0x08,
78 Water = 0x010,
79
80 // by state
81 Phantom = 0x01000,
82 VolumeDtc = 0x02000,
83 Selected = 0x04000,
84 NoShape = 0x08000,
85
86
87 All = 0xffffffff
88 }
89
90 /// <summary>
91 /// Material type for a primitive
92 /// </summary>
93 public enum Material : int
94 {
95 /// <summary></summary>
96 Stone = 0,
97 /// <summary></summary>
98 Metal = 1,
99 /// <summary></summary>
100 Glass = 2,
101 /// <summary></summary>
102 Wood = 3,
103 /// <summary></summary>
104 Flesh = 4,
105 /// <summary></summary>
106 Plastic = 5,
107 /// <summary></summary>
108 Rubber = 6,
109
110 light = 7 // compatibility with old viewers
111 }
112
113 public enum changes : int
114 {
115 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
116 Remove,
117 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
118 // or removes from a object if arg is null
119 DeLink,
120 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
121 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
122 PosOffset, // not in use
123 // arg Vector3 new position in local coords. Changes prim position in object
124 OriOffset, // not in use
125 // arg Vector3 new position in local coords. Changes prim position in object
126 Velocity,
127 AngVelocity,
128 Acceleration,
129 Force,
130 Torque,
131 Momentum,
132
133 AddForce,
134 AddAngForce,
135 AngLock,
136
137 Size,
138 Shape,
139
140 CollidesWater,
141 VolumeDtc,
142
143 Physical,
144 Phantom,
145 Selected,
146 disabled,
147 building,
148
149 VehicleType,
150 VehicleFloatParam,
151 VehicleVectorParam,
152 VehicleRotationParam,
153 VehicleFlags,
154 SetVehicle,
155
156 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
157 }
158
159 public struct ODEchangeitem
160 {
161 public PhysicsActor actor;
162 public OdeCharacter character;
163 public changes what;
164 public Object arg;
165 }
166
167 public class OdeScene : PhysicsScene
168 {
169 private readonly ILog m_log;
170 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
171
172 public bool OdeUbitLib = false;
173// private int threadid = 0;
174 private Random fluidRandomizer = new Random(Environment.TickCount);
175
176 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
177 const float MaxERP = 0.8f;
178 const float minERP = 0.1f;
179 const float comumContactCFM = 0.0001f;
180
181 float frictionMovementMult = 0.8f;
182
183 float TerrainBounce = 0.1f;
184 float TerrainFriction = 0.3f;
185
186 public float AvatarFriction = 0;// 0.9f * 0.5f;
187
188 private const uint m_regionWidth = Constants.RegionSize;
189 private const uint m_regionHeight = Constants.RegionSize;
190
191 public float ODE_STEPSIZE = 0.020f;
192 public float HalfOdeStep = 0.01f;
193 public int odetimestepMS = 20; // rounded
194 private float metersInSpace = 25.6f;
195 private float m_timeDilation = 1.0f;
196
197 DateTime m_lastframe;
198
199 public float gravityx = 0f;
200 public float gravityy = 0f;
201 public float gravityz = -9.8f;
202
203 private float waterlevel = 0f;
204 private int framecount = 0;
205
206// private IntPtr WaterGeom = IntPtr.Zero;
207// private IntPtr WaterHeightmapData = IntPtr.Zero;
208// private GCHandle WaterMapHandler = new GCHandle();
209
210 public float avPIDD = 2200f; // make it visible
211 public float avPIDP = 900f; // make it visible
212 private float avCapRadius = 0.37f;
213 private float avDensity = 3f;
214 private float avMovementDivisorWalk = 1.3f;
215 private float avMovementDivisorRun = 0.8f;
216 private float minimumGroundFlightOffset = 3f;
217 public float maximumMassObject = 10000.01f;
218
219 public bool meshSculptedPrim = true;
220 public bool forceSimplePrimMeshing = false;
221
222 public float meshSculptLOD = 32;
223 public float MeshSculptphysicalLOD = 32;
224
225 public float geomDefaultDensity = 10.000006836f;
226
227 public int geomContactPointsStartthrottle = 3;
228 public int geomUpdatesPerThrottledUpdate = 15;
229
230 public float bodyPIDD = 35f;
231 public float bodyPIDG = 25;
232
233// public int geomCrossingFailuresBeforeOutofbounds = 6;
234
235 public int bodyFramesAutoDisable = 5;
236
237
238 private d.NearCallback nearCallback;
239
240 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
241 private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
242 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
243 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
244
245 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
246
247 /// <summary>
248 /// A list of actors that should receive collision events.
249 /// </summary>
250 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
251 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
252
253 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
254// public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
255 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
256
257 private float contactsurfacelayer = 0.002f;
258
259 private int contactsPerCollision = 80;
260 internal IntPtr ContactgeomsArray = IntPtr.Zero;
261 private IntPtr GlobalContactsArray = IntPtr.Zero;
262
263 const int maxContactsbeforedeath = 4000;
264 private volatile int m_global_contactcount = 0;
265
266
267 private IntPtr contactgroup;
268
269 public ContactData[] m_materialContactsData = new ContactData[8];
270
271 private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
272 private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
273 private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
274
275 private int m_physicsiterations = 10;
276 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
277// private PhysicsActor PANull = new NullPhysicsActor();
278 private float step_time = 0.0f;
279
280 public IntPtr world;
281
282
283 // split the spaces acording to contents type
284 // ActiveSpace contains characters and active prims
285 // StaticSpace contains land and other that is mostly static in enviroment
286 // this can contain subspaces, like the grid in staticspace
287 // as now space only contains this 2 top spaces
288
289 public IntPtr TopSpace; // the global space
290 public IntPtr ActiveSpace; // space for active prims
291 public IntPtr StaticSpace; // space for the static things around
292
293 // some speedup variables
294 private int spaceGridMaxX;
295 private int spaceGridMaxY;
296 private float spacesPerMeter;
297
298 // split static geometry collision into a grid as before
299 private IntPtr[,] staticPrimspace;
300 private IntPtr[] staticPrimspaceOffRegion;
301
302 public Object OdeLock;
303 private static Object SimulationLock;
304
305 public IMesher mesher;
306
307 private IConfigSource m_config;
308
309 public bool physics_logging = false;
310 public int physics_logging_interval = 0;
311 public bool physics_logging_append_existing_logfile = false;
312
313 private Vector3 m_worldOffset = Vector3.Zero;
314 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
315 private PhysicsScene m_parentScene = null;
316
317 private ODERayCastRequestManager m_rayCastManager;
318
319
320/* maybe needed if ode uses tls
321 private void checkThread()
322 {
323
324 int th = Thread.CurrentThread.ManagedThreadId;
325 if(th != threadid)
326 {
327 threadid = th;
328 d.AllocateODEDataForThread(~0U);
329 }
330 }
331 */
332 /// <summary>
333 /// Initiailizes the scene
334 /// Sets many properties that ODE requires to be stable
335 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
336 /// </summary>
337 public OdeScene(string sceneIdentifier)
338 {
339 m_log
340 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
341
342// checkThread();
343 Name = sceneIdentifier;
344
345 OdeLock = new Object();
346 SimulationLock = new Object();
347
348 nearCallback = near;
349
350 m_rayCastManager = new ODERayCastRequestManager(this);
351 lock (OdeLock)
352 {
353 // Create the world and the first space
354 try
355 {
356 world = d.WorldCreate();
357 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
358
359 // now the major subspaces
360 ActiveSpace = d.HashSpaceCreate(TopSpace);
361 StaticSpace = d.HashSpaceCreate(TopSpace);
362 }
363 catch
364 {
365 // i must RtC#FM
366 }
367
368 d.HashSpaceSetLevels(TopSpace, -2, 8);
369 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
370 d.HashSpaceSetLevels(StaticSpace, -2, 8);
371
372 // demote to second level
373 d.SpaceSetSublevel(ActiveSpace, 1);
374 d.SpaceSetSublevel(StaticSpace, 1);
375
376 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
377 CollisionCategories.Geom |
378 CollisionCategories.Character |
379 CollisionCategories.Phantom |
380 CollisionCategories.VolumeDtc
381 ));
382 d.GeomSetCollideBits(ActiveSpace, 0);
383 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
384 CollisionCategories.Geom |
385 CollisionCategories.Land |
386 CollisionCategories.Water |
387 CollisionCategories.Phantom |
388 CollisionCategories.VolumeDtc
389 ));
390 d.GeomSetCollideBits(StaticSpace, 0);
391
392 contactgroup = d.JointGroupCreate(0);
393 //contactgroup
394
395 d.WorldSetAutoDisableFlag(world, false);
396 }
397 }
398
399 // Initialize the mesh plugin
400// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
401 public override void Initialise(IMesher meshmerizer, IConfigSource config)
402 {
403// checkThread();
404 mesher = meshmerizer;
405 m_config = config;
406
407 string ode_config = d.GetConfiguration();
408 if (ode_config != null && ode_config != "")
409 {
410 m_log.WarnFormat("ODE configuration: {0}", ode_config);
411
412 if (ode_config.Contains("ODE_Ubit"))
413 {
414 OdeUbitLib = true;
415 }
416 }
417
418 /*
419 if (region != null)
420 {
421 WorldExtents.X = region.RegionSizeX;
422 WorldExtents.Y = region.RegionSizeY;
423 }
424 */
425
426 // Defaults
427
428 int contactsPerCollision = 80;
429
430 if (m_config != null)
431 {
432 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
433 if (physicsconfig != null)
434 {
435 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
436 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
437 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
438
439 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
440
441 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
442
443 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
444 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
445
446 avDensity = physicsconfig.GetFloat("av_density", avDensity);
447 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
448 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
449 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
450
451 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
452
453 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
454 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
455// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
456
457 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
458 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
459/*
460 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
461 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
462*/
463 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
464 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
465 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
466 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
467/*
468 if (Environment.OSVersion.Platform == PlatformID.Unix)
469 {
470 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
471 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
472 }
473 else
474 {
475
476 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
477 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
478 }
479*/
480 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
481 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
482 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
483
484 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
485 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
486 }
487 }
488
489 HalfOdeStep = ODE_STEPSIZE * 0.5f;
490 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
491
492 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
493 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
494
495 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
496 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
497
498 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
499 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
500
501 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
502 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
503
504 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
505 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
506
507 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
508 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
509
510 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
511 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
512
513 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
514 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
515
516 m_materialContactsData[(int)Material.light].mu = 0.0f;
517 m_materialContactsData[(int)Material.light].bounce = 0.0f;
518
519 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
520
521 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
522 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
523
524 d.WorldSetLinearDamping(world, 0.002f);
525 d.WorldSetAngularDamping(world, 0.002f);
526 d.WorldSetAngularDampingThreshold(world, 0f);
527 d.WorldSetLinearDampingThreshold(world, 0f);
528 d.WorldSetMaxAngularSpeed(world, 100f);
529
530 d.WorldSetCFM(world,1e-6f); // a bit harder than default
531 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
532 d.WorldSetERP(world, 0.6f); // higher than original
533
534 // Set how many steps we go without running collision testing
535 // This is in addition to the step size.
536 // Essentially Steps * m_physicsiterations
537 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
538
539 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
540
541 spacesPerMeter = 1 / metersInSpace;
542 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
543 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
544
545 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
546
547 // create all spaces now
548 int i, j;
549 IntPtr newspace;
550
551 for (i = 0; i < spaceGridMaxX; i++)
552 for (j = 0; j < spaceGridMaxY; j++)
553 {
554 newspace = d.HashSpaceCreate(StaticSpace);
555 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
556 waitForSpaceUnlock(newspace);
557 d.SpaceSetSublevel(newspace, 2);
558 d.HashSpaceSetLevels(newspace, -2, 8);
559 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
560 CollisionCategories.Geom |
561 CollisionCategories.Land |
562 CollisionCategories.Water |
563 CollisionCategories.Phantom |
564 CollisionCategories.VolumeDtc
565 ));
566 d.GeomSetCollideBits(newspace, 0);
567
568 staticPrimspace[i, j] = newspace;
569 }
570 // let this now be real maximum values
571 spaceGridMaxX--;
572 spaceGridMaxY--;
573
574 // create 4 off world spaces (x<0,x>max,y<0,y>max)
575 staticPrimspaceOffRegion = new IntPtr[4];
576
577 for (i = 0; i < 4; i++)
578 {
579 newspace = d.HashSpaceCreate(StaticSpace);
580 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
581 waitForSpaceUnlock(newspace);
582 d.SpaceSetSublevel(newspace, 2);
583 d.HashSpaceSetLevels(newspace, -2, 8);
584 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
585 CollisionCategories.Geom |
586 CollisionCategories.Land |
587 CollisionCategories.Water |
588 CollisionCategories.Phantom |
589 CollisionCategories.VolumeDtc
590 ));
591 d.GeomSetCollideBits(newspace, 0);
592
593 staticPrimspaceOffRegion[i] = newspace;
594 }
595
596 m_lastframe = DateTime.UtcNow;
597 }
598
599 internal void waitForSpaceUnlock(IntPtr space)
600 {
601 //if (space != IntPtr.Zero)
602 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
603 }
604
605 #region Collision Detection
606
607 // sets a global contact for a joint for contactgeom , and base contact description)
608
609 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
610 {
611 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
612 return IntPtr.Zero;
613
614 float erp = contactGeom.depth;
615 erp *= erpscale;
616 if (erp < minERP)
617 erp = minERP;
618 else if (erp > MaxERP)
619 erp = MaxERP;
620
621 float depth = contactGeom.depth * dscale;
622 if (depth > 0.5f)
623 depth = 0.5f;
624
625 d.Contact newcontact = new d.Contact();
626 newcontact.geom.depth = depth;
627 newcontact.geom.g1 = contactGeom.g1;
628 newcontact.geom.g2 = contactGeom.g2;
629 newcontact.geom.pos = contactGeom.pos;
630 newcontact.geom.normal = contactGeom.normal;
631 newcontact.geom.side1 = contactGeom.side1;
632 newcontact.geom.side2 = contactGeom.side2;
633
634 // this needs bounce also
635 newcontact.surface.mode = comumContactFlags;
636 newcontact.surface.mu = mu;
637 newcontact.surface.bounce = bounce;
638 newcontact.surface.soft_cfm = cfm;
639 newcontact.surface.soft_erp = erp;
640
641 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
642 Marshal.StructureToPtr(newcontact, contact, true);
643 return d.JointCreateContactPtr(world, contactgroup, contact);
644 }
645
646 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
647 {
648 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
649 return false;
650
651 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
652 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
653 return true;
654 }
655
656 /// <summary>
657 /// This is our near callback. A geometry is near a body
658 /// </summary>
659 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
660 /// <param name="g1">a geometry or space</param>
661 /// <param name="g2">another geometry or space</param>
662 ///
663
664 private void near(IntPtr space, IntPtr g1, IntPtr g2)
665 {
666 // no lock here! It's invoked from within Simulate(), which is thread-locked
667
668 if (m_global_contactcount >= maxContactsbeforedeath)
669 return;
670
671 // Test if we're colliding a geom with a space.
672 // If so we have to drill down into the space recursively
673
674 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
675 return;
676
677 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
678 {
679 // We'll be calling near recursivly if one
680 // of them is a space to find all of the
681 // contact points in the space
682 try
683 {
684 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
685 }
686 catch (AccessViolationException)
687 {
688 m_log.Warn("[PHYSICS]: Unable to collide test a space");
689 return;
690 }
691 //here one should check collisions of geoms inside a space
692 // but on each space we only should have geoms that not colide amoung each other
693 // so we don't dig inside spaces
694 return;
695 }
696
697 // get geom bodies to check if we already a joint contact
698 // guess this shouldn't happen now
699 IntPtr b1 = d.GeomGetBody(g1);
700 IntPtr b2 = d.GeomGetBody(g2);
701
702 // d.GeomClassID id = d.GeomGetClass(g1);
703
704 // Figure out how many contact points we have
705 int count = 0;
706 try
707 {
708 // Colliding Geom To Geom
709 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
710
711 if (g1 == g2)
712 return; // Can't collide with yourself
713
714 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
715 return;
716
717 if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
718 d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc)
719 {
720 int cflags;
721 unchecked
722 {
723 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
724 }
725 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
726 }
727 else
728 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
729 }
730 catch (SEHException)
731 {
732 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.");
733// ode.drelease(world);
734 base.TriggerPhysicsBasedRestart();
735 }
736 catch (Exception e)
737 {
738 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
739 return;
740 }
741
742 // contacts done
743 if (count == 0)
744 return;
745
746 // try get physical actors
747 PhysicsActor p1;
748 PhysicsActor p2;
749
750 if (!actor_name_map.TryGetValue(g1, out p1))
751 {
752 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
753 return;
754 }
755
756 if (!actor_name_map.TryGetValue(g2, out p2))
757 {
758 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
759 return;
760 }
761
762 // update actors collision score
763 if (p1.CollisionScore >= float.MaxValue - count)
764 p1.CollisionScore = 0;
765 p1.CollisionScore += count;
766
767 if (p2.CollisionScore >= float.MaxValue - count)
768 p2.CollisionScore = 0;
769 p2.CollisionScore += count;
770
771 // get first contact
772 d.ContactGeom curContact = new d.ContactGeom();
773 if (!GetCurContactGeom(0, ref curContact))
774 return;
775 // for now it's the one with max depth
776 ContactPoint maxDepthContact = new ContactPoint(
777 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
778 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
779 curContact.depth
780 );
781 // do volume detection case
782 if (
783 (p1.IsVolumeDtc || p2.IsVolumeDtc))
784 {
785 collision_accounting_events(p1, p2, maxDepthContact);
786 return;
787 }
788
789 // big messy collision analises
790
791 Vector3 normoverride = Vector3.Zero; //damm c#
792
793 float mu = 0;
794 float bounce = 0;
795 float cfm = 0.0001f;
796 float erpscale = 1.0f;
797 float dscale = 1.0f;
798 bool IgnoreNegSides = false;
799
800 ContactData contactdata1 = new ContactData(0, 0, false);
801 ContactData contactdata2 = new ContactData(0, 0, false);
802
803 bool dop1foot = false;
804 bool dop2foot = false;
805 bool ignore = false;
806 bool AvanormOverride = false;
807
808 switch (p1.PhysicsActorType)
809 {
810 case (int)ActorTypes.Agent:
811 {
812 AvanormOverride = true;
813 Vector3 tmp = p2.Position - p1.Position;
814 normoverride = p2.Velocity - p1.Velocity;
815 mu = normoverride.LengthSquared();
816
817 if (mu > 1e-6)
818 {
819 mu = 1.0f / (float)Math.Sqrt(mu);
820 normoverride *= mu;
821 mu = Vector3.Dot(tmp, normoverride);
822 if (mu > 0)
823 normoverride *= -1;
824 }
825 else
826 {
827 tmp.Normalize();
828 normoverride = -tmp;
829 }
830
831 switch (p2.PhysicsActorType)
832 {
833 case (int)ActorTypes.Agent:
834 p1.CollidingObj = true;
835 p2.CollidingObj = true;
836 break;
837
838 case (int)ActorTypes.Prim:
839 if (p2.Velocity.LengthSquared() > 0.0f)
840 p2.CollidingObj = true;
841 dop1foot = true;
842 break;
843
844 default:
845 ignore = true; // avatar to terrain and water ignored
846 break;
847 }
848 break;
849 }
850
851 case (int)ActorTypes.Prim:
852 switch (p2.PhysicsActorType)
853 {
854 case (int)ActorTypes.Agent:
855 AvanormOverride = true;
856
857 Vector3 tmp = p2.Position - p1.Position;
858 normoverride = p2.Velocity - p1.Velocity;
859 mu = normoverride.LengthSquared();
860 if (mu > 1e-6)
861 {
862 mu = 1.0f / (float)Math.Sqrt(mu);
863 normoverride *= mu;
864 mu = Vector3.Dot(tmp, normoverride);
865 if (mu > 0)
866 normoverride *= -1;
867 }
868 else
869 {
870 tmp.Normalize();
871 normoverride = -tmp;
872 }
873
874 bounce = 0;
875 mu = 0;
876 cfm = 0.0001f;
877
878 dop2foot = true;
879 if (p1.Velocity.LengthSquared() > 0.0f)
880 p1.CollidingObj = true;
881 break;
882
883 case (int)ActorTypes.Prim:
884 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
885 {
886 p1.CollidingObj = true;
887 p2.CollidingObj = true;
888 }
889 p1.getContactData(ref contactdata1);
890 p2.getContactData(ref contactdata2);
891 bounce = contactdata1.bounce * contactdata2.bounce;
892 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
893
894 cfm = p1.Mass;
895 if (cfm > p2.Mass)
896 cfm = p2.Mass;
897 dscale = 10 / cfm;
898 dscale = (float)Math.Sqrt(dscale);
899 if (dscale > 1.0f)
900 dscale = 1.0f;
901 erpscale = cfm * 0.01f;
902 cfm = 0.0001f / cfm;
903 if (cfm > 0.01f)
904 cfm = 0.01f;
905 else if (cfm < 0.00001f)
906 cfm = 0.00001f;
907
908 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
909 mu *= frictionMovementMult;
910
911 break;
912
913 case (int)ActorTypes.Ground:
914 p1.getContactData(ref contactdata1);
915 bounce = contactdata1.bounce * TerrainBounce;
916 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
917 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
918 mu *= frictionMovementMult;
919 p1.CollidingGround = true;
920
921 cfm = p1.Mass;
922 dscale = 10 / cfm;
923 dscale = (float)Math.Sqrt(dscale);
924 if (dscale > 1.0f)
925 dscale = 1.0f;
926 erpscale = cfm * 0.01f;
927 cfm = 0.0001f / cfm;
928 if (cfm > 0.01f)
929 cfm = 0.01f;
930 else if (cfm < 0.00001f)
931 cfm = 0.00001f;
932
933 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
934 {
935 if (curContact.side1 > 0)
936 IgnoreNegSides = true;
937 }
938 break;
939
940 case (int)ActorTypes.Water:
941 default:
942 ignore = true;
943 break;
944 }
945 break;
946
947 case (int)ActorTypes.Ground:
948 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
949 {
950 p2.CollidingGround = true;
951 p2.getContactData(ref contactdata2);
952 bounce = contactdata2.bounce * TerrainBounce;
953 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
954
955 cfm = p2.Mass;
956 dscale = 10 / cfm;
957 dscale = (float)Math.Sqrt(dscale);
958
959 if (dscale > 1.0f)
960 dscale = 1.0f;
961
962 erpscale = cfm * 0.01f;
963 cfm = 0.0001f / cfm;
964 if (cfm > 0.01f)
965 cfm = 0.01f;
966 else if (cfm < 0.00001f)
967 cfm = 0.00001f;
968
969 if (curContact.side1 > 0) // should be 2 ?
970 IgnoreNegSides = true;
971
972 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
973 mu *= frictionMovementMult;
974 }
975 else
976 ignore = true;
977 break;
978
979 case (int)ActorTypes.Water:
980 default:
981 break;
982 }
983 if (ignore)
984 return;
985
986 IntPtr Joint;
987
988 int i = 0;
989 while(true)
990 {
991
992 if (IgnoreNegSides && curContact.side1 < 0)
993 {
994 if (++i >= count)
995 break;
996
997 if (!GetCurContactGeom(i, ref curContact))
998 break;
999 }
1000 else
1001
1002 {
1003
1004 if (AvanormOverride)
1005 {
1006 if (curContact.depth > 0.3f)
1007 {
1008 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
1009 p1.IsColliding = true;
1010 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
1011 p2.IsColliding = true;
1012 curContact.normal.X = normoverride.X;
1013 curContact.normal.Y = normoverride.Y;
1014 curContact.normal.Z = normoverride.Z;
1015 }
1016
1017 else
1018 {
1019 if (dop1foot)
1020 {
1021 float sz = p1.Size.Z;
1022 Vector3 vtmp = p1.Position;
1023 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1024 if (ppos > 0f)
1025 {
1026 if (!p1.Flying)
1027 {
1028 d.AABB aabb;
1029 d.GeomGetAABB(g2, out aabb);
1030 float tmp = vtmp.Z - sz * .18f;
1031
1032 if (aabb.MaxZ < tmp)
1033 {
1034 vtmp.X = curContact.pos.X - vtmp.X;
1035 vtmp.Y = curContact.pos.Y - vtmp.Y;
1036 vtmp.Z = -0.2f;
1037 vtmp.Normalize();
1038 curContact.normal.X = vtmp.X;
1039 curContact.normal.Y = vtmp.Y;
1040 curContact.normal.Z = vtmp.Z;
1041 }
1042 }
1043 }
1044 else
1045 p1.IsColliding = true;
1046
1047 }
1048
1049 if (dop2foot)
1050 {
1051 float sz = p2.Size.Z;
1052 Vector3 vtmp = p2.Position;
1053 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1054 if (ppos > 0f)
1055 {
1056 if (!p2.Flying)
1057 {
1058 d.AABB aabb;
1059 d.GeomGetAABB(g1, out aabb);
1060 float tmp = vtmp.Z - sz * .18f;
1061
1062 if (aabb.MaxZ < tmp)
1063 {
1064 vtmp.X = curContact.pos.X - vtmp.X;
1065 vtmp.Y = curContact.pos.Y - vtmp.Y;
1066 vtmp.Z = -0.2f;
1067 vtmp.Normalize();
1068 curContact.normal.X = vtmp.X;
1069 curContact.normal.Y = vtmp.Y;
1070 curContact.normal.Z = vtmp.Z;
1071 }
1072 }
1073 }
1074 else
1075 p2.IsColliding = true;
1076
1077 }
1078 }
1079 }
1080
1081 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1082 d.JointAttach(Joint, b1, b2);
1083
1084 if (++m_global_contactcount >= maxContactsbeforedeath)
1085 break;
1086
1087 if (++i >= count)
1088 break;
1089
1090 if (!GetCurContactGeom(i, ref curContact))
1091 break;
1092
1093 if (curContact.depth > maxDepthContact.PenetrationDepth)
1094 {
1095 maxDepthContact.Position.X = curContact.pos.X;
1096 maxDepthContact.Position.Y = curContact.pos.Y;
1097 maxDepthContact.Position.Z = curContact.pos.Z;
1098 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1099 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1100 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1101 maxDepthContact.PenetrationDepth = curContact.depth;
1102 }
1103 }
1104 }
1105
1106 collision_accounting_events(p1, p2, maxDepthContact);
1107
1108/*
1109 if (notskipedcount > geomContactPointsStartthrottle)
1110 {
1111 // If there are more then 3 contact points, it's likely
1112 // that we've got a pile of objects, so ...
1113 // We don't want to send out hundreds of terse updates over and over again
1114 // so lets throttle them and send them again after it's somewhat sorted out.
1115 this needs checking so out for now
1116 if (b1 != IntPtr.Zero)
1117 p1.ThrottleUpdates = true;
1118 if (b2 != IntPtr.Zero)
1119 p2.ThrottleUpdates = true;
1120
1121 }
1122 */
1123 }
1124
1125 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1126 {
1127 uint obj2LocalID = 0;
1128
1129 bool p1events = p1.SubscribedEvents();
1130 bool p2events = p2.SubscribedEvents();
1131
1132 if (p1.IsVolumeDtc)
1133 p2events = false;
1134 if (p2.IsVolumeDtc)
1135 p1events = false;
1136
1137 if (!p2events && !p1events)
1138 return;
1139
1140 Vector3 vel = Vector3.Zero;
1141 if (p2 != null && p2.IsPhysical)
1142 vel = p2.Velocity;
1143
1144 if (p1 != null && p1.IsPhysical)
1145 vel -= p1.Velocity;
1146
1147 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1148
1149 switch ((ActorTypes)p1.PhysicsActorType)
1150 {
1151 case ActorTypes.Agent:
1152 case ActorTypes.Prim:
1153 {
1154 switch ((ActorTypes)p2.PhysicsActorType)
1155 {
1156 case ActorTypes.Agent:
1157 case ActorTypes.Prim:
1158 if (p2events)
1159 {
1160 AddCollisionEventReporting(p2);
1161 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
1162 }
1163 obj2LocalID = p2.ParentActor.LocalID;
1164 break;
1165
1166 case ActorTypes.Ground:
1167 case ActorTypes.Unknown:
1168 default:
1169 obj2LocalID = 0;
1170 break;
1171 }
1172 if (p1events)
1173 {
1174 contact.SurfaceNormal = -contact.SurfaceNormal;
1175 AddCollisionEventReporting(p1);
1176 p1.AddCollisionEvent(obj2LocalID, contact);
1177 }
1178 break;
1179 }
1180 case ActorTypes.Ground:
1181 case ActorTypes.Unknown:
1182 default:
1183 {
1184 if (p2events && !p2.IsVolumeDtc)
1185 {
1186 AddCollisionEventReporting(p2);
1187 p2.AddCollisionEvent(0, contact);
1188 }
1189 break;
1190 }
1191 }
1192 }
1193
1194 /// <summary>
1195 /// This is our collision testing routine in ODE
1196 /// </summary>
1197 /// <param name="timeStep"></param>
1198 private void collision_optimized()
1199 {
1200 lock (_characters)
1201 {
1202 try
1203 {
1204 foreach (OdeCharacter chr in _characters)
1205 {
1206 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1207 continue;
1208
1209 chr.IsColliding = false;
1210 // chr.CollidingGround = false; not done here
1211 chr.CollidingObj = false;
1212 // do colisions with static space
1213 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1214 }
1215 }
1216 catch (AccessViolationException)
1217 {
1218 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1219 }
1220
1221 }
1222
1223 lock (_activeprims)
1224 {
1225 foreach (OdePrim aprim in _activeprims)
1226 {
1227 aprim.CollisionScore = 0;
1228 aprim.IsColliding = false;
1229 }
1230 }
1231
1232 // collide active prims with static enviroment
1233 lock (_activegroups)
1234 {
1235 try
1236 {
1237 foreach (OdePrim prm in _activegroups)
1238 {
1239 if (!prm.m_outbounds)
1240 {
1241 if (d.BodyIsEnabled(prm.Body))
1242 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1243 }
1244 }
1245 }
1246 catch (AccessViolationException)
1247 {
1248 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1249 }
1250 }
1251 // finally colide active things amoung them
1252 try
1253 {
1254 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1255 }
1256 catch (AccessViolationException)
1257 {
1258 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1259 }
1260// _perloopContact.Clear();
1261 }
1262
1263 #endregion
1264 /// <summary>
1265 /// Add actor to the list that should receive collision events in the simulate loop.
1266 /// </summary>
1267 /// <param name="obj"></param>
1268 public void AddCollisionEventReporting(PhysicsActor obj)
1269 {
1270 if (!_collisionEventPrim.Contains(obj))
1271 _collisionEventPrim.Add(obj);
1272 }
1273
1274 /// <summary>
1275 /// Remove actor from the list that should receive collision events in the simulate loop.
1276 /// </summary>
1277 /// <param name="obj"></param>
1278 public void RemoveCollisionEventReporting(PhysicsActor obj)
1279 {
1280 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1281 _collisionEventPrimRemove.Add(obj);
1282 }
1283
1284
1285 #region Add/Remove Entities
1286
1287 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1288 {
1289 Vector3 pos;
1290 pos.X = position.X;
1291 pos.Y = position.Y;
1292 pos.Z = position.Z;
1293 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1294 newAv.Flying = isFlying;
1295 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1296
1297 return newAv;
1298 }
1299
1300 public void AddCharacter(OdeCharacter chr)
1301 {
1302 lock (_characters)
1303 {
1304 if (!_characters.Contains(chr))
1305 {
1306 _characters.Add(chr);
1307 if (chr.bad)
1308 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1309 }
1310 }
1311 }
1312
1313 public void RemoveCharacter(OdeCharacter chr)
1314 {
1315 lock (_characters)
1316 {
1317 if (_characters.Contains(chr))
1318 {
1319 _characters.Remove(chr);
1320 }
1321 }
1322 }
1323
1324 public void BadCharacter(OdeCharacter chr)
1325 {
1326 lock (_badCharacter)
1327 {
1328 if (!_badCharacter.Contains(chr))
1329 _badCharacter.Add(chr);
1330 }
1331 }
1332
1333 public override void RemoveAvatar(PhysicsActor actor)
1334 {
1335 //m_log.Debug("[PHYSICS]:ODELOCK");
1336 ((OdeCharacter) actor).Destroy();
1337 }
1338
1339 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1340 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1341 {
1342 Vector3 pos = position;
1343 Vector3 siz = size;
1344 Quaternion rot = rotation;
1345
1346 OdePrim newPrim;
1347 lock (OdeLock)
1348 {
1349 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);
1350
1351 lock (_prims)
1352 _prims.Add(newPrim);
1353 }
1354 return newPrim;
1355 }
1356
1357 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1358 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
1359 {
1360 Vector3 pos = position;
1361 Vector3 siz = size;
1362 Quaternion rot = rotation;
1363
1364 OdePrim newPrim;
1365 lock (OdeLock)
1366 {
1367 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);
1368
1369 lock (_prims)
1370 _prims.Add(newPrim);
1371 }
1372 return newPrim;
1373 }
1374
1375 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1376 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1377 {
1378 Vector3 pos = position;
1379 Vector3 siz = size;
1380 Quaternion rot = rotation;
1381
1382 OdePrim newPrim;
1383 lock (OdeLock)
1384 {
1385 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);
1386
1387 lock (_prims)
1388 _prims.Add(newPrim);
1389 }
1390 return newPrim;
1391 }
1392
1393 public void addActivePrim(OdePrim activatePrim)
1394 {
1395 // adds active prim..
1396 lock (_activeprims)
1397 {
1398 if (!_activeprims.Contains(activatePrim))
1399 _activeprims.Add(activatePrim);
1400 }
1401 }
1402
1403 public void addActiveGroups(OdePrim activatePrim)
1404 {
1405 lock (_activegroups)
1406 {
1407 if (!_activegroups.Contains(activatePrim))
1408 _activegroups.Add(activatePrim);
1409 }
1410 }
1411
1412 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1413 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1414 {
1415 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid);
1416 }
1417
1418
1419 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1420 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1421 {
1422#if SPAM
1423 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1424#endif
1425
1426 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1427 }
1428
1429 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1430 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1431 {
1432#if SPAM
1433 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1434#endif
1435
1436 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1437 }
1438
1439 public override float TimeDilation
1440 {
1441 get { return m_timeDilation; }
1442 }
1443
1444 public override bool SupportsNINJAJoints
1445 {
1446 get { return false; }
1447 }
1448
1449
1450 public void remActivePrim(OdePrim deactivatePrim)
1451 {
1452 lock (_activeprims)
1453 {
1454 _activeprims.Remove(deactivatePrim);
1455 }
1456 }
1457 public void remActiveGroup(OdePrim deactivatePrim)
1458 {
1459 lock (_activegroups)
1460 {
1461 _activegroups.Remove(deactivatePrim);
1462 }
1463 }
1464
1465 public override void RemovePrim(PhysicsActor prim)
1466 {
1467 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1468 // removed in the next physics simulate pass.
1469 if (prim is OdePrim)
1470 {
1471// lock (OdeLock)
1472 {
1473
1474 OdePrim p = (OdePrim)prim;
1475 p.setPrimForRemoval();
1476 }
1477 }
1478 }
1479 /// <summary>
1480 /// This is called from within simulate but outside the locked portion
1481 /// We need to do our own locking here
1482 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1483 /// Simulate() -- justincc).
1484 ///
1485 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1486 ///
1487 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1488 /// that the space was using.
1489 /// </summary>
1490 /// <param name="prim"></param>
1491 public void RemovePrimThreadLocked(OdePrim prim)
1492 {
1493 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1494 lock (prim)
1495 {
1496// RemoveCollisionEventReporting(prim);
1497 lock (_prims)
1498 _prims.Remove(prim);
1499 }
1500
1501 }
1502 #endregion
1503
1504 #region Space Separation Calculation
1505
1506 /// <summary>
1507 /// Called when a static prim moves or becomes static
1508 /// Places the prim in a space one the static sub-spaces grid
1509 /// </summary>
1510 /// <param name="geom">the pointer to the geom that moved</param>
1511 /// <param name="pos">the position that the geom moved to</param>
1512 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1513 /// <returns>a pointer to the new space it's in</returns>
1514 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1515 {
1516 // moves a prim into another static sub-space or from another space into a static sub-space
1517
1518 // Called ODEPrim so
1519 // it's already in locked space.
1520
1521 if (geom == IntPtr.Zero) // shouldn't happen
1522 return IntPtr.Zero;
1523
1524 // get the static sub-space for current position
1525 IntPtr newspace = calculateSpaceForGeom(pos);
1526
1527 if (newspace == currentspace) // if we are there all done
1528 return newspace;
1529
1530 // else remove it from its current space
1531 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1532 {
1533 if (d.GeomIsSpace(currentspace))
1534 {
1535 waitForSpaceUnlock(currentspace);
1536 d.SpaceRemove(currentspace, geom);
1537
1538 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1539 {
1540 d.SpaceDestroy(currentspace);
1541 }
1542 }
1543 else
1544 {
1545 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1546 " Geom:" + geom);
1547 }
1548 }
1549 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1550 {
1551 currentspace = d.GeomGetSpace(geom);
1552 if (currentspace != IntPtr.Zero)
1553 {
1554 if (d.GeomIsSpace(currentspace))
1555 {
1556 waitForSpaceUnlock(currentspace);
1557 d.SpaceRemove(currentspace, geom);
1558
1559 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1560 {
1561 d.SpaceDestroy(currentspace);
1562 }
1563
1564 }
1565 }
1566 }
1567
1568 // put the geom in the newspace
1569 waitForSpaceUnlock(newspace);
1570 d.SpaceAdd(newspace, geom);
1571
1572 // let caller know this newspace
1573 return newspace;
1574 }
1575
1576 /// <summary>
1577 /// Calculates the space the prim should be in by its position
1578 /// </summary>
1579 /// <param name="pos"></param>
1580 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1581 public IntPtr calculateSpaceForGeom(Vector3 pos)
1582 {
1583 int x, y;
1584
1585 if (pos.X < 0)
1586 return staticPrimspaceOffRegion[0];
1587
1588 if (pos.Y < 0)
1589 return staticPrimspaceOffRegion[2];
1590
1591 x = (int)(pos.X * spacesPerMeter);
1592 if (x > spaceGridMaxX)
1593 return staticPrimspaceOffRegion[1];
1594
1595 y = (int)(pos.Y * spacesPerMeter);
1596 if (y > spaceGridMaxY)
1597 return staticPrimspaceOffRegion[3];
1598
1599 return staticPrimspace[x, y];
1600 }
1601
1602 #endregion
1603
1604 /// <summary>
1605 /// Routine to figure out if we need to mesh this prim with our mesher
1606 /// </summary>
1607 /// <param name="pbs"></param>
1608 /// <returns></returns>
1609 public bool needsMeshing(PrimitiveBaseShape pbs)
1610 {
1611 // check sculpts or meshs
1612 if (pbs.SculptEntry)
1613 {
1614 if (meshSculptedPrim)
1615 return true;
1616
1617 if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
1618 return true;
1619
1620 return false;
1621 }
1622
1623 if (forceSimplePrimMeshing)
1624 return true;
1625
1626 // 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
1627
1628 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1629 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1630 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1631 {
1632
1633 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1634 && pbs.ProfileHollow == 0
1635 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1636 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1637 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1638 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1639 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1640 {
1641#if SPAM
1642 m_log.Warn("NonMesh");
1643#endif
1644 return false;
1645 }
1646 }
1647
1648 // following code doesn't give meshs to boxes and spheres ever
1649 // and it's odd.. so for now just return true if asked to force meshs
1650 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1651
1652 int iPropertiesNotSupportedDefault = 0;
1653
1654 if (pbs.ProfileHollow != 0)
1655 iPropertiesNotSupportedDefault++;
1656
1657 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1658 iPropertiesNotSupportedDefault++;
1659
1660 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1661 iPropertiesNotSupportedDefault++;
1662
1663 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1664 iPropertiesNotSupportedDefault++;
1665
1666 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1667 iPropertiesNotSupportedDefault++;
1668
1669 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1670 iPropertiesNotSupportedDefault++;
1671
1672 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1673 iPropertiesNotSupportedDefault++;
1674
1675 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))
1676 iPropertiesNotSupportedDefault++;
1677
1678 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1679 iPropertiesNotSupportedDefault++;
1680
1681 // test for torus
1682 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1683 {
1684 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1685 {
1686 iPropertiesNotSupportedDefault++;
1687 }
1688 }
1689 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1690 {
1691 if (pbs.PathCurve == (byte)Extrusion.Straight)
1692 {
1693 iPropertiesNotSupportedDefault++;
1694 }
1695
1696 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1697 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1698 {
1699 iPropertiesNotSupportedDefault++;
1700 }
1701 }
1702 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1703 {
1704 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1705 {
1706 iPropertiesNotSupportedDefault++;
1707 }
1708 }
1709 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1710 {
1711 if (pbs.PathCurve == (byte)Extrusion.Straight)
1712 {
1713 iPropertiesNotSupportedDefault++;
1714 }
1715 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1716 {
1717 iPropertiesNotSupportedDefault++;
1718 }
1719 }
1720
1721 if (iPropertiesNotSupportedDefault == 0)
1722 {
1723#if SPAM
1724 m_log.Warn("NonMesh");
1725#endif
1726 return false;
1727 }
1728#if SPAM
1729 m_log.Debug("Mesh");
1730#endif
1731 return true;
1732 }
1733
1734 /// <summary>
1735 /// Called to queue a change to a actor
1736 /// to use in place of old taint mechanism so changes do have a time sequence
1737 /// </summary>
1738
1739 public void AddChange(PhysicsActor actor, changes what, Object arg)
1740 {
1741 ODEchangeitem item = new ODEchangeitem();
1742 item.actor = actor;
1743 item.what = what;
1744 item.arg = arg;
1745 ChangesQueue.Enqueue(item);
1746 }
1747
1748 /// <summary>
1749 /// Called after our prim properties are set Scale, position etc.
1750 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1751 /// This assures us that we have no race conditions
1752 /// </summary>
1753 /// <param name="prim"></param>
1754 public override void AddPhysicsActorTaint(PhysicsActor prim)
1755 {
1756 }
1757
1758 /// <summary>
1759 /// This is our main simulate loop
1760 /// It's thread locked by a Mutex in the scene.
1761 /// It holds Collisions, it instructs ODE to step through the physical reactions
1762 /// It moves the objects around in memory
1763 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1764 /// </summary>
1765 /// <param name="timeStep"></param>
1766 /// <returns></returns>
1767 public override float Simulate(float timeStep)
1768 {
1769
1770 DateTime now = DateTime.UtcNow;
1771 TimeSpan SinceLastFrame = now - m_lastframe;
1772 m_lastframe = now;
1773 timeStep = (float)SinceLastFrame.TotalSeconds;
1774
1775 // acumulate time so we can reduce error
1776 step_time += timeStep;
1777
1778 if (step_time < HalfOdeStep)
1779 return 0;
1780
1781 if (framecount < 0)
1782 framecount = 0;
1783
1784 framecount++;
1785
1786 int curphysiteractions;
1787
1788 // if in trouble reduce step resolution
1789 if (step_time >= m_SkipFramesAtms)
1790 curphysiteractions = m_physicsiterations / 2;
1791 else
1792 curphysiteractions = m_physicsiterations;
1793
1794 int nodeframes = 0;
1795
1796// checkThread();
1797
1798 lock (SimulationLock)
1799 lock(OdeLock)
1800 {
1801 if (world == IntPtr.Zero)
1802 return 0;
1803
1804 // adjust number of iterations per step
1805
1806// try
1807// {
1808 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1809/* }
1810 catch (StackOverflowException)
1811 {
1812 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1813// ode.drelease(world);
1814 base.TriggerPhysicsBasedRestart();
1815 }
1816*/
1817 while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
1818 {
1819 try
1820 {
1821 // clear pointer/counter to contacts to pass into joints
1822 m_global_contactcount = 0;
1823
1824 ODEchangeitem item;
1825
1826 if(ChangesQueue.Count >0)
1827 {
1828 int ttmpstart = Util.EnvironmentTickCount();
1829 int ttmp;
1830
1831 while(ChangesQueue.Dequeue(out item))
1832 {
1833 if (item.actor != null)
1834 {
1835 try
1836 {
1837 if (item.actor is OdeCharacter)
1838 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1839 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1840 RemovePrimThreadLocked((OdePrim)item.actor);
1841 }
1842 catch
1843 {
1844 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1845 };
1846 }
1847 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1848 if (ttmp > 20)
1849 break;
1850 }
1851 }
1852
1853 // Move characters
1854 lock (_characters)
1855 {
1856 List<OdeCharacter> defects = new List<OdeCharacter>();
1857 foreach (OdeCharacter actor in _characters)
1858 {
1859 if (actor != null)
1860 actor.Move(ODE_STEPSIZE, defects);
1861 }
1862 if (defects.Count != 0)
1863 {
1864 foreach (OdeCharacter defect in defects)
1865 {
1866 RemoveCharacter(defect);
1867 }
1868 defects.Clear();
1869 }
1870 }
1871
1872 // Move other active objects
1873 lock (_activegroups)
1874 {
1875 foreach (OdePrim aprim in _activegroups)
1876 {
1877 aprim.Move();
1878 }
1879 }
1880
1881 //if ((framecount % m_randomizeWater) == 0)
1882 // randomizeWater(waterlevel);
1883
1884 m_rayCastManager.ProcessQueuedRequests();
1885
1886 collision_optimized();
1887
1888 foreach (PhysicsActor obj in _collisionEventPrim)
1889 {
1890 if (obj == null)
1891 continue;
1892
1893 switch ((ActorTypes)obj.PhysicsActorType)
1894 {
1895 case ActorTypes.Agent:
1896 OdeCharacter cobj = (OdeCharacter)obj;
1897 cobj.AddCollisionFrameTime((int)(odetimestepMS));
1898 cobj.SendCollisions();
1899 break;
1900
1901 case ActorTypes.Prim:
1902 OdePrim pobj = (OdePrim)obj;
1903 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1904 if (!pobj.m_outbounds)
1905 {
1906 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1907 pobj.SendCollisions();
1908 }
1909 break;
1910 }
1911 }
1912
1913 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1914 _collisionEventPrim.Remove(obj);
1915
1916 _collisionEventPrimRemove.Clear();
1917
1918 // do a ode simulation step
1919 d.WorldQuickStep(world, ODE_STEPSIZE);
1920 d.JointGroupEmpty(contactgroup);
1921
1922 // update managed ideia of physical data and do updates to core
1923 /*
1924 lock (_characters)
1925 {
1926 foreach (OdeCharacter actor in _characters)
1927 {
1928 if (actor != null)
1929 {
1930 if (actor.bad)
1931 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1932
1933 actor.UpdatePositionAndVelocity();
1934 }
1935 }
1936 }
1937 */
1938
1939 lock (_activegroups)
1940 {
1941 {
1942 foreach (OdePrim actor in _activegroups)
1943 {
1944 if (actor.IsPhysical)
1945 {
1946 actor.UpdatePositionAndVelocity();
1947 }
1948 }
1949 }
1950 }
1951 }
1952 catch (Exception e)
1953 {
1954 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1955// ode.dunlock(world);
1956 }
1957
1958
1959 step_time -= ODE_STEPSIZE;
1960 nodeframes++;
1961 }
1962
1963 lock (_badCharacter)
1964 {
1965 if (_badCharacter.Count > 0)
1966 {
1967 foreach (OdeCharacter chr in _badCharacter)
1968 {
1969 RemoveCharacter(chr);
1970 }
1971
1972 _badCharacter.Clear();
1973 }
1974 }
1975/*
1976 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1977 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1978 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1979 int nbodies = d.NTotalBodies;
1980 int ngeoms = d.NTotalGeoms;
1981*/
1982 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1983 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1984 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1985 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1986 {
1987 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1988 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1989
1990 if (physics_logging_append_existing_logfile)
1991 {
1992 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1993 TextWriter fwriter = File.AppendText(fname);
1994 fwriter.WriteLine(header);
1995 fwriter.Close();
1996 }
1997
1998 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1999 }
2000
2001 // think time dilation as to do with dinamic step size that we dont' have
2002 // even so tell something to world
2003 if (nodeframes < 10) // we did the requested loops
2004 m_timeDilation = 1.0f;
2005 else if (step_time > 0)
2006 {
2007 m_timeDilation = timeStep / step_time;
2008 if (m_timeDilation > 1)
2009 m_timeDilation = 1;
2010 if (step_time > m_SkipFramesAtms)
2011 step_time = 0;
2012 }
2013 }
2014
2015// return nodeframes * ODE_STEPSIZE; // return real simulated time
2016 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
2017 }
2018
2019 /// <summary>
2020 public override void GetResults()
2021 {
2022 }
2023
2024 public override bool IsThreaded
2025 {
2026 // for now we won't be multithreaded
2027 get { return (false); }
2028 }
2029
2030 public float GetTerrainHeightAtXY(float x, float y)
2031 {
2032
2033
2034 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2035 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2036
2037
2038 IntPtr heightFieldGeom = IntPtr.Zero;
2039
2040 // get region map
2041 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
2042 return 0f;
2043
2044 if (heightFieldGeom == IntPtr.Zero)
2045 return 0f;
2046
2047 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
2048 return 0f;
2049
2050 // TerrainHeightField for ODE as offset 1m
2051 x += 1f - offsetX;
2052 y += 1f - offsetY;
2053
2054 // make position fit into array
2055 if (x < 0)
2056 x = 0;
2057 if (y < 0)
2058 y = 0;
2059
2060 // integer indexs
2061 int ix;
2062 int iy;
2063 // interpolators offset
2064 float dx;
2065 float dy;
2066
2067 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
2068
2069 if (OdeUbitLib)
2070 {
2071 if (x < regsize - 1)
2072 {
2073 ix = (int)x;
2074 dx = x - (float)ix;
2075 }
2076 else // out world use external height
2077 {
2078 ix = regsize - 2;
2079 dx = 0;
2080 }
2081 if (y < regsize - 1)
2082 {
2083 iy = (int)y;
2084 dy = y - (float)iy;
2085 }
2086 else
2087 {
2088 iy = regsize - 2;
2089 dy = 0;
2090 }
2091 }
2092
2093 else
2094 {
2095 // we still have square fixed size regions
2096 // also flip x and y because of how map is done for ODE fliped axis
2097 // so ix,iy,dx and dy are inter exchanged
2098 if (x < regsize - 1)
2099 {
2100 iy = (int)x;
2101 dy = x - (float)iy;
2102 }
2103 else // out world use external height
2104 {
2105 iy = regsize - 2;
2106 dy = 0;
2107 }
2108 if (y < regsize - 1)
2109 {
2110 ix = (int)y;
2111 dx = y - (float)ix;
2112 }
2113 else
2114 {
2115 ix = regsize - 2;
2116 dx = 0;
2117 }
2118 }
2119
2120 float h0;
2121 float h1;
2122 float h2;
2123
2124 iy *= regsize;
2125 iy += ix; // all indexes have iy + ix
2126
2127 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2128 /*
2129 if ((dx + dy) <= 1.0f)
2130 {
2131 h0 = ((float)heights[iy]); // 0,0 vertice
2132 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2133 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2134 }
2135 else
2136 {
2137 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2138 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2139 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2140 }
2141 */
2142 h0 = ((float)heights[iy]); // 0,0 vertice
2143
2144 if ((dy > dx))
2145 {
2146 iy += regsize;
2147 h2 = (float)heights[iy]; // 0,1 vertice
2148 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2149 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2150 }
2151 else
2152 {
2153 iy++;
2154 h2 = (float)heights[iy]; // vertice 1,0
2155 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2156 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2157 }
2158
2159 return h0 + h1 + h2;
2160 }
2161
2162
2163 public override void SetTerrain(float[] heightMap)
2164 {
2165 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2166 {
2167 if (m_parentScene is OdeScene)
2168 {
2169 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2170 }
2171 }
2172 else
2173 {
2174 SetTerrain(heightMap, m_worldOffset);
2175 }
2176 }
2177
2178 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2179 {
2180 SetTerrain(heightMap, pOffset);
2181 }
2182
2183 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2184 {
2185 if (OdeUbitLib)
2186 UbitSetTerrain(heightMap, pOffset);
2187 else
2188 OriSetTerrain(heightMap, pOffset);
2189 }
2190
2191 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2192 {
2193 // assumes 1m size grid and constante size square regions
2194 // needs to know about sims around in future
2195
2196 float[] _heightmap;
2197
2198 uint heightmapWidth = Constants.RegionSize + 2;
2199 uint heightmapHeight = Constants.RegionSize + 2;
2200
2201 uint heightmapWidthSamples = heightmapWidth + 1;
2202 uint heightmapHeightSamples = heightmapHeight + 1;
2203
2204 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2205
2206 const float scale = 1.0f;
2207 const float offset = 0.0f;
2208 const float thickness = 10f;
2209 const int wrap = 0;
2210
2211 uint regionsize = Constants.RegionSize;
2212
2213 float hfmin = float.MaxValue;
2214 float hfmax = float.MinValue;
2215 float val;
2216 uint xx;
2217 uint yy;
2218
2219 uint maxXXYY = regionsize - 1;
2220 // flipping map adding one margin all around so things don't fall in edges
2221
2222 uint xt = 0;
2223 xx = 0;
2224
2225 for (uint x = 0; x < heightmapWidthSamples; x++)
2226 {
2227 if (x > 1 && xx < maxXXYY)
2228 xx++;
2229 yy = 0;
2230 for (uint y = 0; y < heightmapHeightSamples; y++)
2231 {
2232 if (y > 1 && y < maxXXYY)
2233 yy += regionsize;
2234
2235 val = heightMap[yy + xx];
2236 if (val < 0.0f)
2237 val = 0.0f; // no neg terrain as in chode
2238 _heightmap[xt + y] = val;
2239
2240 if (hfmin > val)
2241 hfmin = val;
2242 if (hfmax < val)
2243 hfmax = val;
2244 }
2245 xt += heightmapHeightSamples;
2246 }
2247 lock (OdeLock)
2248 {
2249 IntPtr GroundGeom = IntPtr.Zero;
2250 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2251 {
2252 RegionTerrain.Remove(pOffset);
2253 if (GroundGeom != IntPtr.Zero)
2254 {
2255 actor_name_map.Remove(GroundGeom);
2256 d.GeomDestroy(GroundGeom);
2257
2258 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2259 {
2260 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2261 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2262 TerrainHeightFieldHeights.Remove(GroundGeom);
2263 }
2264 }
2265 }
2266 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2267
2268 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2269
2270 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2271 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2272 offset, thickness, wrap);
2273
2274 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2275 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2276 if (GroundGeom != IntPtr.Zero)
2277 {
2278 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2279 d.GeomSetCollideBits(GroundGeom, 0);
2280
2281 PhysicsActor pa = new NullPhysicsActor();
2282 pa.Name = "Terrain";
2283 pa.PhysicsActorType = (int)ActorTypes.Ground;
2284 actor_name_map[GroundGeom] = pa;
2285
2286// geom_name_map[GroundGeom] = "Terrain";
2287
2288 d.Matrix3 R = new d.Matrix3();
2289
2290 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2291 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2292
2293
2294 q1 = q1 * q2;
2295
2296 Vector3 v3;
2297 float angle;
2298 q1.GetAxisAngle(out v3, out angle);
2299
2300 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2301 d.GeomSetRotation(GroundGeom, ref R);
2302 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2303 RegionTerrain.Add(pOffset, GroundGeom);
2304 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2305 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2306 }
2307 }
2308 }
2309
2310 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2311 {
2312 // assumes 1m size grid and constante size square regions
2313 // needs to know about sims around in future
2314
2315 float[] _heightmap;
2316
2317 uint heightmapWidth = Constants.RegionSize + 2;
2318 uint heightmapHeight = Constants.RegionSize + 2;
2319
2320 uint heightmapWidthSamples = heightmapWidth + 1;
2321 uint heightmapHeightSamples = heightmapHeight + 1;
2322
2323 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2324
2325
2326 uint regionsize = Constants.RegionSize;
2327
2328 float hfmin = float.MaxValue;
2329// float hfmax = float.MinValue;
2330 float val;
2331
2332
2333 uint maxXXYY = regionsize - 1;
2334 // adding one margin all around so things don't fall in edges
2335
2336 uint xx;
2337 uint yy = 0;
2338 uint yt = 0;
2339
2340 for (uint y = 0; y < heightmapHeightSamples; y++)
2341 {
2342 if (y > 1 && y < maxXXYY)
2343 yy += regionsize;
2344 xx = 0;
2345 for (uint x = 0; x < heightmapWidthSamples; x++)
2346 {
2347 if (x > 1 && x < maxXXYY)
2348 xx++;
2349
2350 val = heightMap[yy + xx];
2351 if (val < 0.0f)
2352 val = 0.0f; // no neg terrain as in chode
2353 _heightmap[yt + x] = val;
2354
2355 if (hfmin > val)
2356 hfmin = val;
2357// if (hfmax < val)
2358// hfmax = val;
2359 }
2360 yt += heightmapWidthSamples;
2361 }
2362 lock (OdeLock)
2363 {
2364 IntPtr GroundGeom = IntPtr.Zero;
2365 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2366 {
2367 RegionTerrain.Remove(pOffset);
2368 if (GroundGeom != IntPtr.Zero)
2369 {
2370 actor_name_map.Remove(GroundGeom);
2371 d.GeomDestroy(GroundGeom);
2372
2373 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2374 {
2375 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2376 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2377 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2378 TerrainHeightFieldHeights.Remove(GroundGeom);
2379 }
2380 }
2381 }
2382 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2383
2384 const int wrap = 0;
2385 float thickness = hfmin;
2386 if (thickness < 0)
2387 thickness = 1;
2388
2389 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2390
2391 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2392 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2393 thickness, wrap);
2394
2395// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2396 GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1);
2397 if (GroundGeom != IntPtr.Zero)
2398 {
2399 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2400 d.GeomSetCollideBits(GroundGeom, 0);
2401
2402 PhysicsActor pa = new NullPhysicsActor();
2403 pa.Name = "Terrain";
2404 pa.PhysicsActorType = (int)ActorTypes.Ground;
2405 actor_name_map[GroundGeom] = pa;
2406
2407// geom_name_map[GroundGeom] = "Terrain";
2408
2409 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2410 RegionTerrain.Add(pOffset, GroundGeom);
2411 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2412 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2413 }
2414 }
2415 }
2416
2417
2418 public override void DeleteTerrain()
2419 {
2420 }
2421
2422 public float GetWaterLevel()
2423 {
2424 return waterlevel;
2425 }
2426
2427 public override bool SupportsCombining()
2428 {
2429 return true;
2430 }
2431/*
2432 public override void UnCombine(PhysicsScene pScene)
2433 {
2434 IntPtr localGround = IntPtr.Zero;
2435// float[] localHeightfield;
2436 bool proceed = false;
2437 List<IntPtr> geomDestroyList = new List<IntPtr>();
2438
2439 lock (OdeLock)
2440 {
2441 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2442 {
2443 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2444 {
2445 if (geom == localGround)
2446 {
2447// localHeightfield = TerrainHeightFieldHeights[geom];
2448 proceed = true;
2449 }
2450 else
2451 {
2452 geomDestroyList.Add(geom);
2453 }
2454 }
2455
2456 if (proceed)
2457 {
2458 m_worldOffset = Vector3.Zero;
2459 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2460 m_parentScene = null;
2461
2462 foreach (IntPtr g in geomDestroyList)
2463 {
2464 // removingHeightField needs to be done or the garbage collector will
2465 // collect the terrain data before we tell ODE to destroy it causing
2466 // memory corruption
2467 if (TerrainHeightFieldHeights.ContainsKey(g))
2468 {
2469// float[] removingHeightField = TerrainHeightFieldHeights[g];
2470 TerrainHeightFieldHeights.Remove(g);
2471
2472 if (RegionTerrain.ContainsKey(g))
2473 {
2474 RegionTerrain.Remove(g);
2475 }
2476
2477 d.GeomDestroy(g);
2478 //removingHeightField = new float[0];
2479 }
2480 }
2481
2482 }
2483 else
2484 {
2485 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2486 }
2487 }
2488 }
2489 }
2490*/
2491 public override void SetWaterLevel(float baseheight)
2492 {
2493 waterlevel = baseheight;
2494// randomizeWater(waterlevel);
2495 }
2496/*
2497 public void randomizeWater(float baseheight)
2498 {
2499 const uint heightmapWidth = Constants.RegionSize + 2;
2500 const uint heightmapHeight = Constants.RegionSize + 2;
2501 const uint heightmapWidthSamples = heightmapWidth + 1;
2502 const uint heightmapHeightSamples = heightmapHeight + 1;
2503
2504 const float scale = 1.0f;
2505 const float offset = 0.0f;
2506 const int wrap = 0;
2507
2508 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
2509
2510 float maxheigh = float.MinValue;
2511 float minheigh = float.MaxValue;
2512 float val;
2513 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
2514 {
2515
2516 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
2517 _watermap[i] = val;
2518 if (maxheigh < val)
2519 maxheigh = val;
2520 if (minheigh > val)
2521 minheigh = val;
2522 }
2523
2524 float thickness = minheigh;
2525
2526 lock (OdeLock)
2527 {
2528 if (WaterGeom != IntPtr.Zero)
2529 {
2530 actor_name_map.Remove(WaterGeom);
2531 d.GeomDestroy(WaterGeom);
2532 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2533 WaterGeom = IntPtr.Zero;
2534 WaterHeightmapData = IntPtr.Zero;
2535 if(WaterMapHandler.IsAllocated)
2536 WaterMapHandler.Free();
2537 }
2538
2539 WaterHeightmapData = d.GeomHeightfieldDataCreate();
2540
2541 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2542
2543 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
2544 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2545 offset, thickness, wrap);
2546 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
2547 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
2548 if (WaterGeom != IntPtr.Zero)
2549 {
2550 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
2551 d.GeomSetCollideBits(WaterGeom, 0);
2552
2553
2554 PhysicsActor pa = new NullPhysicsActor();
2555 pa.Name = "Water";
2556 pa.PhysicsActorType = (int)ActorTypes.Water;
2557
2558 actor_name_map[WaterGeom] = pa;
2559// geom_name_map[WaterGeom] = "Water";
2560
2561 d.Matrix3 R = new d.Matrix3();
2562
2563 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2564 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2565
2566 q1 = q1 * q2;
2567 Vector3 v3;
2568 float angle;
2569 q1.GetAxisAngle(out v3, out angle);
2570
2571 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2572 d.GeomSetRotation(WaterGeom, ref R);
2573 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
2574 }
2575 }
2576 }
2577*/
2578 public override void Dispose()
2579 {
2580 lock (OdeLock)
2581 {
2582 m_rayCastManager.Dispose();
2583 m_rayCastManager = null;
2584
2585 lock (_prims)
2586 {
2587 ChangesQueue.Clear();
2588 foreach (OdePrim prm in _prims)
2589 {
2590 prm.DoAChange(changes.Remove, null);
2591 _collisionEventPrim.Remove(prm);
2592 }
2593 _prims.Clear();
2594 }
2595
2596 OdeCharacter[] chtorem;
2597 lock (_characters)
2598 {
2599 chtorem = new OdeCharacter[_characters.Count];
2600 _characters.CopyTo(chtorem);
2601 }
2602
2603 ChangesQueue.Clear();
2604 foreach (OdeCharacter ch in chtorem)
2605 ch.DoAChange(changes.Remove, null);
2606
2607
2608 foreach (IntPtr GroundGeom in RegionTerrain.Values)
2609 {
2610 if (GroundGeom != IntPtr.Zero)
2611 d.GeomDestroy(GroundGeom);
2612 }
2613
2614 RegionTerrain.Clear();
2615
2616 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2617 {
2618 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2619 {
2620 if (gch.IsAllocated)
2621 gch.Free();
2622 }
2623 }
2624
2625 TerrainHeightFieldHeightsHandlers.Clear();
2626 TerrainHeightFieldHeights.Clear();
2627/*
2628 if (WaterGeom != IntPtr.Zero)
2629 {
2630 d.GeomDestroy(WaterGeom);
2631 WaterGeom = IntPtr.Zero;
2632 if (WaterHeightmapData != IntPtr.Zero)
2633 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2634 WaterHeightmapData = IntPtr.Zero;
2635
2636 if (WaterMapHandler.IsAllocated)
2637 WaterMapHandler.Free();
2638 }
2639*/
2640 if (ContactgeomsArray != IntPtr.Zero)
2641 Marshal.FreeHGlobal(ContactgeomsArray);
2642 if (GlobalContactsArray != IntPtr.Zero)
2643 Marshal.FreeHGlobal(GlobalContactsArray);
2644
2645
2646 d.WorldDestroy(world);
2647 world = IntPtr.Zero;
2648 //d.CloseODE();
2649 }
2650 }
2651
2652 public override Dictionary<uint, float> GetTopColliders()
2653 {
2654 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2655 int cnt = 0;
2656 lock (_prims)
2657 {
2658 foreach (OdePrim prm in _prims)
2659 {
2660 if (prm.CollisionScore > 0)
2661 {
2662 returncolliders.Add(prm.LocalID, prm.CollisionScore);
2663 cnt++;
2664 prm.CollisionScore = 0f;
2665 if (cnt > 25)
2666 {
2667 break;
2668 }
2669 }
2670 }
2671 }
2672 return returncolliders;
2673 }
2674
2675 public override bool SupportsRayCast()
2676 {
2677 return true;
2678 }
2679
2680 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2681 {
2682 if (retMethod != null)
2683 {
2684 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2685 }
2686 }
2687
2688 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2689 {
2690 if (retMethod != null)
2691 {
2692 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2693 }
2694 }
2695
2696 // don't like this
2697 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2698 {
2699 ContactResult[] ourResults = null;
2700 RayCallback retMethod = delegate(List<ContactResult> results)
2701 {
2702 ourResults = new ContactResult[results.Count];
2703 results.CopyTo(ourResults, 0);
2704 };
2705 int waitTime = 0;
2706 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2707 while (ourResults == null && waitTime < 1000)
2708 {
2709 Thread.Sleep(1);
2710 waitTime++;
2711 }
2712 if (ourResults == null)
2713 return new List<ContactResult>();
2714 return new List<ContactResult>(ourResults);
2715 }
2716
2717 public override bool SuportsRaycastWorldFiltered()
2718 {
2719 return true;
2720 }
2721
2722 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2723 {
2724 object SyncObject = new object();
2725 List<ContactResult> ourresults = new List<ContactResult>();
2726
2727 RayCallback retMethod = delegate(List<ContactResult> results)
2728 {
2729 lock (SyncObject)
2730 {
2731 ourresults = results;
2732 Monitor.PulseAll(SyncObject);
2733 }
2734 };
2735
2736 lock (SyncObject)
2737 {
2738 m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod);
2739 if (!Monitor.Wait(SyncObject, 500))
2740 return null;
2741 else
2742 return ourresults;
2743 }
2744 }
2745
2746 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2747 {
2748 if (retMethod != null && actor !=null)
2749 {
2750 IntPtr geom;
2751 if (actor is OdePrim)
2752 geom = ((OdePrim)actor).prim_geom;
2753 else if (actor is OdeCharacter)
2754 geom = ((OdePrim)actor).prim_geom;
2755 else
2756 return;
2757 if (geom == IntPtr.Zero)
2758 return;
2759 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2760 }
2761 }
2762
2763 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2764 {
2765 if (retMethod != null && actor != null)
2766 {
2767 IntPtr geom;
2768 if (actor is OdePrim)
2769 geom = ((OdePrim)actor).prim_geom;
2770 else if (actor is OdeCharacter)
2771 geom = ((OdePrim)actor).prim_geom;
2772 else
2773 return;
2774 if (geom == IntPtr.Zero)
2775 return;
2776
2777 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2778 }
2779 }
2780
2781 // don't like this
2782 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2783 {
2784 if (actor != null)
2785 {
2786 IntPtr geom;
2787 if (actor is OdePrim)
2788 geom = ((OdePrim)actor).prim_geom;
2789 else if (actor is OdeCharacter)
2790 geom = ((OdePrim)actor).prim_geom;
2791 else
2792 return new List<ContactResult>();
2793 if (geom == IntPtr.Zero)
2794 return new List<ContactResult>();
2795
2796 ContactResult[] ourResults = null;
2797 RayCallback retMethod = delegate(List<ContactResult> results)
2798 {
2799 ourResults = new ContactResult[results.Count];
2800 results.CopyTo(ourResults, 0);
2801 };
2802 int waitTime = 0;
2803 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2804 while (ourResults == null && waitTime < 1000)
2805 {
2806 Thread.Sleep(1);
2807 waitTime++;
2808 }
2809 if (ourResults == null)
2810 return new List<ContactResult>();
2811 return new List<ContactResult>(ourResults);
2812 }
2813 return new List<ContactResult>();
2814 }
2815 }
2816}
generated by cgit v1.1 at 2024-12-23 21:28:37 +0000