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