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.cs1452
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs997
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs4024
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs443
-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.cs2575
8 files changed, 11644 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..4266fda
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1452 @@
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.Geom
119 | CollisionCategories.Space
120 | CollisionCategories.Body
121 | CollisionCategories.Character
122 );
123 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero;
127 public IntPtr Amotor = IntPtr.Zero;
128 public d.Mass ShellMass;
129// public bool collidelock = false;
130
131 private bool m_haseventsubscription = false;
132 public int m_eventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
134
135 // unique UUID of this character object
136 public UUID m_uuid;
137 public bool bad = false;
138
139 float mu;
140
141 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)
142 {
143 m_uuid = UUID.Random();
144
145 if (pos.IsFinite())
146 {
147 if (pos.Z > 99999f)
148 {
149 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
150 }
151 if (pos.Z < -100f) // shouldn't this be 0 ?
152 {
153 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
154 }
155 _position = pos;
156 }
157 else
158 {
159 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
160 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
161 }
162
163 _parent_scene = parent_scene;
164
165 PID_D = pid_d;
166 PID_P = pid_p;
167 CAPSULE_RADIUS = capsule_radius;
168 m_density = density;
169 m_mass = 80f; // sure we have a default
170
171 mu = parent_scene.AvatarFriction;
172
173 walkDivisor = walk_divisor;
174 runDivisor = rundivisor;
175
176 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
177 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
178
179 m_isPhysical = false; // current status: no ODE information exists
180
181 m_name = avName;
182
183 AddChange(changes.Add, null);
184 }
185
186 public override int PhysicsActorType
187 {
188 get { return (int)ActorTypes.Agent; }
189 set { return; }
190 }
191
192 public override void getContactData(ref ContactData cdata)
193 {
194 cdata.mu = mu;
195 cdata.bounce = 0;
196 cdata.softcolide = false;
197 }
198
199 public override bool Building { get; set; }
200
201 /// <summary>
202 /// If this is set, the avatar will move faster
203 /// </summary>
204 public override bool SetAlwaysRun
205 {
206 get { return m_alwaysRun; }
207 set { m_alwaysRun = value; }
208 }
209
210 public override uint LocalID
211 {
212 set { m_localID = value; }
213 }
214
215 public override bool Grabbed
216 {
217 set { return; }
218 }
219
220 public override bool Selected
221 {
222 set { return; }
223 }
224
225 public override float Buoyancy
226 {
227 get { return m_buoyancy; }
228 set { m_buoyancy = value; }
229 }
230
231 public override bool FloatOnWater
232 {
233 set { return; }
234 }
235
236 public override bool IsPhysical
237 {
238 get { return false; }
239 set { return; }
240 }
241
242 public override bool ThrottleUpdates
243 {
244 get { return false; }
245 set { return; }
246 }
247
248 public override bool Flying
249 {
250 get { return flying; }
251 set
252 {
253 flying = value;
254 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
255 }
256 }
257
258 /// <summary>
259 /// Returns if the avatar is colliding in general.
260 /// This includes the ground and objects and avatar.
261 /// </summary>
262 public override bool IsColliding
263 {
264 get { return (m_iscolliding || m_iscollidingGround); }
265 set
266 {
267 if (value)
268 {
269 m_colliderfilter += 2;
270 if (m_colliderfilter > 2)
271 m_colliderfilter = 2;
272 }
273 else
274 {
275 m_colliderfilter--;
276 if (m_colliderfilter < 0)
277 m_colliderfilter = 0;
278 }
279
280 if (m_colliderfilter == 0)
281 m_iscolliding = false;
282 else
283 {
284// SetPidStatus(false);
285 m_pidControllerActive = true;
286 m_iscolliding = true;
287 }
288 }
289 }
290
291 /// <summary>
292 /// Returns if an avatar is colliding with the ground
293 /// </summary>
294 public override bool CollidingGround
295 {
296 get { return m_iscollidingGround; }
297 set
298 {
299 /* we now control this
300 if (value)
301 {
302 m_colliderGroundfilter += 2;
303 if (m_colliderGroundfilter > 2)
304 m_colliderGroundfilter = 2;
305 }
306 else
307 {
308 m_colliderGroundfilter--;
309 if (m_colliderGroundfilter < 0)
310 m_colliderGroundfilter = 0;
311 }
312
313 if (m_colliderGroundfilter == 0)
314 m_iscollidingGround = false;
315 else
316 m_iscollidingGround = true;
317 */
318 }
319
320 }
321
322 /// <summary>
323 /// Returns if the avatar is colliding with an object
324 /// </summary>
325 public override bool CollidingObj
326 {
327 get { return m_iscollidingObj; }
328 set
329 {
330 // Ubit filter this also
331 if (value)
332 {
333 m_colliderObjectfilter += 2;
334 if (m_colliderObjectfilter > 2)
335 m_colliderObjectfilter = 2;
336 }
337 else
338 {
339 m_colliderObjectfilter--;
340 if (m_colliderObjectfilter < 0)
341 m_colliderObjectfilter = 0;
342 }
343
344 if (m_colliderObjectfilter == 0)
345 m_iscollidingObj = false;
346 else
347 m_iscollidingObj = true;
348
349 // m_iscollidingObj = value;
350/*
351 if (m_iscollidingObj)
352 m_pidControllerActive = false;
353 else
354 m_pidControllerActive = true;
355 */
356 }
357 }
358
359 /// <summary>
360 /// turn the PID controller on or off.
361 /// The PID Controller will turn on all by itself in many situations
362 /// </summary>
363 /// <param name="status"></param>
364 public void SetPidStatus(bool status)
365 {
366 m_pidControllerActive = status;
367 }
368
369 public override bool Stopped
370 {
371 get { return _zeroFlag; }
372 }
373
374 /// <summary>
375 /// This 'puts' an avatar somewhere in the physics space.
376 /// Not really a good choice unless you 'know' it's a good
377 /// spot otherwise you're likely to orbit the avatar.
378 /// </summary>
379 public override Vector3 Position
380 {
381 get { return _position; }
382 set
383 {
384 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
385 {
386 if (value.IsFinite())
387 {
388 if (value.Z > 9999999f)
389 {
390 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
391 }
392 if (value.Z < -100f)
393 {
394 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
395 }
396 AddChange(changes.Position, value);
397 }
398 else
399 {
400 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
401 }
402 }
403 }
404 }
405
406 public override Vector3 RotationalVelocity
407 {
408 get { return m_rotationalVelocity; }
409 set { m_rotationalVelocity = value; }
410 }
411
412 /// <summary>
413 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
414 /// and use it to offset landings properly
415 /// </summary>
416 public override Vector3 Size
417 {
418 get {
419 float d = CAPSULE_RADIUS * 2;
420 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
421 set
422 {
423 if (value.IsFinite())
424 {
425 AddChange(changes.Size, value);
426 }
427 else
428 {
429 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
430 }
431 }
432 }
433
434 /// <summary>
435 /// This creates the Avatar's physical Surrogate at the position supplied
436 /// </summary>
437 /// <param name="npositionX"></param>
438 /// <param name="npositionY"></param>
439 /// <param name="npositionZ"></param>
440
441 //
442 /// <summary>
443 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
444 /// This may be used in calculations in the scene/scenepresence
445 /// </summary>
446 public override float Mass
447 {
448 get
449 {
450 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
451 return m_density * AVvolume;
452 }
453 }
454 public override void link(PhysicsActor obj)
455 {
456
457 }
458
459 public override void delink()
460 {
461
462 }
463
464 public override void LockAngularMotion(Vector3 axis)
465 {
466
467 }
468
469
470 public override Vector3 Force
471 {
472 get { return _target_velocity; }
473 set { return; }
474 }
475
476 public override int VehicleType
477 {
478 get { return 0; }
479 set { return; }
480 }
481
482 public override void VehicleFloatParam(int param, float value)
483 {
484
485 }
486
487 public override void VehicleVectorParam(int param, Vector3 value)
488 {
489
490 }
491
492 public override void VehicleRotationParam(int param, Quaternion rotation)
493 {
494
495 }
496
497 public override void VehicleFlags(int param, bool remove)
498 {
499
500 }
501
502 public override void SetVolumeDetect(int param)
503 {
504
505 }
506
507 public override Vector3 CenterOfMass
508 {
509 get
510 {
511 Vector3 pos = _position;
512 return pos;
513 }
514 }
515
516 public override Vector3 GeometricCenter
517 {
518 get
519 {
520 Vector3 pos = _position;
521 return pos;
522 }
523 }
524
525 //UBit mess
526 /* for later use
527 public override Vector3 PrimOOBsize
528 {
529 get
530 {
531 Vector3 s=Size;
532 s.X *=0.5f;
533 s.Y *=0.5f;
534 s.Z *=0.5f;
535 return s;
536 }
537 }
538
539 public override Vector3 PrimOOBoffset
540 {
541 get
542 {
543 return Vector3.Zero;
544 }
545 }
546 */
547
548 public override PrimitiveBaseShape Shape
549 {
550 set { return; }
551 }
552
553 public override Vector3 Velocity
554 {
555 get
556 {
557 return _velocity;
558 }
559 set
560 {
561 if (value.IsFinite())
562 {
563 AddChange(changes.Velocity, value);
564 }
565 else
566 {
567 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
568 }
569 }
570 }
571
572 public override Vector3 Torque
573 {
574 get { return Vector3.Zero; }
575 set { return; }
576 }
577
578 public override float CollisionScore
579 {
580 get { return 0f; }
581 set { }
582 }
583
584 public override bool Kinematic
585 {
586 get { return false; }
587 set { }
588 }
589
590 public override Quaternion Orientation
591 {
592 get { return Quaternion.Identity; }
593 set
594 {
595 }
596 }
597
598 public override Vector3 Acceleration
599 {
600 get { return _acceleration; }
601 set { }
602 }
603
604 public void SetAcceleration(Vector3 accel)
605 {
606 m_pidControllerActive = true;
607 _acceleration = accel;
608 }
609
610 /// <summary>
611 /// Adds the force supplied to the Target Velocity
612 /// The PID controller takes this target velocity and tries to make it a reality
613 /// </summary>
614 /// <param name="force"></param>
615 public override void AddForce(Vector3 force, bool pushforce)
616 {
617 if (force.IsFinite())
618 {
619 if (pushforce)
620 {
621 AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f);
622 }
623 else
624 {
625 AddChange(changes.Velocity, force);
626 }
627 }
628 else
629 {
630 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
631 }
632 //m_lastUpdateSent = false;
633 }
634
635 public override void AddAngularForce(Vector3 force, bool pushforce)
636 {
637
638 }
639
640 public override void SetMomentum(Vector3 momentum)
641 {
642 }
643
644
645 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
646 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
647 // place that is safe to call this routine AvatarGeomAndBodyCreation.
648 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
649 {
650 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
651 if (CAPSULE_LENGTH <= 0)
652 {
653 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
654 CAPSULE_LENGTH = 0.01f;
655
656 }
657
658 if (CAPSULE_RADIUS <= 0)
659 {
660 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
661 CAPSULE_RADIUS = 0.01f;
662
663 }
664 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
665
666 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
667 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
668
669 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
670
671 m_mass = ShellMass.mass; // update mass
672
673 // rescale PID parameters
674 PID_D = _parent_scene.avPIDD;
675 PID_P = _parent_scene.avPIDP;
676
677 // rescale PID parameters so that this aren't affected by mass
678 // and so don't get unstable for some masses
679 // also scale by ode time step so you don't need to refix them
680
681 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
682 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
683 PID_P /= 50 * 80;
684 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
685
686 Body = d.BodyCreate(_parent_scene.world);
687
688 d.BodySetAutoDisableFlag(Body, false);
689 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
690
691 _position.X = npositionX;
692 _position.Y = npositionY;
693 _position.Z = npositionZ;
694
695 d.BodySetMass(Body, ref ShellMass);
696 d.GeomSetBody(Shell, Body);
697
698 // The purpose of the AMotor here is to keep the avatar's physical
699 // surrogate from rotating while moving
700 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
701 d.JointAttach(Amotor, Body, IntPtr.Zero);
702
703 d.JointSetAMotorMode(Amotor, 0);
704 d.JointSetAMotorNumAxes(Amotor, 3);
705 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
706 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
707 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
708
709 d.JointSetAMotorAngle(Amotor, 0, 0);
710 d.JointSetAMotorAngle(Amotor, 1, 0);
711 d.JointSetAMotorAngle(Amotor, 2, 0);
712
713 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
714 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
715 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
716 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
717 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
718 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
719
720 // These lowstops and high stops are effectively (no wiggle room)
721 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
722 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
723 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
724 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
725 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
726 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
727
728 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
729 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
730 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
731
732 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f);
733 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f);
734 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f);
735 }
736
737 /// <summary>
738 /// Destroys the avatar body and geom
739
740 private void AvatarGeomAndBodyDestroy()
741 {
742 // Kill the Amotor
743 if (Amotor != IntPtr.Zero)
744 {
745 d.JointDestroy(Amotor);
746 Amotor = IntPtr.Zero;
747 }
748
749 if (Body != IntPtr.Zero)
750 {
751 //kill the body
752 d.BodyDestroy(Body);
753 Body = IntPtr.Zero;
754 }
755
756 //kill the Geometry
757 if (Shell != IntPtr.Zero)
758 {
759 _parent_scene.geom_name_map.Remove(Shell);
760 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
761 d.GeomDestroy(Shell);
762 _parent_scene.geom_name_map.Remove(Shell);
763 Shell = IntPtr.Zero;
764 }
765 }
766
767 /// <summary>
768 /// Called from Simulate
769 /// This is the avatar's movement control + PID Controller
770 /// </summary>
771 /// <param name="timeStep"></param>
772 public void Move(float timeStep, List<OdeCharacter> defects)
773 {
774 // no lock; for now it's only called from within Simulate()
775
776 // If the PID Controller isn't active then we set our force
777 // calculating base velocity to the current position
778
779 if (Body == IntPtr.Zero)
780 return;
781
782 d.Vector3 dtmp;
783 d.BodyCopyPosition(Body, out dtmp);
784 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
785
786 // the Amotor still lets avatar rotation to drift during colisions
787 // so force it back to identity
788
789 d.Quaternion qtmp;
790 qtmp.W = 1;
791 qtmp.X = 0;
792 qtmp.Y = 0;
793 qtmp.Z = 0;
794 d.BodySetQuaternion(Body, ref qtmp);
795
796 if (m_pidControllerActive == false)
797 {
798 _zeroPosition = localpos;
799 }
800 //PidStatus = true;
801
802
803 if (!localpos.IsFinite())
804 {
805
806 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
807 defects.Add(this);
808 // _parent_scene.RemoveCharacter(this);
809
810 // destroy avatar capsule and related ODE data
811 AvatarGeomAndBodyDestroy();
812
813 return;
814 }
815
816 Vector3 vec = Vector3.Zero;
817 dtmp = d.BodyGetLinearVel(Body);
818 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
819
820 float movementdivisor = 1f;
821 //Ubit change divisions into multiplications below
822 if (!m_alwaysRun)
823 {
824 movementdivisor = 1 / walkDivisor;
825 }
826 else
827 {
828 movementdivisor = 1 / runDivisor;
829 }
830
831 // colide with land
832
833 d.AABB aabb;
834 d.GeomGetAABB(Shell, out aabb);
835 float chrminZ = aabb.MinZ;
836
837 Vector3 posch = localpos;
838
839 float ftmp;
840
841 if (flying)
842 {
843 ftmp = timeStep;
844 posch.X += vel.X * ftmp;
845 posch.Y += vel.Y * ftmp;
846 }
847
848 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
849 if (chrminZ < terrainheight)
850 {
851 float depth = terrainheight - chrminZ;
852 if (!flying)
853 {
854 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
855 }
856 else
857 vec.Z = depth * PID_P * 50;
858
859 /*
860 Vector3 vtmp;
861 vtmp.X = _target_velocity.X * timeStep;
862 vtmp.Y = _target_velocity.Y * timeStep;
863 // fake and avoid squares
864 float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
865 if (k > 0)
866 {
867 posch.X += vtmp.X;
868 posch.Y += vtmp.Y;
869 terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
870 k = 1 + Math.Abs(terrainheight) / k;
871 movementdivisor /= k;
872
873 if (k < 1)
874 k = 1;
875 }
876 */
877
878
879 if (depth < 0.1f)
880 {
881 m_iscolliding = true;
882 m_colliderfilter = 2;
883 m_iscollidingGround = true;
884
885 ContactPoint contact = new ContactPoint();
886 contact.PenetrationDepth = depth;
887 contact.Position.X = localpos.X;
888 contact.Position.Y = localpos.Y;
889 contact.Position.Z = chrminZ;
890 contact.SurfaceNormal.X = 0f;
891 contact.SurfaceNormal.Y = 0f;
892 contact.SurfaceNormal.Z = -1f;
893 AddCollisionEvent(0, contact);
894
895 vec.Z *= 0.5f;
896 }
897
898 else
899 m_iscollidingGround = false;
900 }
901 else
902 m_iscollidingGround = false;
903
904
905 // if velocity is zero, use position control; otherwise, velocity control
906 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
907 && m_iscolliding)
908 {
909 // keep track of where we stopped. No more slippin' & slidin'
910 if (!_zeroFlag)
911 {
912 _zeroFlag = true;
913 _zeroPosition = localpos;
914 }
915 if (m_pidControllerActive)
916 {
917 // We only want to deactivate the PID Controller if we think we want to have our surrogate
918 // react to the physics scene by moving it's position.
919 // Avatar to Avatar collisions
920 // Prim to avatar collisions
921
922 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
923 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
924 if (flying)
925 {
926 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
927 }
928 }
929 //PidStatus = true;
930 }
931 else
932 {
933 m_pidControllerActive = true;
934 _zeroFlag = false;
935
936 if (m_iscolliding)
937 {
938 if (!flying)
939 {
940 if (_target_velocity.Z > 0.0f)
941 {
942 // We're colliding with something and we're not flying but we're moving
943 // This means we're walking or running. JUMPING
944 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
945 }
946 // We're standing on something
947 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
948 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
949 }
950 else
951 {
952 // We're flying and colliding with something
953 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
954 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
955 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
956 }
957 }
958 else // ie not colliding
959 {
960 if (flying) //(!m_iscolliding && flying)
961 {
962 // we're in mid air suspended
963 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
964 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
965 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
966 }
967
968 else
969 {
970 // we're not colliding and we're not flying so that means we're falling!
971 // m_iscolliding includes collisions with the ground.
972
973 // d.Vector3 pos = d.BodyGetPosition(Body);
974 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
975 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
976 }
977 }
978 }
979
980 if (flying)
981 {
982 vec.Z -= _parent_scene.gravityz * m_mass;
983
984 //Added for auto fly height. Kitto Flora
985 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
986
987 if (localpos.Z < target_altitude)
988 {
989 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
990 }
991 // end add Kitto Flora
992 }
993
994 if (vec.IsFinite())
995 {
996 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
997 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
998 }
999 else
1000 {
1001 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1002 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1003 defects.Add(this);
1004 // _parent_scene.RemoveCharacter(this);
1005 // destroy avatar capsule and related ODE data
1006 AvatarGeomAndBodyDestroy();
1007 }
1008 }
1009
1010 /// <summary>
1011 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1012 /// </summary>
1013 public void UpdatePositionAndVelocity()
1014 {
1015 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1016 if (Body == IntPtr.Zero)
1017 return;
1018
1019 d.Vector3 vec;
1020 try
1021 {
1022 d.BodyCopyPosition(Body, out vec);
1023 }
1024 catch (NullReferenceException)
1025 {
1026 bad = true;
1027 _parent_scene.BadCharacter(this);
1028 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1029 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1030 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1031 }
1032
1033 _position.X = vec.X;
1034 _position.Y = vec.Y;
1035 _position.Z = vec.Z;
1036
1037 bool fixbody = false;
1038
1039 if (_position.X < 0.0f)
1040 {
1041 fixbody = true;
1042 _position.X = 0.1f;
1043 }
1044 else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
1045 {
1046 fixbody = true;
1047 _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
1048 }
1049
1050 if (_position.Y < 0.0f)
1051 {
1052 fixbody = true;
1053 _position.Y = 0.1f;
1054 }
1055 else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
1056 {
1057 fixbody = true;
1058 _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
1059 }
1060
1061 if (fixbody)
1062 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1063
1064 // Did we move last? = zeroflag
1065 // This helps keep us from sliding all over
1066/*
1067 if (_zeroFlag)
1068 {
1069 _velocity.X = 0.0f;
1070 _velocity.Y = 0.0f;
1071 _velocity.Z = 0.0f;
1072
1073 // Did we send out the 'stopped' message?
1074 if (!m_lastUpdateSent)
1075 {
1076 m_lastUpdateSent = true;
1077 base.RequestPhysicsterseUpdate();
1078 }
1079 }
1080 else
1081 {
1082 m_lastUpdateSent = false;
1083 */
1084 try
1085 {
1086 vec = d.BodyGetLinearVel(Body);
1087 }
1088 catch (NullReferenceException)
1089 {
1090 vec.X = _velocity.X;
1091 vec.Y = _velocity.Y;
1092 vec.Z = _velocity.Z;
1093 }
1094 _velocity.X = (vec.X);
1095 _velocity.Y = (vec.Y);
1096 _velocity.Z = (vec.Z);
1097 // }
1098 }
1099
1100 /// <summary>
1101 /// Cleanup the things we use in the scene.
1102 /// </summary>
1103 public void Destroy()
1104 {
1105 AddChange(changes.Remove, null);
1106 }
1107
1108 public override void CrossingFailure()
1109 {
1110 }
1111
1112 public override Vector3 PIDTarget { set { return; } }
1113 public override bool PIDActive { set { return; } }
1114 public override float PIDTau { set { return; } }
1115
1116 public override float PIDHoverHeight { set { return; } }
1117 public override bool PIDHoverActive { set { return; } }
1118 public override PIDHoverType PIDHoverType { set { return; } }
1119 public override float PIDHoverTau { set { return; } }
1120
1121 public override Quaternion APIDTarget { set { return; } }
1122
1123 public override bool APIDActive { set { return; } }
1124
1125 public override float APIDStrength { set { return; } }
1126
1127 public override float APIDDamping { set { return; } }
1128
1129
1130 public override void SubscribeEvents(int ms)
1131 {
1132 m_requestedUpdateFrequency = ms;
1133 m_eventsubscription = ms;
1134 _parent_scene.AddCollisionEventReporting(this);
1135 m_haseventsubscription = true;
1136 }
1137
1138 public override void UnSubscribeEvents()
1139 {
1140 m_haseventsubscription = false;
1141 _parent_scene.RemoveCollisionEventReporting(this);
1142 m_requestedUpdateFrequency = 0;
1143 m_eventsubscription = 0;
1144 }
1145
1146 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1147 {
1148 if (m_haseventsubscription)
1149 {
1150 // m_log.DebugFormat(
1151 // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
1152
1153 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1154 }
1155 }
1156
1157 public void SendCollisions()
1158 {
1159 if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency)
1160 {
1161 if (CollisionEventsThisFrame != null)
1162 {
1163 base.SendCollisionUpdate(CollisionEventsThisFrame);
1164 }
1165 CollisionEventsThisFrame = new CollisionEventUpdate();
1166 m_eventsubscription = 0;
1167 }
1168 }
1169
1170 public override bool SubscribedEvents()
1171 {
1172 return m_haseventsubscription;
1173 }
1174
1175 private void changePhysicsStatus(bool NewStatus)
1176 {
1177 if (NewStatus != m_isPhysical)
1178 {
1179 if (NewStatus)
1180 {
1181 // Create avatar capsule and related ODE data
1182 if ((Shell != IntPtr.Zero))
1183 {
1184 // a lost shell ?
1185 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1186 + (Shell != IntPtr.Zero ? "Shell " : "")
1187 + (Body != IntPtr.Zero ? "Body " : "")
1188 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1189 AvatarGeomAndBodyDestroy();
1190 }
1191
1192 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1193 _parent_scene.geom_name_map[Shell] = m_name;
1194 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1195 _parent_scene.AddCharacter(this);
1196 }
1197 else
1198 {
1199 _parent_scene.RemoveCharacter(this);
1200 // destroy avatar capsule and related ODE data
1201 AvatarGeomAndBodyDestroy();
1202 }
1203
1204 m_isPhysical = NewStatus;
1205 }
1206 }
1207
1208 private void changeAdd()
1209 {
1210 changePhysicsStatus(true);
1211 }
1212
1213 private void changeRemove()
1214 {
1215 changePhysicsStatus(false);
1216 }
1217
1218 private void changeShape(PrimitiveBaseShape arg)
1219 {
1220 }
1221
1222 private void changeSize(Vector3 Size)
1223 {
1224 if (Size.IsFinite())
1225 {
1226 float caplen = Size.Z;
1227
1228 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1229
1230 if (caplen != CAPSULE_LENGTH)
1231 {
1232 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1233 {
1234 AvatarGeomAndBodyDestroy();
1235
1236 float prevCapsule = CAPSULE_LENGTH;
1237 CAPSULE_LENGTH = caplen;
1238
1239 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1240 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
1241
1242 Velocity = Vector3.Zero;
1243
1244 _parent_scene.geom_name_map[Shell] = m_name;
1245 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1246 }
1247 else
1248 {
1249 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1250 + (Shell == IntPtr.Zero ? "Shell " : "")
1251 + (Body == IntPtr.Zero ? "Body " : "")
1252 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1253 }
1254 }
1255
1256 m_pidControllerActive = true;
1257 }
1258 else
1259 {
1260 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1261 }
1262 }
1263
1264 private void changePosition( Vector3 newPos)
1265 {
1266 if (Body != IntPtr.Zero)
1267 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1268 _position = newPos;
1269 }
1270
1271 private void changeOrientation(Quaternion newOri)
1272 {
1273 }
1274
1275 private void changeVelocity(Vector3 newVel)
1276 {
1277 m_pidControllerActive = true;
1278 _target_velocity = newVel;
1279 }
1280
1281 private void changeSetTorque(Vector3 newTorque)
1282 {
1283 }
1284
1285 private void changeAddForce(Vector3 newForce)
1286 {
1287 }
1288
1289 private void changeAddAngularForce(Vector3 arg)
1290 {
1291 }
1292
1293 private void changeAngularLock(Vector3 arg)
1294 {
1295 }
1296
1297 private void changeFloatOnWater(bool arg)
1298 {
1299 }
1300
1301 private void changeVolumedetetion(bool arg)
1302 {
1303 }
1304
1305 private void changeSelectedStatus(bool arg)
1306 {
1307 }
1308
1309 private void changeDisable(bool arg)
1310 {
1311 }
1312
1313 private void changeBuilding(bool arg)
1314 {
1315 }
1316
1317 private void changeForce(Vector3 newForce)
1318 {
1319 m_pidControllerActive = false;
1320 if (Body != IntPtr.Zero)
1321 {
1322 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1323 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1324 }
1325 }
1326
1327 private void donullchange()
1328 {
1329 }
1330
1331 public bool DoAChange(changes what, object arg)
1332 {
1333 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1334 {
1335 return false;
1336 }
1337
1338 // nasty switch
1339 switch (what)
1340 {
1341 case changes.Add:
1342 changeAdd();
1343 break;
1344 case changes.Remove:
1345 changeRemove();
1346 break;
1347
1348 case changes.Position:
1349 changePosition((Vector3)arg);
1350 break;
1351
1352 case changes.Orientation:
1353 changeOrientation((Quaternion)arg);
1354 break;
1355
1356 case changes.PosOffset:
1357 donullchange();
1358 break;
1359
1360 case changes.OriOffset:
1361 donullchange();
1362 break;
1363
1364 case changes.Velocity:
1365 changeVelocity((Vector3)arg);
1366 break;
1367
1368 // case changes.Acceleration:
1369 // changeacceleration((Vector3)arg);
1370 // break;
1371 // case changes.AngVelocity:
1372 // changeangvelocity((Vector3)arg);
1373 // break;
1374
1375 case changes.Force:
1376 changeForce((Vector3)arg);
1377 break;
1378
1379 case changes.Torque:
1380 changeSetTorque((Vector3)arg);
1381 break;
1382
1383 case changes.AddForce:
1384 changeAddForce((Vector3)arg);
1385 break;
1386
1387 case changes.AddAngForce:
1388 changeAddAngularForce((Vector3)arg);
1389 break;
1390
1391 case changes.AngLock:
1392 changeAngularLock((Vector3)arg);
1393 break;
1394
1395 case changes.Size:
1396 changeSize((Vector3)arg);
1397 break;
1398/* not in use for now
1399 case changes.Shape:
1400 changeShape((PrimitiveBaseShape)arg);
1401 break;
1402
1403 case changes.CollidesWater:
1404 changeFloatOnWater((bool)arg);
1405 break;
1406
1407 case changes.VolumeDtc:
1408 changeVolumedetetion((bool)arg);
1409 break;
1410
1411 case changes.Physical:
1412 changePhysicsStatus((bool)arg);
1413 break;
1414
1415 case changes.Selected:
1416 changeSelectedStatus((bool)arg);
1417 break;
1418
1419 case changes.disabled:
1420 changeDisable((bool)arg);
1421 break;
1422
1423 case changes.building:
1424 changeBuilding((bool)arg);
1425 break;
1426*/
1427 case changes.Null:
1428 donullchange();
1429 break;
1430
1431 default:
1432 donullchange();
1433 break;
1434 }
1435 return false;
1436 }
1437
1438 public void AddChange(changes what, object arg)
1439 {
1440 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1441 }
1442
1443
1444 internal void AddCollisionFrameTime(int p)
1445 {
1446 // protect it from overflow crashing
1447 if (m_eventsubscription + p >= int.MaxValue)
1448 m_eventsubscription = 0;
1449 m_eventsubscription += p;
1450 }
1451 }
1452}
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..f739183
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,4024 @@
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
72 protected bool m_building;
73 protected bool m_forcePosOrRotation;
74
75 private Quaternion m_lastorientation = new Quaternion();
76 private Quaternion _orientation;
77
78 private Vector3 _position;
79 private Vector3 _velocity;
80 private Vector3 _torque;
81 private Vector3 m_lastVelocity;
82 private Vector3 m_lastposition;
83 private Vector3 m_rotationalVelocity;
84 private Vector3 _size;
85 private Vector3 _acceleration;
86 private Vector3 m_angularlock = Vector3.One;
87 private IntPtr Amotor = IntPtr.Zero;
88
89 private Vector3 m_force;
90 private Vector3 m_forceacc;
91 private Vector3 m_angularForceacc;
92
93 private Vector3 m_PIDTarget;
94 private float m_PIDTau;
95 private float PID_D = 35f;
96 private float PID_G = 25f;
97 private bool m_usePID;
98
99 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
100 // and are for non-VEHICLES only.
101
102 private float m_PIDHoverHeight;
103 private float m_PIDHoverTau;
104 private bool m_useHoverPID;
105 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
106 private float m_targetHoverHeight;
107 private float m_groundHeight;
108 private float m_waterHeight;
109 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
110
111 private int body_autodisable_frames = 20;
112
113 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
114 | CollisionCategories.Space
115 | CollisionCategories.Body
116 | CollisionCategories.Character
117 );
118// private bool m_collidesLand = true;
119 private bool m_collidesWater;
120 public bool m_returnCollisions;
121 private bool m_softcolide;
122
123 private bool m_NoColide; // for now only for internal use for bad meshs
124
125 // Default we're a Geometry
126 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
127
128 // Default, Collide with Other Geometries, spaces and Bodies
129 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
130
131 public bool m_disabled;
132
133 public uint m_localID;
134
135 private IMesh m_mesh;
136 private object m_meshlock = new object();
137 private PrimitiveBaseShape _pbs;
138 public OdeScene _parent_scene;
139
140 /// <summary>
141 /// The physics space which contains prim geometry
142 /// </summary>
143 public IntPtr m_targetSpace = IntPtr.Zero;
144
145 public IntPtr prim_geom;
146 public IntPtr _triMeshData;
147
148 private PhysicsActor _parent;
149
150 private List<OdePrim> childrenPrim = new List<OdePrim>();
151
152 private bool m_iscolliding;
153
154 public bool m_isSelected;
155 private bool m_delaySelect;
156 private bool m_lastdoneSelected;
157 public bool m_outbounds;
158
159 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
160
161 private bool m_throttleUpdates;
162 private int throttleCounter;
163 public float m_collisionscore;
164 int m_colliderfilter = 0;
165
166 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
167
168 private float m_density = 10.000006836f; // Aluminum g/cm3;
169 private byte m_shapetype;
170 public bool _zeroFlag;
171 private bool m_lastUpdateSent;
172
173 public IntPtr Body = IntPtr.Zero;
174 public String Name { get; private set; }
175 private Vector3 _target_velocity;
176
177 public Vector3 primOOBsize; // prim real dimensions from mesh
178 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
179 public float primOOBradiusSQ;
180 public d.Mass primdMass; // prim inertia information on it's own referencial
181 float primMass; // prim own mass
182 float _mass; // object mass acording to case
183 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
184
185 public int givefakepos = 0;
186 private Vector3 fakepos;
187 public int givefakeori = 0;
188 private Quaternion fakeori;
189
190 public int m_eventsubscription;
191 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
192
193 public volatile bool childPrim;
194
195 public ODEDynamics m_vehicle;
196
197 internal int m_material = (int)Material.Wood;
198 private float mu;
199 private float bounce;
200
201 /// <summary>
202 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
203 /// </summary>
204 public override bool IsPhysical // this is not reliable for internal use
205 {
206 get { return m_fakeisphysical; }
207 set
208 {
209 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
210 // and also to stop imediatly some updates
211 // but real change will only happen in taintprocessing
212
213 if (!value) // Zero the remembered last velocity
214 m_lastVelocity = Vector3.Zero;
215 AddChange(changes.Physical, value);
216 }
217 }
218
219 public override bool Phantom // this is not reliable for internal use
220 {
221 get { return m_fakeisphantom; }
222 set
223 {
224 m_fakeisphantom = value; // we show imediatly to outside that we changed physical
225 // and also to stop imediatly some updates
226 // but real change will only happen in taintprocessing
227
228 AddChange(changes.Phantom, value);
229 }
230 }
231
232 public override bool Building // this is not reliable for internal use
233 {
234 get { return m_building; }
235 set
236 {
237 if (value)
238 m_building = true;
239 AddChange(changes.building, value);
240 }
241 }
242
243 public override void getContactData(ref ContactData cdata)
244 {
245 cdata.mu = mu;
246 cdata.bounce = bounce;
247
248 // cdata.softcolide = m_softcolide;
249 cdata.softcolide = false;
250
251 if (m_isphysical)
252 {
253 ODEDynamics veh;
254 if (_parent != null)
255 veh = ((OdePrim)_parent).m_vehicle;
256 else
257 veh = m_vehicle;
258
259 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
260 cdata.mu *= veh.FrictionFactor;
261 }
262 }
263
264 public override int PhysicsActorType
265 {
266 get { return (int)ActorTypes.Prim; }
267 set { return; }
268 }
269
270 public override bool SetAlwaysRun
271 {
272 get { return false; }
273 set { return; }
274 }
275
276 public override uint LocalID
277 {
278 get
279 {
280 return m_localID;
281 }
282 set
283 {
284 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
285 m_localID = value;
286 }
287 }
288
289 public override bool Grabbed
290 {
291 set { return; }
292 }
293
294 public override bool Selected
295 {
296 set
297 {
298 if (value)
299 m_isSelected = value; // if true set imediatly to stop moves etc
300 AddChange(changes.Selected, value);
301 }
302 }
303
304 public override bool Flying
305 {
306 // no flying prims for you
307 get { return false; }
308 set { }
309 }
310
311 public override bool IsColliding
312 {
313 get { return m_iscolliding; }
314 set
315 {
316 if (value)
317 {
318 m_colliderfilter += 2;
319 if (m_colliderfilter > 2)
320 m_colliderfilter = 2;
321 }
322 else
323 {
324 m_colliderfilter--;
325 if (m_colliderfilter < 0)
326 m_colliderfilter = 0;
327 }
328
329 if (m_colliderfilter == 0)
330 {
331 m_softcolide = false;
332 m_iscolliding = false;
333 }
334 else
335 m_iscolliding = true;
336 }
337 }
338
339 public override bool CollidingGround
340 {
341 get { return false; }
342 set { return; }
343 }
344
345 public override bool CollidingObj
346 {
347 get { return false; }
348 set { return; }
349 }
350
351 public override bool ThrottleUpdates
352 {
353 get { return m_throttleUpdates; }
354 set { m_throttleUpdates = value; }
355 }
356
357 public override bool Stopped
358 {
359 get { return _zeroFlag; }
360 }
361
362 public override Vector3 Position
363 {
364 get
365 {
366 if (givefakepos > 0)
367 return fakepos;
368 else
369 return _position;
370 }
371
372 set
373 {
374 fakepos = value;
375 givefakepos++;
376 AddChange(changes.Position, value);
377 }
378 }
379
380 public override Vector3 Size
381 {
382 get { return _size; }
383 set
384 {
385 if (value.IsFinite())
386 {
387 AddChange(changes.Size, value);
388 }
389 else
390 {
391 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
392 }
393 }
394 }
395
396 public override float Mass
397 {
398 get { return primMass; }
399 }
400
401 public override Vector3 Force
402 {
403 //get { return Vector3.Zero; }
404 get { return m_force; }
405 set
406 {
407 if (value.IsFinite())
408 {
409 AddChange(changes.Force, value);
410 }
411 else
412 {
413 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
414 }
415 }
416 }
417
418 public override void SetVolumeDetect(int param)
419 {
420 AddChange(changes.VolumeDtc, (param != 0));
421 }
422
423 public override Vector3 GeometricCenter
424 {
425 get
426 {
427 return Vector3.Zero;
428 }
429 }
430
431 public override Vector3 CenterOfMass
432 {
433 get
434 {
435 d.Vector3 dtmp;
436 if (IsPhysical && !childPrim && Body != IntPtr.Zero)
437 {
438 dtmp = d.BodyGetPosition(Body);
439 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
440 }
441 else if (prim_geom != IntPtr.Zero)
442 {
443 d.Quaternion dq;
444 d.GeomCopyQuaternion(prim_geom, out dq);
445 Quaternion q;
446 q.X = dq.X;
447 q.Y = dq.Y;
448 q.Z = dq.Z;
449 q.W = dq.W;
450
451 Vector3 vtmp = primOOBoffset * q;
452 dtmp = d.GeomGetPosition(prim_geom);
453 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
454 }
455 else
456 return Vector3.Zero;
457 }
458 }
459 /*
460 public override Vector3 PrimOOBsize
461 {
462 get
463 {
464 return primOOBsize;
465 }
466 }
467
468 public override Vector3 PrimOOBoffset
469 {
470 get
471 {
472 return primOOBoffset;
473 }
474 }
475
476 public override float PrimOOBRadiusSQ
477 {
478 get
479 {
480 return primOOBradiusSQ;
481 }
482 }
483 */
484 public override PrimitiveBaseShape Shape
485 {
486 set
487 {
488/*
489 IMesh mesh = null;
490 if (_parent_scene.needsMeshing(value))
491 {
492 bool convex;
493 if (m_shapetype == 0)
494 convex = false;
495 else
496 convex = true;
497 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
498 }
499
500 if (mesh != null)
501 {
502 lock (m_meshlock)
503 m_mesh = mesh;
504 }
505*/
506 AddChange(changes.Shape, value);
507 }
508 }
509
510 public override byte PhysicsShapeType
511 {
512 get
513 {
514 return m_shapetype;
515 }
516 set
517 {
518 m_shapetype = value;
519 AddChange(changes.Shape, null);
520 }
521 }
522
523
524 public override Vector3 Velocity
525 {
526 get
527 {
528 if (_zeroFlag)
529 return Vector3.Zero;
530 return _velocity;
531 }
532 set
533 {
534 if (value.IsFinite())
535 {
536 AddChange(changes.Velocity, value);
537 // _velocity = value;
538
539 }
540 else
541 {
542 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
543 }
544
545 }
546 }
547
548 public override Vector3 Torque
549 {
550 get
551 {
552 if (!IsPhysical || Body == IntPtr.Zero)
553 return Vector3.Zero;
554
555 return _torque;
556 }
557
558 set
559 {
560 if (value.IsFinite())
561 {
562 AddChange(changes.Torque, value);
563 }
564 else
565 {
566 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
567 }
568 }
569 }
570
571 public override float CollisionScore
572 {
573 get { return m_collisionscore; }
574 set { m_collisionscore = value; }
575 }
576
577 public override bool Kinematic
578 {
579 get { return false; }
580 set { }
581 }
582
583 public override Quaternion Orientation
584 {
585 get
586 {
587 if (givefakeori > 0)
588 return fakeori;
589 else
590
591 return _orientation;
592 }
593 set
594 {
595 if (QuaternionIsFinite(value))
596 {
597 fakeori = value;
598 givefakeori++;
599 AddChange(changes.Orientation, value);
600 }
601 else
602 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
603
604 }
605 }
606
607 public override Vector3 Acceleration
608 {
609 get { return _acceleration; }
610 set { }
611 }
612
613 public override Vector3 RotationalVelocity
614 {
615 get
616 {
617 Vector3 pv = Vector3.Zero;
618 if (_zeroFlag)
619 return pv;
620 m_lastUpdateSent = false;
621
622 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
623 return pv;
624
625 return m_rotationalVelocity;
626 }
627 set
628 {
629 if (value.IsFinite())
630 {
631 m_rotationalVelocity = value;
632 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
633 d.BodyEnable(Body);
634 }
635 else
636 {
637 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
638 }
639 }
640 }
641
642
643 public override float Buoyancy
644 {
645 get { return m_buoyancy; }
646 set
647 {
648 m_buoyancy = value;
649 }
650 }
651
652 public override bool FloatOnWater
653 {
654 set
655 {
656 AddChange(changes.CollidesWater, value);
657 }
658 }
659
660 public override Vector3 PIDTarget
661 {
662 set
663 {
664 if (value.IsFinite())
665 {
666 m_PIDTarget = value;
667 }
668 else
669 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
670 }
671 }
672
673 public override bool PIDActive { set { m_usePID = value; } }
674 public override float PIDTau { set { m_PIDTau = value; } }
675
676 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
677 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
678 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
679 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
680
681 public override Quaternion APIDTarget { set { return; } }
682
683 public override bool APIDActive { set { return; } }
684
685 public override float APIDStrength { set { return; } }
686
687 public override float APIDDamping { set { return; } }
688
689 public override int VehicleType
690 {
691 // we may need to put a fake on this
692 get
693 {
694 if (m_vehicle == null)
695 return (int)Vehicle.TYPE_NONE;
696 else
697 return (int)m_vehicle.Type;
698 }
699 set
700 {
701 AddChange(changes.VehicleType, value);
702 }
703 }
704
705 public override void VehicleFloatParam(int param, float value)
706 {
707 strVehicleFloatParam fp = new strVehicleFloatParam();
708 fp.param = param;
709 fp.value = value;
710 AddChange(changes.VehicleFloatParam, fp);
711 }
712
713 public override void VehicleVectorParam(int param, Vector3 value)
714 {
715 strVehicleVectorParam fp = new strVehicleVectorParam();
716 fp.param = param;
717 fp.value = value;
718 AddChange(changes.VehicleVectorParam, fp);
719 }
720
721 public override void VehicleRotationParam(int param, Quaternion value)
722 {
723 strVehicleQuatParam fp = new strVehicleQuatParam();
724 fp.param = param;
725 fp.value = value;
726 AddChange(changes.VehicleRotationParam, fp);
727 }
728
729 public override void VehicleFlags(int param, bool value)
730 {
731 strVehicleBoolParam bp = new strVehicleBoolParam();
732 bp.param = param;
733 bp.value = value;
734 AddChange(changes.VehicleFlags, bp);
735 }
736
737 public override void SetVehicle(object vdata)
738 {
739 AddChange(changes.SetVehicle, vdata);
740 }
741 public void SetAcceleration(Vector3 accel)
742 {
743 _acceleration = accel;
744 }
745
746 public override void AddForce(Vector3 force, bool pushforce)
747 {
748 if (force.IsFinite())
749 {
750 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE);
751 }
752 else
753 {
754 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
755 }
756 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
757 }
758
759 public override void AddAngularForce(Vector3 force, bool pushforce)
760 {
761 if (force.IsFinite())
762 {
763 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE);
764 }
765 else
766 {
767 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
768 }
769 }
770
771 public override void CrossingFailure()
772 {
773 if (m_outbounds)
774 {
775 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
776 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
777 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
778
779 m_lastposition = _position;
780 _velocity.X = 0;
781 _velocity.Y = 0;
782 _velocity.Z = 0;
783
784 m_lastVelocity = _velocity;
785 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
786 m_vehicle.Stop();
787
788 if(Body != IntPtr.Zero)
789 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
790 if (prim_geom != IntPtr.Zero)
791 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
792
793 m_outbounds = false;
794 changeDisable(false);
795 base.RequestPhysicsterseUpdate();
796 }
797 }
798
799 public override void SetMomentum(Vector3 momentum)
800 {
801 }
802
803 public override void SetMaterial(int pMaterial)
804 {
805 m_material = pMaterial;
806 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
807 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
808 }
809
810 public void setPrimForRemoval()
811 {
812 AddChange(changes.Remove, null);
813 }
814
815 public override void link(PhysicsActor obj)
816 {
817 AddChange(changes.Link, obj);
818 }
819
820 public override void delink()
821 {
822 AddChange(changes.DeLink, null);
823 }
824
825 public override void LockAngularMotion(Vector3 axis)
826 {
827 // reverse the zero/non zero values for ODE.
828 if (axis.IsFinite())
829 {
830 axis.X = (axis.X > 0) ? 1f : 0f;
831 axis.Y = (axis.Y > 0) ? 1f : 0f;
832 axis.Z = (axis.Z > 0) ? 1f : 0f;
833 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
834 AddChange(changes.AngLock, axis);
835 }
836 else
837 {
838 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
839 }
840 }
841
842 public override void SubscribeEvents(int ms)
843 {
844 m_eventsubscription = ms;
845 _parent_scene.AddCollisionEventReporting(this);
846 }
847
848 public override void UnSubscribeEvents()
849 {
850 _parent_scene.RemoveCollisionEventReporting(this);
851 m_eventsubscription = 0;
852 }
853
854 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
855 {
856 if (CollisionEventsThisFrame == null)
857 CollisionEventsThisFrame = new CollisionEventUpdate();
858
859 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
860 }
861
862 public void SendCollisions()
863 {
864 if (CollisionEventsThisFrame == null)
865 return;
866
867 base.SendCollisionUpdate(CollisionEventsThisFrame);
868
869 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
870 CollisionEventsThisFrame = null;
871 else
872 CollisionEventsThisFrame = new CollisionEventUpdate();
873 }
874
875 public override bool SubscribedEvents()
876 {
877 if (m_eventsubscription > 0)
878 return true;
879 return false;
880 }
881
882
883 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
884 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
885 {
886 Name = primName;
887 LocalID = plocalID;
888
889 m_vehicle = null;
890
891 if (!pos.IsFinite())
892 {
893 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
894 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
895 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
896 }
897 _position = pos;
898 givefakepos = 0;
899
900 PID_D = parent_scene.bodyPIDD;
901 PID_G = parent_scene.bodyPIDG;
902 m_density = parent_scene.geomDefaultDensity;
903 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
904 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
905
906 prim_geom = IntPtr.Zero;
907 collide_geom = IntPtr.Zero;
908 Body = IntPtr.Zero;
909
910 if (!size.IsFinite())
911 {
912 size = new Vector3(0.5f, 0.5f, 0.5f);
913 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
914 }
915
916 if (size.X <= 0) size.X = 0.01f;
917 if (size.Y <= 0) size.Y = 0.01f;
918 if (size.Z <= 0) size.Z = 0.01f;
919
920 _size = size;
921
922 if (!QuaternionIsFinite(rotation))
923 {
924 rotation = Quaternion.Identity;
925 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
926 }
927
928 _orientation = rotation;
929 givefakeori = 0;
930
931 _pbs = pbs;
932
933 _parent_scene = parent_scene;
934 m_targetSpace = IntPtr.Zero;
935
936 if (pos.Z < 0)
937 {
938 m_isphysical = false;
939 }
940 else
941 {
942 m_isphysical = pisPhysical;
943 }
944 m_fakeisphysical = m_isphysical;
945
946 m_isVolumeDetect = false;
947
948 m_force = Vector3.Zero;
949
950 m_iscolliding = false;
951 m_colliderfilter = 0;
952 m_softcolide = true;
953 m_NoColide = false;
954
955 hasOOBoffsetFromMesh = false;
956 _triMeshData = IntPtr.Zero;
957
958 m_shapetype = _shapeType;
959
960 m_lastdoneSelected = false;
961 m_isSelected = false;
962 m_delaySelect = false;
963
964 m_isphantom = pisPhantom;
965 m_fakeisphantom = pisPhantom;
966
967 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
968 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
969
970 CalcPrimBodyData();
971
972 m_mesh = null;
973 if (_parent_scene.needsMeshing(pbs))
974 {
975 bool convex;
976 if (m_shapetype == 0)
977 convex = false;
978 else
979 convex = true;
980
981 m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
982 }
983
984
985 m_building = true; // control must set this to false when done
986
987 AddChange(changes.Add, null);
988 }
989
990 private void resetCollisionAccounting()
991 {
992 m_collisionscore = 0;
993 }
994
995 private void createAMotor(Vector3 axis)
996 {
997 if (Body == IntPtr.Zero)
998 return;
999
1000 if (Amotor != IntPtr.Zero)
1001 {
1002 d.JointDestroy(Amotor);
1003 Amotor = IntPtr.Zero;
1004 }
1005
1006 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1007
1008 if (axisnum <= 0)
1009 return;
1010
1011 // stop it
1012 d.BodySetTorque(Body, 0, 0, 0);
1013 d.BodySetAngularVel(Body, 0, 0, 0);
1014
1015 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1016 d.JointAttach(Amotor, Body, IntPtr.Zero);
1017
1018 d.JointSetAMotorMode(Amotor, 0);
1019
1020 d.JointSetAMotorNumAxes(Amotor, axisnum);
1021
1022 // get current orientation to lock
1023
1024 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1025 Quaternion curr; // crap convertion between identical things
1026 curr.X = dcur.X;
1027 curr.Y = dcur.Y;
1028 curr.Z = dcur.Z;
1029 curr.W = dcur.W;
1030 Vector3 ax;
1031
1032 int i = 0;
1033 int j = 0;
1034 if (axis.X == 0)
1035 {
1036 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1037 // ODE should do this with axis relative to body 1 but seems to fail
1038 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1039 d.JointSetAMotorAngle(Amotor, 0, 0);
1040 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1041 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1042 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1043 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1044 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1045 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1046 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1047 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1048 i++;
1049 j = 256; // move to next axis set
1050 }
1051
1052 if (axis.Y == 0)
1053 {
1054 ax = (new Vector3(0, 1, 0)) * curr;
1055 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1056 d.JointSetAMotorAngle(Amotor, i, 0);
1057 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1058 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1059 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1060 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1061 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1062 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1063 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1064 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1065 i++;
1066 j += 256;
1067 }
1068
1069 if (axis.Z == 0)
1070 {
1071 ax = (new Vector3(0, 0, 1)) * curr;
1072 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1073 d.JointSetAMotorAngle(Amotor, i, 0);
1074 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1075 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1076 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1077 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1078 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1079 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1080 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1081 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1082 }
1083 }
1084
1085 private bool setMesh(OdeScene parent_scene)
1086 {
1087 IntPtr vertices, indices;
1088 int vertexCount, indexCount;
1089 int vertexStride, triStride;
1090
1091 if (Body != IntPtr.Zero)
1092 {
1093 if (childPrim)
1094 {
1095 if (_parent != null)
1096 {
1097 OdePrim parent = (OdePrim)_parent;
1098 parent.ChildDelink(this, false);
1099 }
1100 }
1101 else
1102 {
1103 DestroyBody();
1104 }
1105 }
1106
1107 IMesh mesh = null;
1108
1109
1110 lock (m_meshlock)
1111 {
1112 if (m_mesh == null)
1113 {
1114 bool convex;
1115 if (m_shapetype == 0)
1116 convex = false;
1117 else
1118 convex = true;
1119
1120 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1121 }
1122 else
1123 {
1124 mesh = m_mesh;
1125 }
1126
1127 if (mesh == null)
1128 {
1129 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1130 return false;
1131 }
1132
1133
1134 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1135 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1136
1137 if (vertexCount == 0 || indexCount == 0)
1138 {
1139 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1140 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1141 mesh.releaseSourceMeshData();
1142 return false;
1143 }
1144
1145 primOOBoffset = mesh.GetCentroid();
1146 hasOOBoffsetFromMesh = true;
1147
1148 mesh.releaseSourceMeshData();
1149 m_mesh = null;
1150 }
1151
1152 IntPtr geo = IntPtr.Zero;
1153
1154 try
1155 {
1156 _triMeshData = d.GeomTriMeshDataCreate();
1157
1158 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1159 d.GeomTriMeshDataPreprocess(_triMeshData);
1160
1161 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1162 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1163 }
1164
1165 catch (Exception e)
1166 {
1167 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1168 if (_triMeshData != IntPtr.Zero)
1169 {
1170 d.GeomTriMeshDataDestroy(_triMeshData);
1171 _triMeshData = IntPtr.Zero;
1172 }
1173 return false;
1174 }
1175
1176 SetGeom(geo);
1177 return true;
1178 }
1179
1180 private void SetGeom(IntPtr geom)
1181 {
1182 prim_geom = geom;
1183 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1184 if (prim_geom != IntPtr.Zero)
1185 {
1186 if (m_NoColide)
1187 {
1188 d.GeomSetCategoryBits(prim_geom, 0);
1189 if (m_isphysical)
1190 {
1191 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1192 }
1193 else
1194 {
1195 d.GeomSetCollideBits(prim_geom, 0);
1196 d.GeomDisable(prim_geom);
1197 }
1198 }
1199 else
1200 {
1201 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1202 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1203 }
1204
1205 CalcPrimBodyData();
1206
1207 _parent_scene.geom_name_map[prim_geom] = Name;
1208 _parent_scene.actor_name_map[prim_geom] = this;
1209
1210 }
1211 else
1212 m_log.Warn("Setting bad Geom");
1213 }
1214
1215
1216 /// <summary>
1217 /// Create a geometry for the given mesh in the given target space.
1218 /// </summary>
1219 /// <param name="m_targetSpace"></param>
1220 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1221 private void CreateGeom()
1222 {
1223 if (_triMeshData != IntPtr.Zero)
1224 {
1225 d.GeomTriMeshDataDestroy(_triMeshData);
1226 _triMeshData = IntPtr.Zero;
1227 }
1228
1229 bool haveMesh = false;
1230 hasOOBoffsetFromMesh = false;
1231 m_NoColide = false;
1232
1233 if (_parent_scene.needsMeshing(_pbs))
1234 {
1235 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1236 if (!haveMesh)
1237 m_NoColide = true;
1238 }
1239
1240 if (!haveMesh)
1241 {
1242 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1243 && _size.X == _size.Y && _size.Y == _size.Z)
1244 { // it's a sphere
1245 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1246 try
1247 {
1248 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1249 }
1250 catch (Exception e)
1251 {
1252 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1253 return;
1254 }
1255 }
1256 else
1257 {// do it as a box
1258 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1259 try
1260 {
1261 //Console.WriteLine(" CreateGeom 4");
1262 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1263 }
1264 catch (Exception e)
1265 {
1266 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1267 return;
1268 }
1269 }
1270 }
1271 }
1272
1273 /// <summary>
1274 /// Set a new geometry for this prim.
1275 /// </summary>
1276 /// <param name="geom"></param>
1277 private void RemoveGeom()
1278 {
1279 if (prim_geom != IntPtr.Zero)
1280 {
1281 _parent_scene.geom_name_map.Remove(prim_geom);
1282 _parent_scene.actor_name_map.Remove(prim_geom);
1283 try
1284 {
1285 d.GeomDestroy(prim_geom);
1286 if (_triMeshData != IntPtr.Zero)
1287 {
1288 d.GeomTriMeshDataDestroy(_triMeshData);
1289 _triMeshData = IntPtr.Zero;
1290 }
1291 }
1292 // catch (System.AccessViolationException)
1293 catch (Exception e)
1294 {
1295 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1296 }
1297
1298 prim_geom = IntPtr.Zero;
1299 }
1300 else
1301 {
1302 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1303 }
1304 Body = IntPtr.Zero;
1305 hasOOBoffsetFromMesh = false;
1306 CalcPrimBodyData();
1307 }
1308
1309 private void ChildSetGeom(OdePrim odePrim)
1310 {
1311 // well..
1312 DestroyBody();
1313 MakeBody();
1314 }
1315
1316 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1317 // should only be called for non physical prims unless they are becoming non physical
1318 private void SetInStaticSpace(OdePrim prim)
1319 {
1320 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1321 prim.m_targetSpace = targetSpace;
1322 d.GeomEnable(prim_geom);
1323 }
1324
1325 public void enableBodySoft()
1326 {
1327 if (!childPrim && !m_isSelected)
1328 {
1329 if (m_isphysical && Body != IntPtr.Zero)
1330 {
1331 if (m_isphantom && !m_isVolumeDetect)
1332 {
1333 m_collisionCategories = 0;
1334 m_collisionFlags = CollisionCategories.Land;
1335 }
1336 else
1337 {
1338 m_collisionCategories |= CollisionCategories.Body;
1339 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1340 }
1341
1342 foreach (OdePrim prm in childrenPrim)
1343 {
1344 prm.m_collisionCategories = m_collisionCategories;
1345 prm.m_collisionFlags = m_collisionFlags;
1346
1347 if (prm.prim_geom != IntPtr.Zero)
1348 {
1349 if (prm.m_NoColide)
1350 {
1351 d.GeomSetCategoryBits(prm.prim_geom, 0);
1352 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1353 }
1354 else
1355 {
1356 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1357 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1358 }
1359 d.GeomEnable(prm.prim_geom);
1360 }
1361 }
1362
1363 if (prim_geom != IntPtr.Zero)
1364 {
1365 if (m_NoColide)
1366 {
1367 d.GeomSetCategoryBits(prim_geom, 0);
1368 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1369 }
1370 else
1371 {
1372 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1373 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1374 }
1375 d.GeomEnable(prim_geom);
1376 }
1377 d.BodyEnable(Body);
1378 }
1379 }
1380 m_disabled = false;
1381 resetCollisionAccounting(); // this sets m_disable to false
1382 }
1383
1384 private void disableBodySoft()
1385 {
1386 m_disabled = true;
1387 if (!childPrim)
1388 {
1389 if (m_isphysical && Body != IntPtr.Zero)
1390 {
1391 m_collisionCategories &= ~CollisionCategories.Body;
1392 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1393
1394 foreach (OdePrim prm in childrenPrim)
1395 {
1396 prm.m_collisionCategories = m_collisionCategories;
1397 prm.m_collisionFlags = m_collisionFlags;
1398
1399 if (prm.prim_geom != IntPtr.Zero)
1400 {
1401 if (prm.m_NoColide)
1402 {
1403 d.GeomSetCategoryBits(prm.prim_geom, 0);
1404 d.GeomSetCollideBits(prm.prim_geom, 0);
1405 }
1406 else
1407 {
1408 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1409 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1410 }
1411 d.GeomDisable(prm.prim_geom);
1412 }
1413 }
1414
1415 if (prim_geom != IntPtr.Zero)
1416 {
1417 if (m_NoColide)
1418 {
1419 d.GeomSetCategoryBits(prim_geom, 0);
1420 d.GeomSetCollideBits(prim_geom, 0);
1421 }
1422 else
1423 {
1424 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1425 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1426 }
1427 d.GeomDisable(prim_geom);
1428 }
1429
1430 d.BodyDisable(Body);
1431 }
1432 }
1433 }
1434
1435 private void MakeBody()
1436 {
1437 if (!m_isphysical) // only physical get bodies
1438 return;
1439
1440 if (childPrim) // child prims don't get bodies;
1441 return;
1442
1443 if (m_building)
1444 return;
1445
1446 if (prim_geom == IntPtr.Zero)
1447 {
1448 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1449 return;
1450 }
1451
1452 if (Body != IntPtr.Zero)
1453 {
1454 d.BodyDestroy(Body);
1455 Body = IntPtr.Zero;
1456 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1457 }
1458
1459
1460 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1461 {
1462 d.GeomSetBody(prim_geom, IntPtr.Zero);
1463 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1464 }
1465
1466 d.Matrix3 mymat = new d.Matrix3();
1467 d.Quaternion myrot = new d.Quaternion();
1468 d.Mass objdmass = new d.Mass { };
1469
1470 Body = d.BodyCreate(_parent_scene.world);
1471
1472 DMassDup(ref primdMass, out objdmass);
1473
1474 // rotate inertia
1475 myrot.X = _orientation.X;
1476 myrot.Y = _orientation.Y;
1477 myrot.Z = _orientation.Z;
1478 myrot.W = _orientation.W;
1479
1480 d.RfromQ(out mymat, ref myrot);
1481 d.MassRotate(ref objdmass, ref mymat);
1482
1483 // set the body rotation
1484 d.BodySetRotation(Body, ref mymat);
1485
1486 // recompute full object inertia if needed
1487 if (childrenPrim.Count > 0)
1488 {
1489 d.Matrix3 mat = new d.Matrix3();
1490 d.Quaternion quat = new d.Quaternion();
1491 d.Mass tmpdmass = new d.Mass { };
1492 Vector3 rcm;
1493
1494 rcm.X = _position.X + objdmass.c.X;
1495 rcm.Y = _position.Y + objdmass.c.Y;
1496 rcm.Z = _position.Z + objdmass.c.Z;
1497
1498 lock (childrenPrim)
1499 {
1500 foreach (OdePrim prm in childrenPrim)
1501 {
1502 if (prm.prim_geom == IntPtr.Zero)
1503 {
1504 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1505 continue;
1506 }
1507
1508 DMassCopy(ref prm.primdMass, ref tmpdmass);
1509
1510 // apply prim current rotation to inertia
1511 quat.X = prm._orientation.X;
1512 quat.Y = prm._orientation.Y;
1513 quat.Z = prm._orientation.Z;
1514 quat.W = prm._orientation.W;
1515 d.RfromQ(out mat, ref quat);
1516 d.MassRotate(ref tmpdmass, ref mat);
1517
1518 Vector3 ppos = prm._position;
1519 ppos.X += tmpdmass.c.X - rcm.X;
1520 ppos.Y += tmpdmass.c.Y - rcm.Y;
1521 ppos.Z += tmpdmass.c.Z - rcm.Z;
1522
1523 // refer inertia to root prim center of mass position
1524 d.MassTranslate(ref tmpdmass,
1525 ppos.X,
1526 ppos.Y,
1527 ppos.Z);
1528
1529 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1530 // fix prim colision cats
1531
1532 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1533 {
1534 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1535 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1536 }
1537
1538 d.GeomClearOffset(prm.prim_geom);
1539 d.GeomSetBody(prm.prim_geom, Body);
1540 prm.Body = Body;
1541 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1542 }
1543 }
1544 }
1545
1546 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1547 // associate root geom with body
1548 d.GeomSetBody(prim_geom, Body);
1549
1550 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1551 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1552
1553 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1554 myrot.W = -myrot.W;
1555 d.RfromQ(out mymat, ref myrot);
1556 d.MassRotate(ref objdmass, ref mymat);
1557 d.BodySetMass(Body, ref objdmass);
1558 _mass = objdmass.mass;
1559
1560 // disconnect from world gravity so we can apply buoyancy
1561 d.BodySetGravityMode(Body, false);
1562
1563 d.BodySetAutoDisableFlag(Body, true);
1564 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1565 // d.BodySetLinearDampingThreshold(Body, 0.01f);
1566 // d.BodySetAngularDampingThreshold(Body, 0.001f);
1567 d.BodySetDamping(Body, .002f, .002f);
1568
1569
1570 if (m_targetSpace != IntPtr.Zero)
1571 {
1572 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1573 if (d.SpaceQuery(m_targetSpace, prim_geom))
1574 d.SpaceRemove(m_targetSpace, prim_geom);
1575 }
1576
1577
1578 if (childrenPrim.Count == 0)
1579 {
1580 collide_geom = prim_geom;
1581 m_targetSpace = _parent_scene.ActiveSpace;
1582 d.SpaceAdd(m_targetSpace, prim_geom);
1583 }
1584 else
1585 {
1586 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1587 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1588 d.SpaceSetSublevel(m_targetSpace, 3);
1589 d.SpaceSetCleanup(m_targetSpace, false);
1590 d.SpaceAdd(m_targetSpace, prim_geom);
1591 collide_geom = m_targetSpace;
1592 }
1593
1594 if (m_delaySelect)
1595 {
1596 m_isSelected = true;
1597 m_delaySelect = false;
1598 }
1599
1600 lock (childrenPrim)
1601 {
1602 foreach (OdePrim prm in childrenPrim)
1603 {
1604 if (prm.prim_geom == IntPtr.Zero)
1605 continue;
1606
1607 Vector3 ppos = prm._position;
1608 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1609
1610 if (prm.m_targetSpace != m_targetSpace)
1611 {
1612 if (prm.m_targetSpace != IntPtr.Zero)
1613 {
1614 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1615 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1616 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1617 }
1618 prm.m_targetSpace = m_targetSpace;
1619 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1620 }
1621
1622 if (m_isSelected || m_disabled)
1623 {
1624 prm.m_collisionCategories &= ~CollisionCategories.Body;
1625 prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1626 d.GeomDisable(prm.prim_geom);
1627 }
1628 else
1629 {
1630 if (m_isphantom && !m_isVolumeDetect)
1631 {
1632 prm.m_collisionCategories = 0;
1633 prm.m_collisionFlags = CollisionCategories.Land;
1634 }
1635 else
1636 {
1637 prm.m_collisionCategories |= CollisionCategories.Body;
1638 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1639 }
1640 d.GeomEnable(prm.prim_geom);
1641 }
1642
1643 if (prm.m_NoColide)
1644 {
1645 d.GeomSetCategoryBits(prm.prim_geom, 0);
1646 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1647 d.GeomEnable(prm.prim_geom);
1648 }
1649 else
1650 {
1651 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1652 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1653 }
1654 prm.m_collisionscore = 0;
1655
1656 if(!m_disabled)
1657 prm.m_disabled = false;
1658
1659 _parent_scene.addActivePrim(prm);
1660 }
1661 }
1662
1663 // The body doesn't already have a finite rotation mode set here
1664 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1665 {
1666 createAMotor(m_angularlock);
1667 }
1668
1669 if (m_isSelected || m_disabled)
1670 {
1671 m_collisionCategories &= ~CollisionCategories.Body;
1672 m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1673
1674 d.GeomDisable(prim_geom);
1675 d.BodyDisable(Body);
1676 }
1677 else
1678 {
1679 if (m_isphantom && !m_isVolumeDetect)
1680 {
1681 m_collisionCategories = 0;
1682 m_collisionFlags = CollisionCategories.Land;
1683 }
1684 else
1685 {
1686 m_collisionCategories |= CollisionCategories.Body;
1687 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1688 }
1689
1690 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1691 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1692 }
1693
1694 if (m_NoColide)
1695 {
1696 d.GeomSetCategoryBits(prim_geom, 0);
1697 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1698 }
1699 else
1700 {
1701 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1702 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1703 }
1704
1705 m_collisionscore = 0;
1706
1707 m_softcolide = true;
1708 _parent_scene.addActivePrim(this);
1709 _parent_scene.addActiveGroups(this);
1710 }
1711
1712 private void DestroyBody()
1713 {
1714 if (Body != IntPtr.Zero)
1715 {
1716 _parent_scene.remActivePrim(this);
1717 m_collisionCategories &= ~CollisionCategories.Body;
1718 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1719 if (prim_geom != IntPtr.Zero)
1720 {
1721 if (m_NoColide)
1722 {
1723 d.GeomSetCategoryBits(prim_geom, 0);
1724 d.GeomSetCollideBits(prim_geom, 0);
1725 }
1726 else
1727 {
1728 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1729 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1730 }
1731 UpdateDataFromGeom();
1732 d.GeomSetBody(prim_geom, IntPtr.Zero);
1733 SetInStaticSpace(this);
1734 }
1735
1736 if (!childPrim)
1737 {
1738 lock (childrenPrim)
1739 {
1740 foreach (OdePrim prm in childrenPrim)
1741 {
1742 _parent_scene.remActivePrim(prm);
1743 prm.m_collisionCategories = m_collisionCategories;
1744 prm.m_collisionFlags = m_collisionFlags;
1745 if (prm.prim_geom != IntPtr.Zero)
1746 {
1747 if (prm.m_NoColide)
1748 {
1749 d.GeomSetCategoryBits(prm.prim_geom, 0);
1750 d.GeomSetCollideBits(prm.prim_geom, 0);
1751 }
1752 else
1753 {
1754 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1755 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1756 }
1757 prm.UpdateDataFromGeom();
1758 SetInStaticSpace(prm);
1759 }
1760 prm.Body = IntPtr.Zero;
1761 prm._mass = prm.primMass;
1762 prm.m_collisionscore = 0;
1763 }
1764 }
1765 if (Amotor != IntPtr.Zero)
1766 {
1767 d.JointDestroy(Amotor);
1768 Amotor = IntPtr.Zero;
1769 }
1770 _parent_scene.remActiveGroup(this);
1771 d.BodyDestroy(Body);
1772 }
1773 Body = IntPtr.Zero;
1774 }
1775 _mass = primMass;
1776 m_collisionscore = 0;
1777 }
1778
1779 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1780 {
1781 d.Matrix3 mat = new d.Matrix3();
1782 d.Quaternion quat = new d.Quaternion();
1783
1784 d.Mass tmpdmass = new d.Mass { };
1785 d.Mass objdmass = new d.Mass { };
1786
1787 d.BodyGetMass(Body, out tmpdmass);
1788 objdmass = tmpdmass;
1789
1790 d.Vector3 dobjpos;
1791 d.Vector3 thispos;
1792
1793 // get current object position and rotation
1794 dobjpos = d.BodyGetPosition(Body);
1795
1796 // get prim own inertia in its local frame
1797 tmpdmass = primdMass;
1798
1799 // transform to object frame
1800 mat = d.GeomGetOffsetRotation(prim_geom);
1801 d.MassRotate(ref tmpdmass, ref mat);
1802
1803 thispos = d.GeomGetOffsetPosition(prim_geom);
1804 d.MassTranslate(ref tmpdmass,
1805 thispos.X,
1806 thispos.Y,
1807 thispos.Z);
1808
1809 // subtract current prim inertia from object
1810 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1811
1812 // back prim own inertia
1813 tmpdmass = primdMass;
1814
1815 // update to new position and orientation
1816 _position = NewPos;
1817 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1818 _orientation = newrot;
1819 quat.X = newrot.X;
1820 quat.Y = newrot.Y;
1821 quat.Z = newrot.Z;
1822 quat.W = newrot.W;
1823 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1824
1825 mat = d.GeomGetOffsetRotation(prim_geom);
1826 d.MassRotate(ref tmpdmass, ref mat);
1827
1828 thispos = d.GeomGetOffsetPosition(prim_geom);
1829 d.MassTranslate(ref tmpdmass,
1830 thispos.X,
1831 thispos.Y,
1832 thispos.Z);
1833
1834 d.MassAdd(ref objdmass, ref tmpdmass);
1835
1836 // fix all positions
1837 IntPtr g = d.BodyGetFirstGeom(Body);
1838 while (g != IntPtr.Zero)
1839 {
1840 thispos = d.GeomGetOffsetPosition(g);
1841 thispos.X -= objdmass.c.X;
1842 thispos.Y -= objdmass.c.Y;
1843 thispos.Z -= objdmass.c.Z;
1844 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1845 g = d.dBodyGetNextGeom(g);
1846 }
1847 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1848
1849 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1850 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1851 d.BodySetMass(Body, ref objdmass);
1852 _mass = objdmass.mass;
1853 }
1854
1855
1856
1857 private void FixInertia(Vector3 NewPos)
1858 {
1859 d.Matrix3 primmat = new d.Matrix3();
1860 d.Mass tmpdmass = new d.Mass { };
1861 d.Mass objdmass = new d.Mass { };
1862 d.Mass primmass = new d.Mass { };
1863
1864 d.Vector3 dobjpos;
1865 d.Vector3 thispos;
1866
1867 d.BodyGetMass(Body, out objdmass);
1868
1869 // get prim own inertia in its local frame
1870 primmass = primdMass;
1871 // transform to object frame
1872 primmat = d.GeomGetOffsetRotation(prim_geom);
1873 d.MassRotate(ref primmass, ref primmat);
1874
1875 tmpdmass = primmass;
1876
1877 thispos = d.GeomGetOffsetPosition(prim_geom);
1878 d.MassTranslate(ref tmpdmass,
1879 thispos.X,
1880 thispos.Y,
1881 thispos.Z);
1882
1883 // subtract current prim inertia from object
1884 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1885
1886 // update to new position
1887 _position = NewPos;
1888 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1889
1890 thispos = d.GeomGetOffsetPosition(prim_geom);
1891 d.MassTranslate(ref primmass,
1892 thispos.X,
1893 thispos.Y,
1894 thispos.Z);
1895
1896 d.MassAdd(ref objdmass, ref primmass);
1897
1898 // fix all positions
1899 IntPtr g = d.BodyGetFirstGeom(Body);
1900 while (g != IntPtr.Zero)
1901 {
1902 thispos = d.GeomGetOffsetPosition(g);
1903 thispos.X -= objdmass.c.X;
1904 thispos.Y -= objdmass.c.Y;
1905 thispos.Z -= objdmass.c.Z;
1906 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1907 g = d.dBodyGetNextGeom(g);
1908 }
1909
1910 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
1911
1912 // get current object position and rotation
1913 dobjpos = d.BodyGetPosition(Body);
1914
1915 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1916 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1917 d.BodySetMass(Body, ref objdmass);
1918 _mass = objdmass.mass;
1919 }
1920
1921 private void FixInertia(Quaternion newrot)
1922 {
1923 d.Matrix3 mat = new d.Matrix3();
1924 d.Quaternion quat = new d.Quaternion();
1925
1926 d.Mass tmpdmass = new d.Mass { };
1927 d.Mass objdmass = new d.Mass { };
1928 d.Vector3 dobjpos;
1929 d.Vector3 thispos;
1930
1931 d.BodyGetMass(Body, out objdmass);
1932
1933 // get prim own inertia in its local frame
1934 tmpdmass = primdMass;
1935 mat = d.GeomGetOffsetRotation(prim_geom);
1936 d.MassRotate(ref tmpdmass, ref mat);
1937 // transform to object frame
1938 thispos = d.GeomGetOffsetPosition(prim_geom);
1939 d.MassTranslate(ref tmpdmass,
1940 thispos.X,
1941 thispos.Y,
1942 thispos.Z);
1943
1944 // subtract current prim inertia from object
1945 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1946
1947 // update to new orientation
1948 _orientation = newrot;
1949 quat.X = newrot.X;
1950 quat.Y = newrot.Y;
1951 quat.Z = newrot.Z;
1952 quat.W = newrot.W;
1953 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1954
1955 tmpdmass = primdMass;
1956 mat = d.GeomGetOffsetRotation(prim_geom);
1957 d.MassRotate(ref tmpdmass, ref mat);
1958 d.MassTranslate(ref tmpdmass,
1959 thispos.X,
1960 thispos.Y,
1961 thispos.Z);
1962
1963 d.MassAdd(ref objdmass, ref tmpdmass);
1964
1965 // fix all positions
1966 IntPtr g = d.BodyGetFirstGeom(Body);
1967 while (g != IntPtr.Zero)
1968 {
1969 thispos = d.GeomGetOffsetPosition(g);
1970 thispos.X -= objdmass.c.X;
1971 thispos.Y -= objdmass.c.Y;
1972 thispos.Z -= objdmass.c.Z;
1973 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1974 g = d.dBodyGetNextGeom(g);
1975 }
1976
1977 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
1978 // get current object position and rotation
1979 dobjpos = d.BodyGetPosition(Body);
1980
1981 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1982 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1983 d.BodySetMass(Body, ref objdmass);
1984 _mass = objdmass.mass;
1985 }
1986
1987
1988 #region Mass Calculation
1989
1990 private float CalculatePrimVolume()
1991 {
1992 float volume = _size.X * _size.Y * _size.Z; // default
1993 float tmp;
1994
1995 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1996 float hollowVolume = hollowAmount * hollowAmount;
1997
1998 switch (_pbs.ProfileShape)
1999 {
2000 case ProfileShape.Square:
2001 // default box
2002
2003 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2004 {
2005 if (hollowAmount > 0.0)
2006 {
2007 switch (_pbs.HollowShape)
2008 {
2009 case HollowShape.Square:
2010 case HollowShape.Same:
2011 break;
2012
2013 case HollowShape.Circle:
2014
2015 hollowVolume *= 0.78539816339f;
2016 break;
2017
2018 case HollowShape.Triangle:
2019
2020 hollowVolume *= (0.5f * .5f);
2021 break;
2022
2023 default:
2024 hollowVolume = 0;
2025 break;
2026 }
2027 volume *= (1.0f - hollowVolume);
2028 }
2029 }
2030
2031 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2032 {
2033 //a tube
2034
2035 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
2036 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
2037 volume -= volume * tmp * tmp;
2038
2039 if (hollowAmount > 0.0)
2040 {
2041 hollowVolume *= hollowAmount;
2042
2043 switch (_pbs.HollowShape)
2044 {
2045 case HollowShape.Square:
2046 case HollowShape.Same:
2047 break;
2048
2049 case HollowShape.Circle:
2050 hollowVolume *= 0.78539816339f;
2051 break;
2052
2053 case HollowShape.Triangle:
2054 hollowVolume *= 0.5f * 0.5f;
2055 break;
2056 default:
2057 hollowVolume = 0;
2058 break;
2059 }
2060 volume *= (1.0f - hollowVolume);
2061 }
2062 }
2063
2064 break;
2065
2066 case ProfileShape.Circle:
2067
2068 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2069 {
2070 volume *= 0.78539816339f; // elipse base
2071
2072 if (hollowAmount > 0.0)
2073 {
2074 switch (_pbs.HollowShape)
2075 {
2076 case HollowShape.Same:
2077 case HollowShape.Circle:
2078 break;
2079
2080 case HollowShape.Square:
2081 hollowVolume *= 0.5f * 2.5984480504799f;
2082 break;
2083
2084 case HollowShape.Triangle:
2085 hollowVolume *= .5f * 1.27323954473516f;
2086 break;
2087
2088 default:
2089 hollowVolume = 0;
2090 break;
2091 }
2092 volume *= (1.0f - hollowVolume);
2093 }
2094 }
2095
2096 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2097 {
2098 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
2099 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2100 volume *= (1.0f - tmp * tmp);
2101
2102 if (hollowAmount > 0.0)
2103 {
2104
2105 // calculate the hollow volume by it's shape compared to the prim shape
2106 hollowVolume *= hollowAmount;
2107
2108 switch (_pbs.HollowShape)
2109 {
2110 case HollowShape.Same:
2111 case HollowShape.Circle:
2112 break;
2113
2114 case HollowShape.Square:
2115 hollowVolume *= 0.5f * 2.5984480504799f;
2116 break;
2117
2118 case HollowShape.Triangle:
2119 hollowVolume *= .5f * 1.27323954473516f;
2120 break;
2121
2122 default:
2123 hollowVolume = 0;
2124 break;
2125 }
2126 volume *= (1.0f - hollowVolume);
2127 }
2128 }
2129 break;
2130
2131 case ProfileShape.HalfCircle:
2132 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2133 {
2134 volume *= 0.52359877559829887307710723054658f;
2135 }
2136 break;
2137
2138 case ProfileShape.EquilateralTriangle:
2139
2140 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2141 {
2142 volume *= 0.32475953f;
2143
2144 if (hollowAmount > 0.0)
2145 {
2146
2147 // calculate the hollow volume by it's shape compared to the prim shape
2148 switch (_pbs.HollowShape)
2149 {
2150 case HollowShape.Same:
2151 case HollowShape.Triangle:
2152 hollowVolume *= .25f;
2153 break;
2154
2155 case HollowShape.Square:
2156 hollowVolume *= 0.499849f * 3.07920140172638f;
2157 break;
2158
2159 case HollowShape.Circle:
2160 // Hollow shape is a perfect cyllinder in respect to the cube's scale
2161 // Cyllinder hollow volume calculation
2162
2163 hollowVolume *= 0.1963495f * 3.07920140172638f;
2164 break;
2165
2166 default:
2167 hollowVolume = 0;
2168 break;
2169 }
2170 volume *= (1.0f - hollowVolume);
2171 }
2172 }
2173 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2174 {
2175 volume *= 0.32475953f;
2176 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
2177 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2178 volume *= (1.0f - tmp * tmp);
2179
2180 if (hollowAmount > 0.0)
2181 {
2182
2183 hollowVolume *= hollowAmount;
2184
2185 switch (_pbs.HollowShape)
2186 {
2187 case HollowShape.Same:
2188 case HollowShape.Triangle:
2189 hollowVolume *= .25f;
2190 break;
2191
2192 case HollowShape.Square:
2193 hollowVolume *= 0.499849f * 3.07920140172638f;
2194 break;
2195
2196 case HollowShape.Circle:
2197
2198 hollowVolume *= 0.1963495f * 3.07920140172638f;
2199 break;
2200
2201 default:
2202 hollowVolume = 0;
2203 break;
2204 }
2205 volume *= (1.0f - hollowVolume);
2206 }
2207 }
2208 break;
2209
2210 default:
2211 break;
2212 }
2213
2214 float taperX1;
2215 float taperY1;
2216 float taperX;
2217 float taperY;
2218 float pathBegin;
2219 float pathEnd;
2220 float profileBegin;
2221 float profileEnd;
2222
2223 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
2224 {
2225 taperX1 = _pbs.PathScaleX * 0.01f;
2226 if (taperX1 > 1.0f)
2227 taperX1 = 2.0f - taperX1;
2228 taperX = 1.0f - taperX1;
2229
2230 taperY1 = _pbs.PathScaleY * 0.01f;
2231 if (taperY1 > 1.0f)
2232 taperY1 = 2.0f - taperY1;
2233 taperY = 1.0f - taperY1;
2234 }
2235 else
2236 {
2237 taperX = _pbs.PathTaperX * 0.01f;
2238 if (taperX < 0.0f)
2239 taperX = -taperX;
2240 taperX1 = 1.0f - taperX;
2241
2242 taperY = _pbs.PathTaperY * 0.01f;
2243 if (taperY < 0.0f)
2244 taperY = -taperY;
2245 taperY1 = 1.0f - taperY;
2246 }
2247
2248 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
2249
2250 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
2251 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
2252 volume *= (pathEnd - pathBegin);
2253
2254 // this is crude aproximation
2255 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
2256 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
2257 volume *= (profileEnd - profileBegin);
2258
2259 return volume;
2260 }
2261
2262
2263 private void CalcPrimBodyData()
2264 {
2265 float volume;
2266
2267 if (prim_geom == IntPtr.Zero)
2268 {
2269 // Ubit let's have a initial basic OOB
2270 primOOBsize.X = _size.X;
2271 primOOBsize.Y = _size.Y;
2272 primOOBsize.Z = _size.Z;
2273 primOOBoffset = Vector3.Zero;
2274 }
2275 else
2276 {
2277 d.AABB AABB;
2278 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
2279
2280 primOOBsize.X = (AABB.MaxX - AABB.MinX);
2281 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
2282 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
2283 if (!hasOOBoffsetFromMesh)
2284 {
2285 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
2286 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
2287 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
2288 }
2289 }
2290
2291 // also its own inertia and mass
2292 // keep using basic shape mass for now
2293 volume = CalculatePrimVolume();
2294
2295 primMass = m_density * volume;
2296
2297 if (primMass <= 0)
2298 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2299 if (primMass > _parent_scene.maximumMassObject)
2300 primMass = _parent_scene.maximumMassObject;
2301
2302 _mass = primMass; // just in case
2303
2304 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
2305
2306 d.MassTranslate(ref primdMass,
2307 primOOBoffset.X,
2308 primOOBoffset.Y,
2309 primOOBoffset.Z);
2310
2311 primOOBsize *= 0.5f; // let obb size be a corner coords
2312 primOOBradiusSQ = primOOBsize.LengthSquared();
2313 }
2314
2315
2316 #endregion
2317
2318
2319 /// <summary>
2320 /// Add a child prim to this parent prim.
2321 /// </summary>
2322 /// <param name="prim">Child prim</param>
2323 // I'm the parent
2324 // prim is the child
2325 public void ParentPrim(OdePrim prim)
2326 {
2327 //Console.WriteLine("ParentPrim " + m_primName);
2328 if (this.m_localID != prim.m_localID)
2329 {
2330 DestroyBody(); // for now we need to rebuil entire object on link change
2331
2332 lock (childrenPrim)
2333 {
2334 // adopt the prim
2335 if (!childrenPrim.Contains(prim))
2336 childrenPrim.Add(prim);
2337
2338 // see if this prim has kids and adopt them also
2339 // should not happen for now
2340 foreach (OdePrim prm in prim.childrenPrim)
2341 {
2342 if (!childrenPrim.Contains(prm))
2343 {
2344 if (prm.Body != IntPtr.Zero)
2345 {
2346 if (prm.prim_geom != IntPtr.Zero)
2347 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2348 if (prm.Body != prim.Body)
2349 prm.DestroyBody(); // don't loose bodies around
2350 prm.Body = IntPtr.Zero;
2351 }
2352
2353 childrenPrim.Add(prm);
2354 prm._parent = this;
2355 }
2356 }
2357 }
2358 //Remove old children from the prim
2359 prim.childrenPrim.Clear();
2360
2361 if (prim.Body != IntPtr.Zero)
2362 {
2363 if (prim.prim_geom != IntPtr.Zero)
2364 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2365 prim.DestroyBody(); // don't loose bodies around
2366 prim.Body = IntPtr.Zero;
2367 }
2368
2369 prim.childPrim = true;
2370 prim._parent = this;
2371
2372 MakeBody(); // full nasty reconstruction
2373 }
2374 }
2375
2376 private void UpdateChildsfromgeom()
2377 {
2378 if (childrenPrim.Count > 0)
2379 {
2380 foreach (OdePrim prm in childrenPrim)
2381 prm.UpdateDataFromGeom();
2382 }
2383 }
2384
2385 private void UpdateDataFromGeom()
2386 {
2387 if (prim_geom != IntPtr.Zero)
2388 {
2389 d.Quaternion qtmp = new d.Quaternion { };
2390 d.GeomCopyQuaternion(prim_geom, out qtmp);
2391 _orientation.W = qtmp.W;
2392 _orientation.X = qtmp.X;
2393 _orientation.Y = qtmp.Y;
2394 _orientation.Z = qtmp.Z;
2395
2396 d.Vector3 lpos;
2397 d.GeomCopyPosition(prim_geom, out lpos);
2398 _position.X = lpos.X;
2399 _position.Y = lpos.Y;
2400 _position.Z = lpos.Z;
2401 }
2402 }
2403
2404 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2405 {
2406 // Okay, we have a delinked child.. destroy all body and remake
2407 if (odePrim != this && !childrenPrim.Contains(odePrim))
2408 return;
2409
2410 DestroyBody();
2411
2412 if (odePrim == this) // delinking the root prim
2413 {
2414 OdePrim newroot = null;
2415 lock (childrenPrim)
2416 {
2417 if (childrenPrim.Count > 0)
2418 {
2419 newroot = childrenPrim[0];
2420 childrenPrim.RemoveAt(0);
2421 foreach (OdePrim prm in childrenPrim)
2422 {
2423 newroot.childrenPrim.Add(prm);
2424 }
2425 childrenPrim.Clear();
2426 }
2427 if (newroot != null)
2428 {
2429 newroot.childPrim = false;
2430 newroot._parent = null;
2431 if (remakebodies)
2432 newroot.MakeBody();
2433 }
2434 }
2435 }
2436
2437 else
2438 {
2439 lock (childrenPrim)
2440 {
2441 childrenPrim.Remove(odePrim);
2442 odePrim.childPrim = false;
2443 odePrim._parent = null;
2444 // odePrim.UpdateDataFromGeom();
2445 if (remakebodies)
2446 odePrim.MakeBody();
2447 }
2448 }
2449 if (remakebodies)
2450 MakeBody();
2451 }
2452
2453 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2454 {
2455 // Okay, we have a delinked child.. destroy all body and remake
2456 if (odePrim != this && !childrenPrim.Contains(odePrim))
2457 return;
2458
2459 DestroyBody();
2460
2461 if (odePrim == this)
2462 {
2463 OdePrim newroot = null;
2464 lock (childrenPrim)
2465 {
2466 if (childrenPrim.Count > 0)
2467 {
2468 newroot = childrenPrim[0];
2469 childrenPrim.RemoveAt(0);
2470 foreach (OdePrim prm in childrenPrim)
2471 {
2472 newroot.childrenPrim.Add(prm);
2473 }
2474 childrenPrim.Clear();
2475 }
2476 if (newroot != null)
2477 {
2478 newroot.childPrim = false;
2479 newroot._parent = null;
2480 newroot.MakeBody();
2481 }
2482 }
2483 if (reMakeBody)
2484 MakeBody();
2485 return;
2486 }
2487 else
2488 {
2489 lock (childrenPrim)
2490 {
2491 childrenPrim.Remove(odePrim);
2492 odePrim.childPrim = false;
2493 odePrim._parent = null;
2494 if (reMakeBody)
2495 odePrim.MakeBody();
2496 }
2497 }
2498 MakeBody();
2499 }
2500
2501 #region changes
2502
2503 private void changeadd()
2504 {
2505 CreateGeom();
2506
2507 if (prim_geom != IntPtr.Zero)
2508 {
2509 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2510 d.Quaternion myrot = new d.Quaternion();
2511 myrot.X = _orientation.X;
2512 myrot.Y = _orientation.Y;
2513 myrot.Z = _orientation.Z;
2514 myrot.W = _orientation.W;
2515 d.GeomSetQuaternion(prim_geom, ref myrot);
2516
2517 if (!m_isphysical)
2518 SetInStaticSpace(this);
2519 }
2520
2521 if (m_isphysical && Body == IntPtr.Zero)
2522 {
2523 MakeBody();
2524 }
2525 }
2526
2527 private void changeAngularLock(Vector3 newLock)
2528 {
2529 // do we have a Physical object?
2530 if (Body != IntPtr.Zero)
2531 {
2532 //Check that we have a Parent
2533 //If we have a parent then we're not authorative here
2534 if (_parent == null)
2535 {
2536 if (!newLock.ApproxEquals(Vector3.One, 0f))
2537 {
2538 createAMotor(newLock);
2539 }
2540 else
2541 {
2542 if (Amotor != IntPtr.Zero)
2543 {
2544 d.JointDestroy(Amotor);
2545 Amotor = IntPtr.Zero;
2546 }
2547 }
2548 }
2549 }
2550 // Store this for later in case we get turned into a separate body
2551 m_angularlock = newLock;
2552 }
2553
2554 private void changeLink(OdePrim NewParent)
2555 {
2556 if (_parent == null && NewParent != null)
2557 {
2558 NewParent.ParentPrim(this);
2559 }
2560 else if (_parent != null)
2561 {
2562 if (_parent is OdePrim)
2563 {
2564 if (NewParent != _parent)
2565 {
2566 (_parent as OdePrim).ChildDelink(this, false); // for now...
2567 childPrim = false;
2568
2569 if (NewParent != null)
2570 {
2571 NewParent.ParentPrim(this);
2572 }
2573 }
2574 }
2575 }
2576 _parent = NewParent;
2577 }
2578
2579
2580 private void Stop()
2581 {
2582 if (!childPrim)
2583 {
2584 m_force = Vector3.Zero;
2585 m_forceacc = Vector3.Zero;
2586 m_angularForceacc = Vector3.Zero;
2587 _torque = Vector3.Zero;
2588 _velocity = Vector3.Zero;
2589 _acceleration = Vector3.Zero;
2590 m_rotationalVelocity = Vector3.Zero;
2591 _target_velocity = Vector3.Zero;
2592 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2593 m_vehicle.Stop();
2594 }
2595
2596 if (Body != IntPtr.Zero)
2597 {
2598 d.BodySetForce(Body, 0f, 0f, 0f);
2599 d.BodySetTorque(Body, 0f, 0f, 0f);
2600 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2601 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2602 }
2603 }
2604
2605
2606 private void changePhantomStatus(bool newval)
2607 {
2608 m_isphantom = newval;
2609
2610 if (m_isSelected)
2611 {
2612 m_collisionCategories = CollisionCategories.Selected;
2613 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2614 }
2615 else
2616 {
2617 if (m_isphantom && !m_isVolumeDetect)
2618 {
2619 m_collisionCategories = 0;
2620 if (m_isphysical)
2621 m_collisionFlags = CollisionCategories.Land;
2622 else
2623 m_collisionFlags = 0; // should never happen
2624 }
2625
2626 else
2627 {
2628 m_collisionCategories = CollisionCategories.Geom;
2629 if (m_isphysical)
2630 m_collisionCategories |= CollisionCategories.Body;
2631
2632 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2633
2634 if (m_collidesWater)
2635 m_collisionFlags |= CollisionCategories.Water;
2636 }
2637 }
2638
2639 if (!childPrim)
2640 {
2641 foreach (OdePrim prm in childrenPrim)
2642 {
2643 prm.m_collisionCategories = m_collisionCategories;
2644 prm.m_collisionFlags = m_collisionFlags;
2645
2646 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2647 {
2648 if (prm.m_NoColide)
2649 {
2650 d.GeomSetCategoryBits(prm.prim_geom, 0);
2651 if (m_isphysical)
2652 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2653 else
2654 d.GeomSetCollideBits(prm.prim_geom, 0);
2655 }
2656 else
2657 {
2658 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2659 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2660 }
2661 if(!m_isSelected)
2662 d.GeomEnable(prm.prim_geom);
2663 }
2664 }
2665 }
2666
2667 if (!m_disabled && prim_geom != IntPtr.Zero)
2668 {
2669 if (m_NoColide)
2670 {
2671 d.GeomSetCategoryBits(prim_geom, 0);
2672 if (m_isphysical)
2673 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2674 else
2675 d.GeomSetCollideBits(prim_geom, 0);
2676 }
2677 else
2678 {
2679 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2680 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2681 }
2682 if(!m_isSelected)
2683 d.GeomEnable(prim_geom);
2684 }
2685 }
2686
2687 private void changeSelectedStatus(bool newval)
2688 {
2689 if (m_lastdoneSelected == newval)
2690 return;
2691
2692 m_lastdoneSelected = newval;
2693 DoSelectedStatus(newval);
2694 }
2695
2696 private void CheckDelaySelect()
2697 {
2698 if (m_delaySelect)
2699 {
2700 DoSelectedStatus(m_isSelected);
2701 }
2702 }
2703
2704 private void DoSelectedStatus(bool newval)
2705 {
2706 m_isSelected = newval;
2707 Stop();
2708
2709 if (newval)
2710 {
2711 if (!childPrim && Body != IntPtr.Zero)
2712 d.BodyDisable(Body);
2713
2714 if (m_delaySelect || m_isphysical)
2715 {
2716 m_collisionCategories = CollisionCategories.Selected;
2717 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2718
2719 if (!childPrim)
2720 {
2721 foreach (OdePrim prm in childrenPrim)
2722 {
2723 prm.m_collisionCategories = m_collisionCategories;
2724 prm.m_collisionFlags = m_collisionFlags;
2725
2726 if (prm.prim_geom != null)
2727 {
2728
2729 if (prm.m_NoColide)
2730 {
2731 d.GeomSetCategoryBits(prm.prim_geom, 0);
2732 d.GeomSetCollideBits(prm.prim_geom, 0);
2733 }
2734 else
2735 {
2736 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2737 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2738 }
2739 d.GeomDisable(prm.prim_geom);
2740 }
2741 prm.m_delaySelect = false;
2742 }
2743 }
2744
2745 if (prim_geom != null)
2746 {
2747 if (m_NoColide)
2748 {
2749 d.GeomSetCategoryBits(prim_geom, 0);
2750 d.GeomSetCollideBits(prim_geom, 0);
2751 }
2752 else
2753 {
2754 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2755 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2756 }
2757 d.GeomDisable(prim_geom);
2758 }
2759
2760 m_delaySelect = false;
2761 }
2762 else if(!m_isphysical)
2763 {
2764 m_delaySelect = true;
2765 }
2766 }
2767 else
2768 {
2769 if (!childPrim && Body != IntPtr.Zero && !m_disabled)
2770 d.BodyEnable(Body);
2771
2772 if (m_isphantom && !m_isVolumeDetect)
2773 {
2774 m_collisionCategories = 0;
2775 if(m_isphysical)
2776 m_collisionFlags = CollisionCategories.Land;
2777 else
2778 m_collisionFlags = 0;
2779 }
2780 else
2781 {
2782 m_collisionCategories = CollisionCategories.Geom;
2783 if (m_isphysical)
2784 m_collisionCategories |= CollisionCategories.Body;
2785
2786 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2787
2788 if (m_collidesWater)
2789 m_collisionFlags |= CollisionCategories.Water;
2790 }
2791
2792 if (!childPrim)
2793 {
2794 foreach (OdePrim prm in childrenPrim)
2795 {
2796 prm.m_collisionCategories = m_collisionCategories;
2797 prm.m_collisionFlags = m_collisionFlags;
2798
2799 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2800 {
2801 if (prm.m_NoColide)
2802 {
2803 d.GeomSetCategoryBits(prm.prim_geom, 0);
2804 if (m_isphysical)
2805 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2806 else
2807 d.GeomSetCollideBits(prm.prim_geom, 0);
2808 }
2809 else
2810 {
2811 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2812 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2813 }
2814 d.GeomEnable(prm.prim_geom);
2815 }
2816 prm.m_delaySelect = false;
2817 prm.m_softcolide = true;
2818 }
2819 }
2820
2821 if (!m_disabled && prim_geom != IntPtr.Zero)
2822 {
2823 if (m_NoColide)
2824 {
2825 d.GeomSetCategoryBits(prim_geom, 0);
2826 if (m_isphysical)
2827 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2828 else
2829 d.GeomSetCollideBits(prim_geom, 0);
2830 }
2831 else
2832 {
2833 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2834 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2835 }
2836 d.GeomEnable(prim_geom);
2837 }
2838
2839 m_delaySelect = false;
2840 m_softcolide = true;
2841 }
2842
2843 resetCollisionAccounting();
2844 }
2845
2846 private void changePosition(Vector3 newPos)
2847 {
2848 CheckDelaySelect();
2849 if (m_isphysical)
2850 {
2851 if (childPrim) // inertia is messed, must rebuild
2852 {
2853 if (m_building)
2854 {
2855 _position = newPos;
2856 }
2857
2858 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2859 {
2860 FixInertia(newPos);
2861 if (!d.BodyIsEnabled(Body))
2862 d.BodyEnable(Body);
2863 }
2864 }
2865 else
2866 {
2867 if (_position != newPos)
2868 {
2869 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2870 _position = newPos;
2871 }
2872 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2873 d.BodyEnable(Body);
2874 }
2875 }
2876 else
2877 {
2878 if (prim_geom != IntPtr.Zero)
2879 {
2880 if (newPos != _position)
2881 {
2882 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2883 _position = newPos;
2884
2885 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2886 }
2887 }
2888 }
2889 givefakepos--;
2890 if (givefakepos < 0)
2891 givefakepos = 0;
2892 // changeSelectedStatus();
2893 m_softcolide = true;
2894 resetCollisionAccounting();
2895 }
2896
2897 private void changeOrientation(Quaternion newOri)
2898 {
2899 CheckDelaySelect();
2900 if (m_isphysical)
2901 {
2902 if (childPrim) // inertia is messed, must rebuild
2903 {
2904 if (m_building)
2905 {
2906 _orientation = newOri;
2907 }
2908 /*
2909 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2910 {
2911 FixInertia(_position, newOri);
2912 if (!d.BodyIsEnabled(Body))
2913 d.BodyEnable(Body);
2914 }
2915 */
2916 }
2917 else
2918 {
2919 if (newOri != _orientation)
2920 {
2921 d.Quaternion myrot = new d.Quaternion();
2922 myrot.X = newOri.X;
2923 myrot.Y = newOri.Y;
2924 myrot.Z = newOri.Z;
2925 myrot.W = newOri.W;
2926 d.GeomSetQuaternion(prim_geom, ref myrot);
2927 _orientation = newOri;
2928 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2929 createAMotor(m_angularlock);
2930 }
2931 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2932 d.BodyEnable(Body);
2933 }
2934 }
2935 else
2936 {
2937 if (prim_geom != IntPtr.Zero)
2938 {
2939 if (newOri != _orientation)
2940 {
2941 d.Quaternion myrot = new d.Quaternion();
2942 myrot.X = newOri.X;
2943 myrot.Y = newOri.Y;
2944 myrot.Z = newOri.Z;
2945 myrot.W = newOri.W;
2946 d.GeomSetQuaternion(prim_geom, ref myrot);
2947 _orientation = newOri;
2948 }
2949 }
2950 }
2951 givefakeori--;
2952 if (givefakeori < 0)
2953 givefakeori = 0;
2954 m_softcolide = true;
2955 resetCollisionAccounting();
2956 }
2957
2958 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2959 {
2960 CheckDelaySelect();
2961 if (m_isphysical)
2962 {
2963 if (childPrim && m_building) // inertia is messed, must rebuild
2964 {
2965 _position = newPos;
2966 _orientation = newOri;
2967 }
2968 else
2969 {
2970 if (newOri != _orientation)
2971 {
2972 d.Quaternion myrot = new d.Quaternion();
2973 myrot.X = newOri.X;
2974 myrot.Y = newOri.Y;
2975 myrot.Z = newOri.Z;
2976 myrot.W = newOri.W;
2977 d.GeomSetQuaternion(prim_geom, ref myrot);
2978 _orientation = newOri;
2979 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2980 createAMotor(m_angularlock);
2981 }
2982 if (_position != newPos)
2983 {
2984 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2985 _position = newPos;
2986 }
2987 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2988 d.BodyEnable(Body);
2989 }
2990 }
2991 else
2992 {
2993 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2994 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2995
2996 if (prim_geom != IntPtr.Zero)
2997 {
2998 if (newOri != _orientation)
2999 {
3000 d.Quaternion myrot = new d.Quaternion();
3001 myrot.X = newOri.X;
3002 myrot.Y = newOri.Y;
3003 myrot.Z = newOri.Z;
3004 myrot.W = newOri.W;
3005 d.GeomSetQuaternion(prim_geom, ref myrot);
3006 _orientation = newOri;
3007 }
3008
3009 if (newPos != _position)
3010 {
3011 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3012 _position = newPos;
3013
3014 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
3015 }
3016 }
3017 }
3018 givefakepos--;
3019 if (givefakepos < 0)
3020 givefakepos = 0;
3021 givefakeori--;
3022 if (givefakeori < 0)
3023 givefakeori = 0;
3024
3025 m_softcolide = true;
3026 resetCollisionAccounting();
3027 }
3028
3029
3030 private void changeDisable(bool disable)
3031 {
3032 if (disable)
3033 {
3034 if (!m_disabled)
3035 disableBodySoft();
3036 }
3037 else
3038 {
3039 if (m_disabled)
3040 enableBodySoft();
3041 }
3042 }
3043
3044 private void changePhysicsStatus(bool NewStatus)
3045 {
3046 CheckDelaySelect();
3047
3048 m_isphysical = NewStatus;
3049
3050 if (!childPrim)
3051 {
3052 if (NewStatus)
3053 {
3054 if (Body == IntPtr.Zero)
3055 MakeBody();
3056 }
3057 else
3058 {
3059 if (Body != IntPtr.Zero)
3060 {
3061 DestroyBody();
3062 }
3063 Stop();
3064 }
3065 }
3066
3067 resetCollisionAccounting();
3068 }
3069
3070 private void changeprimsizeshape()
3071 {
3072 CheckDelaySelect();
3073
3074 OdePrim parent = (OdePrim)_parent;
3075
3076 bool chp = childPrim;
3077
3078 if (chp)
3079 {
3080 if (parent != null)
3081 {
3082 parent.DestroyBody();
3083 }
3084 }
3085 else
3086 {
3087 DestroyBody();
3088 }
3089
3090 RemoveGeom();
3091
3092 // we don't need to do space calculation because the client sends a position update also.
3093 if (_size.X <= 0)
3094 _size.X = 0.01f;
3095 if (_size.Y <= 0)
3096 _size.Y = 0.01f;
3097 if (_size.Z <= 0)
3098 _size.Z = 0.01f;
3099 // Construction of new prim
3100
3101 CreateGeom();
3102
3103 if (prim_geom != IntPtr.Zero)
3104 {
3105 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3106 d.Quaternion myrot = new d.Quaternion();
3107 myrot.X = _orientation.X;
3108 myrot.Y = _orientation.Y;
3109 myrot.Z = _orientation.Z;
3110 myrot.W = _orientation.W;
3111 d.GeomSetQuaternion(prim_geom, ref myrot);
3112 }
3113
3114 if (chp)
3115 {
3116 if (parent != null)
3117 {
3118 parent.MakeBody();
3119 }
3120 }
3121 else
3122 MakeBody();
3123
3124 m_softcolide = true;
3125 resetCollisionAccounting();
3126 }
3127
3128 private void changeSize(Vector3 newSize)
3129 {
3130 _size = newSize;
3131 changeprimsizeshape();
3132 }
3133
3134 private void changeShape(PrimitiveBaseShape newShape)
3135 {
3136 if(newShape != null)
3137 _pbs = newShape;
3138 changeprimsizeshape();
3139 }
3140
3141 private void changeFloatOnWater(bool newval)
3142 {
3143 m_collidesWater = newval;
3144
3145 if (prim_geom != IntPtr.Zero && !m_isphantom)
3146 {
3147 if (m_collidesWater)
3148 {
3149 m_collisionFlags |= CollisionCategories.Water;
3150 }
3151 else
3152 {
3153 m_collisionFlags &= ~CollisionCategories.Water;
3154 }
3155 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
3156 }
3157 }
3158
3159 private void changeSetTorque(Vector3 newtorque)
3160 {
3161 if (!m_isSelected)
3162 {
3163 if (m_isphysical && Body != IntPtr.Zero)
3164 {
3165 if (m_disabled)
3166 enableBodySoft();
3167 else if (!d.BodyIsEnabled(Body))
3168 d.BodyEnable(Body);
3169
3170 }
3171 _torque = newtorque;
3172 }
3173 }
3174
3175 private void changeForce(Vector3 force)
3176 {
3177 m_force = force;
3178 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3179 d.BodyEnable(Body);
3180 }
3181
3182 private void changeAddForce(Vector3 force)
3183 {
3184 m_forceacc += force;
3185 if (!m_isSelected)
3186 {
3187 lock (this)
3188 {
3189 //m_log.Info("[PHYSICS]: dequeing forcelist");
3190 if (m_isphysical && Body != IntPtr.Zero)
3191 {
3192 if (m_disabled)
3193 enableBodySoft();
3194 else if (!d.BodyIsEnabled(Body))
3195 d.BodyEnable(Body);
3196 }
3197 }
3198
3199 m_collisionscore = 0;
3200 }
3201 }
3202
3203 private void changeAddAngularForce(Vector3 aforce)
3204 {
3205 m_angularForceacc += aforce;
3206 if (!m_isSelected)
3207 {
3208 lock (this)
3209 {
3210 if (m_isphysical && Body != IntPtr.Zero)
3211 {
3212 if (m_disabled)
3213 enableBodySoft();
3214 else if (!d.BodyIsEnabled(Body))
3215 d.BodyEnable(Body);
3216 }
3217 }
3218 m_collisionscore = 0;
3219 }
3220 }
3221
3222 private void changevelocity(Vector3 newVel)
3223 {
3224 if (!m_isSelected)
3225 {
3226 if (Body != IntPtr.Zero)
3227 {
3228 if (m_disabled)
3229 enableBodySoft();
3230 else if (!d.BodyIsEnabled(Body))
3231 d.BodyEnable(Body);
3232
3233 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3234 }
3235 //resetCollisionAccounting();
3236 }
3237 _velocity = newVel;
3238 }
3239
3240 private void changeVolumedetetion(bool newVolDtc)
3241 {
3242 m_isVolumeDetect = newVolDtc;
3243 }
3244
3245 protected void changeBuilding(bool newbuilding)
3246 {
3247 if ((bool)newbuilding)
3248 {
3249 m_building = true;
3250 if (!childPrim)
3251 DestroyBody();
3252 }
3253 else
3254 {
3255 m_building = false;
3256 CheckDelaySelect();
3257 if (!childPrim)
3258 MakeBody();
3259 }
3260 if (!childPrim && childrenPrim.Count > 0)
3261 {
3262 foreach (OdePrim prm in childrenPrim)
3263 prm.changeBuilding(m_building); // call directly
3264 }
3265 }
3266
3267 public void changeSetVehicle(VehicleData vdata)
3268 {
3269 if (m_vehicle == null)
3270 m_vehicle = new ODEDynamics(this);
3271 m_vehicle.DoSetVehicle(vdata);
3272 }
3273 private void changeVehicleType(int value)
3274 {
3275 if (value == (int)Vehicle.TYPE_NONE)
3276 {
3277 if (m_vehicle != null)
3278 m_vehicle = null;
3279 }
3280 else
3281 {
3282 if (m_vehicle == null)
3283 m_vehicle = new ODEDynamics(this);
3284
3285 m_vehicle.ProcessTypeChange((Vehicle)value);
3286 }
3287 }
3288
3289 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3290 {
3291 if (m_vehicle == null)
3292 return;
3293
3294 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3295 }
3296
3297 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3298 {
3299 if (m_vehicle == null)
3300 return;
3301 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3302 }
3303
3304 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3305 {
3306 if (m_vehicle == null)
3307 return;
3308 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3309 }
3310
3311 private void changeVehicleFlags(strVehicleBoolParam bp)
3312 {
3313 if (m_vehicle == null)
3314 return;
3315 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3316 }
3317
3318 #endregion
3319
3320 public void Move()
3321 {
3322 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3323 !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds)
3324 // !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3325 {
3326// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3327
3328 float timestep = _parent_scene.ODE_STEPSIZE;
3329
3330 // check outside region
3331 d.Vector3 lpos;
3332 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3333
3334 if (lpos.Z < -100 || lpos.Z > 100000f)
3335 {
3336 m_outbounds = true;
3337
3338 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3339 _acceleration.X = 0;
3340 _acceleration.Y = 0;
3341 _acceleration.Z = 0;
3342
3343 _velocity.X = 0;
3344 _velocity.Y = 0;
3345 _velocity.Z = 0;
3346 m_rotationalVelocity.X = 0;
3347 m_rotationalVelocity.Y = 0;
3348 m_rotationalVelocity.Z = 0;
3349
3350 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3351 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3352 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3353 m_lastposition = _position;
3354 m_lastorientation = _orientation;
3355
3356 base.RequestPhysicsterseUpdate();
3357
3358 m_throttleUpdates = false;
3359 throttleCounter = 0;
3360 _zeroFlag = true;
3361
3362 disableBodySoft(); // disable it and colisions
3363 base.RaiseOutOfBounds(_position);
3364 return;
3365 }
3366
3367 if (lpos.X < 0f)
3368 {
3369 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3370 m_outbounds = true;
3371 }
3372 else if(lpos.X > _parent_scene.WorldExtents.X)
3373 {
3374 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3375 m_outbounds = true;
3376 }
3377 if (lpos.Y < 0f)
3378 {
3379 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3380 m_outbounds = true;
3381 }
3382 else if(lpos.Y > _parent_scene.WorldExtents.Y)
3383 {
3384 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3385 m_outbounds = true;
3386 }
3387
3388 if(m_outbounds)
3389 {
3390 m_lastposition = _position;
3391 m_lastorientation = _orientation;
3392
3393 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3394 m_rotationalVelocity.X = dtmp.X;
3395 m_rotationalVelocity.Y = dtmp.Y;
3396 m_rotationalVelocity.Z = dtmp.Z;
3397
3398 dtmp = d.BodyGetLinearVel(Body);
3399 _velocity.X = dtmp.X;
3400 _velocity.Y = dtmp.Y;
3401 _velocity.Z = dtmp.Z;
3402
3403 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3404 d.BodySetAngularVel(Body, 0, 0, 0);
3405 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3406 disableBodySoft(); // stop collisions
3407 base.RequestPhysicsterseUpdate();
3408 return;
3409 }
3410
3411
3412 float fx = 0;
3413 float fy = 0;
3414 float fz = 0;
3415
3416 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3417 {
3418 // 'VEHICLES' are dealt with in ODEDynamics.cs
3419 m_vehicle.Step();
3420 }
3421 else
3422 {
3423 float m_mass = _mass;
3424
3425 // fz = 0f;
3426 //m_log.Info(m_collisionFlags.ToString());
3427 if (m_usePID)
3428 {
3429
3430 // If the PID Controller isn't active then we set our force
3431 // calculating base velocity to the current position
3432
3433 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3434 {
3435 //PID_G = PID_G / m_PIDTau;
3436 m_PIDTau = 1;
3437 }
3438
3439 if ((PID_G - m_PIDTau) <= 0)
3440 {
3441 PID_G = m_PIDTau + 1;
3442 }
3443
3444 d.Vector3 vel = d.BodyGetLinearVel(Body);
3445 d.Vector3 pos = d.BodyGetPosition(Body);
3446 _target_velocity =
3447 new Vector3(
3448 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3449 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3450 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3451 );
3452
3453 // if velocity is zero, use position control; otherwise, velocity control
3454
3455 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3456 {
3457 // keep track of where we stopped. No more slippin' & slidin'
3458
3459 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3460 // react to the physics scene by moving it's position.
3461 // Avatar to Avatar collisions
3462 // Prim to avatar collisions
3463
3464 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3465 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3466 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3467 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3468 d.BodySetLinearVel(Body, 0, 0, 0);
3469 d.BodyAddForce(Body, 0, 0, fz);
3470 return;
3471 }
3472 else
3473 {
3474 _zeroFlag = false;
3475
3476 // We're flying and colliding with something
3477 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3478 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3479
3480 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3481
3482 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3483 }
3484 } // end if (m_usePID)
3485
3486 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3487 else if (m_useHoverPID)
3488 {
3489 //Console.WriteLine("Hover " + Name);
3490
3491 // If we're using the PID controller, then we have no gravity
3492
3493 // no lock; for now it's only called from within Simulate()
3494
3495 // If the PID Controller isn't active then we set our force
3496 // calculating base velocity to the current position
3497
3498 if ((m_PIDTau < 1))
3499 {
3500 PID_G = PID_G / m_PIDTau;
3501 }
3502
3503 if ((PID_G - m_PIDTau) <= 0)
3504 {
3505 PID_G = m_PIDTau + 1;
3506 }
3507
3508 // Where are we, and where are we headed?
3509 d.Vector3 pos = d.BodyGetPosition(Body);
3510 d.Vector3 vel = d.BodyGetLinearVel(Body);
3511
3512 // Non-Vehicles have a limited set of Hover options.
3513 // determine what our target height really is based on HoverType
3514 switch (m_PIDHoverType)
3515 {
3516 case PIDHoverType.Ground:
3517 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3518 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3519 break;
3520 case PIDHoverType.GroundAndWater:
3521 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3522 m_waterHeight = _parent_scene.GetWaterLevel();
3523 if (m_groundHeight > m_waterHeight)
3524 {
3525 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3526 }
3527 else
3528 {
3529 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3530 }
3531 break;
3532
3533 } // end switch (m_PIDHoverType)
3534
3535
3536 _target_velocity =
3537 new Vector3(0.0f, 0.0f,
3538 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3539 );
3540
3541 // if velocity is zero, use position control; otherwise, velocity control
3542
3543 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3544 {
3545 // keep track of where we stopped. No more slippin' & slidin'
3546
3547 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3548 // react to the physics scene by moving it's position.
3549 // Avatar to Avatar collisions
3550 // Prim to avatar collisions
3551
3552 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3553 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3554 // ? d.BodyAddForce(Body, 0, 0, fz);
3555 return;
3556 }
3557 else
3558 {
3559 _zeroFlag = false;
3560
3561 // We're flying and colliding with something
3562 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3563 }
3564 }
3565 else
3566 {
3567 float b = (1.0f - m_buoyancy);
3568 fx = _parent_scene.gravityx * b;
3569 fy = _parent_scene.gravityy * b;
3570 fz = _parent_scene.gravityz * b;
3571 }
3572
3573 fx *= m_mass;
3574 fy *= m_mass;
3575 fz *= m_mass;
3576
3577 // constant force
3578 fx += m_force.X;
3579 fy += m_force.Y;
3580 fz += m_force.Z;
3581
3582 fx += m_forceacc.X;
3583 fy += m_forceacc.Y;
3584 fz += m_forceacc.Z;
3585
3586 m_forceacc = Vector3.Zero;
3587
3588 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3589 if (fx != 0 || fy != 0 || fz != 0)
3590 {
3591 d.BodyAddForce(Body, fx, fy, fz);
3592 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3593 }
3594
3595 Vector3 trq;
3596
3597 trq = _torque;
3598 trq += m_angularForceacc;
3599 m_angularForceacc = Vector3.Zero;
3600 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3601 {
3602 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3603 }
3604
3605 }
3606 }
3607 else
3608 { // is not physical, or is not a body or is selected
3609 // _zeroPosition = d.BodyGetPosition(Body);
3610 return;
3611 //Console.WriteLine("Nothing " + Name);
3612
3613 }
3614 }
3615
3616
3617 public void UpdatePositionAndVelocity(float simulatedtime)
3618 {
3619 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
3620 if (_parent == null && !m_disabled && !m_building && !m_outbounds)
3621 {
3622 if (Body != IntPtr.Zero)
3623 {
3624 Vector3 pv = Vector3.Zero;
3625 bool lastZeroFlag = _zeroFlag;
3626
3627 d.Vector3 lpos;
3628 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3629
3630
3631 d.Quaternion ori;
3632 d.GeomCopyQuaternion(prim_geom, out ori);
3633 d.Vector3 vel = d.BodyGetLinearVel(Body);
3634 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3635
3636 if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
3637 && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
3638 && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
3639 && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)
3640 && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)
3641 && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)
3642 )
3643 {
3644 _zeroFlag = true;
3645 //Console.WriteLine("ZFT 2");
3646 m_throttleUpdates = false;
3647 }
3648 else
3649 {
3650 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3651 _zeroFlag = false;
3652 m_lastUpdateSent = false;
3653 //m_throttleUpdates = false;
3654 }
3655
3656 if (_zeroFlag)
3657 {
3658 m_lastposition = _position;
3659 m_lastorientation = _orientation;
3660
3661 _velocity.X = 0.0f;
3662 _velocity.Y = 0.0f;
3663 _velocity.Z = 0.0f;
3664
3665 _acceleration.X = 0;
3666 _acceleration.Y = 0;
3667 _acceleration.Z = 0;
3668
3669 m_rotationalVelocity.X = 0;
3670 m_rotationalVelocity.Y = 0;
3671 m_rotationalVelocity.Z = 0;
3672 if (!m_lastUpdateSent)
3673 {
3674 m_throttleUpdates = false;
3675 throttleCounter = 0;
3676 m_rotationalVelocity = pv;
3677
3678 base.RequestPhysicsterseUpdate();
3679
3680 m_lastUpdateSent = true;
3681 }
3682 }
3683 else
3684 {
3685 if (lastZeroFlag != _zeroFlag)
3686 {
3687 base.RequestPhysicsterseUpdate();
3688 }
3689
3690 m_lastVelocity = _velocity;
3691
3692 _position.X = lpos.X;
3693 _position.Y = lpos.Y;
3694 _position.Z = lpos.Z;
3695
3696 _velocity.X = vel.X;
3697 _velocity.Y = vel.Y;
3698 _velocity.Z = vel.Z;
3699
3700 _orientation.X = ori.X;
3701 _orientation.Y = ori.Y;
3702 _orientation.Z = ori.Z;
3703 _orientation.W = ori.W;
3704
3705 _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);
3706
3707 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
3708 {
3709 m_rotationalVelocity = pv;
3710 }
3711 else
3712 {
3713 m_rotationalVelocity.X = rotvel.X;
3714 m_rotationalVelocity.Y = rotvel.Y;
3715 m_rotationalVelocity.Z = rotvel.Z;
3716 }
3717
3718 m_lastUpdateSent = false;
3719 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3720 {
3721 m_lastposition = _position;
3722 m_lastorientation = _orientation;
3723 base.RequestPhysicsterseUpdate();
3724 }
3725 else
3726 {
3727 throttleCounter++;
3728 }
3729 }
3730 }
3731 else if (!m_lastUpdateSent || !_zeroFlag)
3732 {
3733 // Not a body.. so Make sure the client isn't interpolating
3734 _velocity.X = 0;
3735 _velocity.Y = 0;
3736 _velocity.Z = 0;
3737
3738 _acceleration.X = 0;
3739 _acceleration.Y = 0;
3740 _acceleration.Z = 0;
3741
3742 m_rotationalVelocity.X = 0;
3743 m_rotationalVelocity.Y = 0;
3744 m_rotationalVelocity.Z = 0;
3745 _zeroFlag = true;
3746
3747 if (!m_lastUpdateSent)
3748 {
3749 m_throttleUpdates = false;
3750 throttleCounter = 0;
3751
3752 base.RequestPhysicsterseUpdate();
3753
3754 m_lastUpdateSent = true;
3755 }
3756 }
3757 }
3758 }
3759
3760 internal static bool QuaternionIsFinite(Quaternion q)
3761 {
3762 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3763 return false;
3764 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3765 return false;
3766 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3767 return false;
3768 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3769 return false;
3770 return true;
3771 }
3772
3773 internal static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3774 {
3775 dst.c.W = src.c.W;
3776 dst.c.X = src.c.X;
3777 dst.c.Y = src.c.Y;
3778 dst.c.Z = src.c.Z;
3779 dst.mass = src.mass;
3780 dst.I.M00 = src.I.M00;
3781 dst.I.M01 = src.I.M01;
3782 dst.I.M02 = src.I.M02;
3783 dst.I.M10 = src.I.M10;
3784 dst.I.M11 = src.I.M11;
3785 dst.I.M12 = src.I.M12;
3786 dst.I.M20 = src.I.M20;
3787 dst.I.M21 = src.I.M21;
3788 dst.I.M22 = src.I.M22;
3789 }
3790
3791 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
3792 {
3793 // assumes object center of mass is zero
3794 float smass = part.mass;
3795 theobj.mass -= smass;
3796
3797 smass *= 1.0f / (theobj.mass); ;
3798
3799 theobj.c.X -= part.c.X * smass;
3800 theobj.c.Y -= part.c.Y * smass;
3801 theobj.c.Z -= part.c.Z * smass;
3802
3803 theobj.I.M00 -= part.I.M00;
3804 theobj.I.M01 -= part.I.M01;
3805 theobj.I.M02 -= part.I.M02;
3806 theobj.I.M10 -= part.I.M10;
3807 theobj.I.M11 -= part.I.M11;
3808 theobj.I.M12 -= part.I.M12;
3809 theobj.I.M20 -= part.I.M20;
3810 theobj.I.M21 -= part.I.M21;
3811 theobj.I.M22 -= part.I.M22;
3812 }
3813
3814 private static void DMassDup(ref d.Mass src, out d.Mass dst)
3815 {
3816 dst = new d.Mass { };
3817
3818 dst.c.W = src.c.W;
3819 dst.c.X = src.c.X;
3820 dst.c.Y = src.c.Y;
3821 dst.c.Z = src.c.Z;
3822 dst.mass = src.mass;
3823 dst.I.M00 = src.I.M00;
3824 dst.I.M01 = src.I.M01;
3825 dst.I.M02 = src.I.M02;
3826 dst.I.M10 = src.I.M10;
3827 dst.I.M11 = src.I.M11;
3828 dst.I.M12 = src.I.M12;
3829 dst.I.M20 = src.I.M20;
3830 dst.I.M21 = src.I.M21;
3831 dst.I.M22 = src.I.M22;
3832 }
3833 private void donullchange()
3834 {
3835 }
3836
3837 public bool DoAChange(changes what, object arg)
3838 {
3839 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3840 {
3841 return false;
3842 }
3843
3844 // nasty switch
3845 switch (what)
3846 {
3847 case changes.Add:
3848 changeadd();
3849 break;
3850 case changes.Remove:
3851 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3852 //When we return true, it destroys all of the prims in the linkset anyway
3853 if (_parent != null)
3854 {
3855 OdePrim parent = (OdePrim)_parent;
3856 parent.ChildRemove(this, false);
3857 }
3858 else
3859 ChildRemove(this, false);
3860
3861 m_vehicle = null;
3862 RemoveGeom();
3863 m_targetSpace = IntPtr.Zero;
3864 if (m_eventsubscription > 0)
3865 UnSubscribeEvents();
3866 return true;
3867
3868 case changes.Link:
3869 OdePrim tmp = (OdePrim)arg;
3870 changeLink(tmp);
3871 break;
3872
3873 case changes.DeLink:
3874 changeLink(null);
3875 break;
3876
3877 case changes.Position:
3878 changePosition((Vector3)arg);
3879 break;
3880
3881 case changes.Orientation:
3882 changeOrientation((Quaternion)arg);
3883 break;
3884
3885 case changes.PosOffset:
3886 donullchange();
3887 break;
3888
3889 case changes.OriOffset:
3890 donullchange();
3891 break;
3892
3893 case changes.Velocity:
3894 changevelocity((Vector3)arg);
3895 break;
3896
3897 // case changes.Acceleration:
3898 // changeacceleration((Vector3)arg);
3899 // break;
3900 // case changes.AngVelocity:
3901 // changeangvelocity((Vector3)arg);
3902 // break;
3903
3904 case changes.Force:
3905 changeForce((Vector3)arg);
3906 break;
3907
3908 case changes.Torque:
3909 changeSetTorque((Vector3)arg);
3910 break;
3911
3912 case changes.AddForce:
3913 changeAddForce((Vector3)arg);
3914 break;
3915
3916 case changes.AddAngForce:
3917 changeAddAngularForce((Vector3)arg);
3918 break;
3919
3920 case changes.AngLock:
3921 changeAngularLock((Vector3)arg);
3922 break;
3923
3924 case changes.Size:
3925 changeSize((Vector3)arg);
3926 break;
3927
3928 case changes.Shape:
3929 changeShape((PrimitiveBaseShape)arg);
3930 break;
3931
3932 case changes.CollidesWater:
3933 changeFloatOnWater((bool)arg);
3934 break;
3935
3936 case changes.VolumeDtc:
3937 changeVolumedetetion((bool)arg);
3938 break;
3939
3940 case changes.Phantom:
3941 changePhantomStatus((bool)arg);
3942 break;
3943
3944 case changes.Physical:
3945 changePhysicsStatus((bool)arg);
3946 break;
3947
3948 case changes.Selected:
3949 changeSelectedStatus((bool)arg);
3950 break;
3951
3952 case changes.disabled:
3953 changeDisable((bool)arg);
3954 break;
3955
3956 case changes.building:
3957 changeBuilding((bool)arg);
3958 break;
3959
3960 case changes.VehicleType:
3961 changeVehicleType((int)arg);
3962 break;
3963
3964 case changes.VehicleFlags:
3965 changeVehicleFlags((strVehicleBoolParam) arg);
3966 break;
3967
3968 case changes.VehicleFloatParam:
3969 changeVehicleFloatParam((strVehicleFloatParam) arg);
3970 break;
3971
3972 case changes.VehicleVectorParam:
3973 changeVehicleVectorParam((strVehicleVectorParam) arg);
3974 break;
3975
3976 case changes.VehicleRotationParam:
3977 changeVehicleRotationParam((strVehicleQuatParam) arg);
3978 break;
3979
3980 case changes.SetVehicle:
3981 changeSetVehicle((VehicleData) arg);
3982 break;
3983 case changes.Null:
3984 donullchange();
3985 break;
3986
3987 default:
3988 donullchange();
3989 break;
3990 }
3991 return false;
3992 }
3993
3994 public void AddChange(changes what, object arg)
3995 {
3996 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3997 }
3998
3999
4000 private struct strVehicleBoolParam
4001 {
4002 public int param;
4003 public bool value;
4004 }
4005
4006 private struct strVehicleFloatParam
4007 {
4008 public int param;
4009 public float value;
4010 }
4011
4012 private struct strVehicleQuatParam
4013 {
4014 public int param;
4015 public Quaternion value;
4016 }
4017
4018 private struct strVehicleVectorParam
4019 {
4020 public int param;
4021 public Vector3 value;
4022 }
4023 }
4024} \ 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..4b3f83b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,443 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenMetaverse;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37
38namespace OpenSim.Region.Physics.OdePlugin
39{
40 /// <summary>
41 /// Processes raycast requests as ODE is in a state to be able to do them.
42 /// This ensures that it's thread safe and there will be no conflicts.
43 /// Requests get returned by a different thread then they were requested by.
44 /// </summary>
45 public class ODERayCastRequestManager
46 {
47 /// <summary>
48 /// Pending ray requests
49 /// </summary>
50 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
51
52 /// <summary>
53 /// Scene that created this object.
54 /// </summary>
55 private OdeScene m_scene;
56
57 IntPtr ray;
58
59 private const int ColisionContactGeomsPerTest = 5;
60
61 /// <summary>
62 /// ODE near callback delegate
63 /// </summary>
64 private d.NearCallback nearCallback;
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66 private List<ContactResult> m_contactResults = new List<ContactResult>();
67
68 public ODERayCastRequestManager(OdeScene pScene)
69 {
70 m_scene = pScene;
71 nearCallback = near;
72 ray = d.CreateRay(IntPtr.Zero, 1.0f);
73 }
74
75 /// <summary>
76 /// Queues a raycast
77 /// </summary>
78 /// <param name="position">Origin of Ray</param>
79 /// <param name="direction">Ray normal</param>
80 /// <param name="length">Ray length</param>
81 /// <param name="retMethod">Return method to send the results</param>
82 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
83 {
84 ODERayRequest req = new ODERayRequest();
85 req.geom = IntPtr.Zero;
86 req.callbackMethod = retMethod;
87 req.Count = 0;
88 req.length = length;
89 req.Normal = direction;
90 req.Origin = position;
91
92 m_PendingRequests.Enqueue(req);
93 }
94
95 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
96 {
97 ODERayRequest req = new ODERayRequest();
98 req.geom = geom;
99 req.callbackMethod = retMethod;
100 req.length = length;
101 req.Normal = direction;
102 req.Origin = position;
103 req.Count = 0;
104
105 m_PendingRequests.Enqueue(req);
106 }
107
108 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
109 {
110 ODERayRequest req = new ODERayRequest();
111 req.geom = IntPtr.Zero;
112 req.callbackMethod = retMethod;
113 req.Count = 0;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117
118 m_PendingRequests.Enqueue(req);
119 }
120
121 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
122 {
123 ODERayRequest req = new ODERayRequest();
124 req.geom = geom;
125 req.callbackMethod = retMethod;
126 req.length = length;
127 req.Normal = direction;
128 req.Origin = position;
129 req.Count = 0;
130
131 m_PendingRequests.Enqueue(req);
132 }
133
134 /// <summary>
135 /// Queues a raycast
136 /// </summary>
137 /// <param name="position">Origin of Ray</param>
138 /// <param name="direction">Ray normal</param>
139 /// <param name="length">Ray length</param>
140 /// <param name="count"></param>
141 /// <param name="retMethod">Return method to send the results</param>
142 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
143 {
144 ODERayRequest req = new ODERayRequest();
145 req.geom = IntPtr.Zero;
146 req.callbackMethod = retMethod;
147 req.length = length;
148 req.Normal = direction;
149 req.Origin = position;
150 req.Count = count;
151
152 m_PendingRequests.Enqueue(req);
153 }
154
155 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
156 {
157 ODERayRequest req = new ODERayRequest();
158 req.geom = geom;
159 req.callbackMethod = retMethod;
160 req.length = length;
161 req.Normal = direction;
162 req.Origin = position;
163 req.Count = count;
164
165 m_PendingRequests.Enqueue(req);
166 }
167
168 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
169 {
170 ODERayRequest req = new ODERayRequest();
171 req.geom = IntPtr.Zero;
172 req.callbackMethod = retMethod;
173 req.length = length;
174 req.Normal = direction;
175 req.Origin = position;
176 req.Count = count;
177
178 m_PendingRequests.Enqueue(req);
179 }
180
181 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
182 {
183 ODERayRequest req = new ODERayRequest();
184 req.geom = geom;
185 req.callbackMethod = retMethod;
186 req.length = length;
187 req.Normal = direction;
188 req.Origin = position;
189 req.Count = count;
190
191 m_PendingRequests.Enqueue(req);
192 }
193
194 /// <summary>
195 /// Process all queued raycast requests
196 /// </summary>
197 /// <returns>Time in MS the raycasts took to process.</returns>
198 public int ProcessQueuedRequests()
199 {
200 int time = System.Environment.TickCount;
201
202 if (m_PendingRequests.Count <= 0)
203 return 0;
204
205 if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still
206 {
207 m_PendingRequests.Clear();
208 return 0;
209 }
210
211 ODERayRequest req;
212
213 int i = 50; // arbitary limit of processed tests per frame
214
215 while(m_PendingRequests.Dequeue(out req))
216 {
217 if (req.geom == IntPtr.Zero)
218 doSpaceRay(req);
219 else
220 doGeomRay(req);
221 if(--i < 0)
222 break;
223 }
224
225 lock (m_contactResults)
226 m_contactResults.Clear();
227
228 return System.Environment.TickCount - time;
229 }
230 /// <summary>
231 /// Method that actually initiates the raycast with full top space
232 /// </summary>
233 /// <param name="req"></param>
234 private void doSpaceRay(ODERayRequest req)
235 {
236 // Create the ray
237// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
238 d.GeomRaySetLength(ray, req.length);
239 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
240
241 // Collide test
242 d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback);
243
244 // Remove Ray
245// d.GeomDestroy(ray);
246
247 if (req.callbackMethod == null)
248 return;
249
250 if (req.callbackMethod is RaycastCallback)
251 {
252 // Define default results
253 bool hitYN = false;
254 uint hitConsumerID = 0;
255 float distance = 999999999999f;
256 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
257 Vector3 snormal = Vector3.Zero;
258
259 // Find closest contact and object.
260 lock (m_contactResults)
261 {
262 foreach (ContactResult cResult in m_contactResults)
263 {
264 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
265 {
266 closestcontact = cResult.Pos;
267 hitConsumerID = cResult.ConsumerID;
268 distance = cResult.Depth;
269 hitYN = true;
270 snormal = cResult.Normal;
271 }
272 }
273 m_contactResults.Clear();
274 }
275
276 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
277 }
278 else
279 {
280 ((RayCallback)req.callbackMethod)(m_contactResults);
281 lock (m_PendingRequests)
282 m_contactResults.Clear();
283 }
284 }
285
286 /// <summary>
287 /// Method that actually initiates the raycast with a geom
288 /// </summary>
289 /// <param name="req"></param>
290 private void doGeomRay(ODERayRequest req)
291 {
292 // Create the ray
293// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
294 d.GeomRaySetLength(ray, req.length);
295 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
296
297 // Collide test
298 d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
299
300 // Remove Ray
301// d.GeomDestroy(ray);
302
303 if (req.callbackMethod == null)
304 return;
305
306 if (req.callbackMethod is RaycastCallback)
307 {
308 // Define default results
309 bool hitYN = false;
310 uint hitConsumerID = 0;
311 float distance = 999999999999f;
312 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
313 Vector3 snormal = Vector3.Zero;
314
315 // Find closest contact and object.
316 lock (m_contactResults)
317 {
318 foreach (ContactResult cResult in m_contactResults)
319 {
320 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
321 {
322 closestcontact = cResult.Pos;
323 hitConsumerID = cResult.ConsumerID;
324 distance = cResult.Depth;
325 hitYN = true;
326 snormal = cResult.Normal;
327 }
328 }
329 m_contactResults.Clear();
330 }
331
332 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
333 }
334 else
335 {
336 ((RayCallback)req.callbackMethod)(m_contactResults);
337 lock (m_PendingRequests)
338 m_contactResults.Clear();
339 }
340 }
341
342 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
343 {
344 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
345 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
346 return false;
347
348 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
349 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
350 return true;
351 }
352
353 // This is the standard Near. g2 is the ray
354 private void near(IntPtr space, IntPtr g1, IntPtr g2)
355 {
356 //Don't test against heightfield Geom, or you'll be sorry!
357 // Exclude heightfield geom
358
359 if (g1 == IntPtr.Zero || g1 == g2)
360 return;
361
362 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
363 return;
364
365 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
366 if (d.GeomIsSpace(g1))
367 {
368 try
369 {
370 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
371 }
372 catch (Exception e)
373 {
374 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
375 }
376 return;
377 }
378
379 int count = 0;
380 try
381 {
382 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
383 }
384 catch (SEHException)
385 {
386 m_log.Error("[PHYSICS Ray]: 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.");
387 }
388 catch (Exception e)
389 {
390 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
391 return;
392 }
393
394 if (count == 0)
395 return;
396
397 PhysicsActor p1 = null;
398
399 if (g1 != IntPtr.Zero)
400 m_scene.actor_name_map.TryGetValue(g1, out p1);
401
402 d.ContactGeom curcontact = new d.ContactGeom();
403 // Loop over contacts, build results.
404 for (int i = 0; i < count; i++)
405 {
406 if (!GetCurContactGeom(i, ref curcontact))
407 break;
408 if (p1 != null) {
409 if (p1 is OdePrim)
410 {
411 ContactResult collisionresult = new ContactResult();
412
413 collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
414 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
415 collisionresult.Depth = curcontact.depth;
416 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
417 curcontact.normal.Z);
418 lock (m_contactResults)
419 m_contactResults.Add(collisionresult);
420 }
421 }
422 }
423 }
424
425 /// <summary>
426 /// Dereference the creator scene so that it can be garbage collected if needed.
427 /// </summary>
428 internal void Dispose()
429 {
430 m_scene = null;
431 }
432 }
433
434 public struct ODERayRequest
435 {
436 public IntPtr geom;
437 public Vector3 Origin;
438 public Vector3 Normal;
439 public int Count;
440 public float length;
441 public object callbackMethod;
442 }
443} \ 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..f5129cb
--- /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 int 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 int 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, int bits);
1090
1091 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1092 public static extern void GeomSetCollideBits(IntPtr geom, int 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..3e0ccef
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2575 @@
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 [Flags]
64 public enum CollisionCategories : int
65 {
66 Disabled = 0,
67 Geom = 0x00000001,
68 Body = 0x00000002,
69 Space = 0x00000004,
70 Character = 0x00000008,
71 Land = 0x00000010,
72 Water = 0x00000020,
73 Wind = 0x00000040,
74 Sensor = 0x00000080,
75 Selected = 0x00000100
76 }
77
78 /// <summary>
79 /// Material type for a primitive
80 /// </summary>
81 public enum Material : int
82 {
83 /// <summary></summary>
84 Stone = 0,
85 /// <summary></summary>
86 Metal = 1,
87 /// <summary></summary>
88 Glass = 2,
89 /// <summary></summary>
90 Wood = 3,
91 /// <summary></summary>
92 Flesh = 4,
93 /// <summary></summary>
94 Plastic = 5,
95 /// <summary></summary>
96 Rubber = 6,
97
98 light = 7 // compatibility with old viewers
99 }
100
101 public enum changes : int
102 {
103 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
104 Remove,
105 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
106 // or removes from a object if arg is null
107 DeLink,
108 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
109 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
110 PosOffset, // not in use
111 // arg Vector3 new position in local coords. Changes prim position in object
112 OriOffset, // not in use
113 // arg Vector3 new position in local coords. Changes prim position in object
114 Velocity,
115 AngVelocity,
116 Acceleration,
117 Force,
118 Torque,
119
120 AddForce,
121 AddAngForce,
122 AngLock,
123
124 Size,
125 Shape,
126
127 CollidesWater,
128 VolumeDtc,
129
130 Physical,
131 Phantom,
132 Selected,
133 disabled,
134 building,
135
136 VehicleType,
137 VehicleFloatParam,
138 VehicleVectorParam,
139 VehicleRotationParam,
140 VehicleFlags,
141 SetVehicle,
142
143 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
144 }
145
146 public struct ODEchangeitem
147 {
148 public PhysicsActor actor;
149 public OdeCharacter character;
150 public changes what;
151 public Object arg;
152 }
153
154 public class OdeScene : PhysicsScene
155 {
156 private readonly ILog m_log;
157 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
158
159 public bool OdeUbitLib = false;
160// private int threadid = 0;
161 private Random fluidRandomizer = new Random(Environment.TickCount);
162
163 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
164 const float MaxERP = 0.8f;
165 const float minERP = 0.1f;
166 const float comumContactCFM = 0.0001f;
167
168 float frictionMovementMult = 0.3f;
169
170 float TerrainBounce = 0.1f;
171 float TerrainFriction = 0.3f;
172
173 public float AvatarFriction = 0;// 0.9f * 0.5f;
174
175 private const uint m_regionWidth = Constants.RegionSize;
176 private const uint m_regionHeight = Constants.RegionSize;
177
178 public float ODE_STEPSIZE = 0.020f;
179 private float metersInSpace = 25.6f;
180 private float m_timeDilation = 1.0f;
181
182 public float gravityx = 0f;
183 public float gravityy = 0f;
184 public float gravityz = -9.8f;
185
186 private float waterlevel = 0f;
187 private int framecount = 0;
188
189 internal IntPtr WaterGeom;
190
191 public float avPIDD = 2200f; // make it visible
192 public float avPIDP = 900f; // make it visible
193 private float avCapRadius = 0.37f;
194 private float avDensity = 3f;
195 private float avMovementDivisorWalk = 1.3f;
196 private float avMovementDivisorRun = 0.8f;
197 private float minimumGroundFlightOffset = 3f;
198 public float maximumMassObject = 10000.01f;
199
200 public bool meshSculptedPrim = true;
201 public bool forceSimplePrimMeshing = false;
202
203 public float meshSculptLOD = 32;
204 public float MeshSculptphysicalLOD = 32;
205
206 public float geomDefaultDensity = 10.000006836f;
207
208 public int geomContactPointsStartthrottle = 3;
209 public int geomUpdatesPerThrottledUpdate = 15;
210
211 public float bodyPIDD = 35f;
212 public float bodyPIDG = 25;
213
214// public int geomCrossingFailuresBeforeOutofbounds = 6;
215
216 public int bodyFramesAutoDisable = 20;
217
218 private float[] _watermap;
219
220 private d.NearCallback nearCallback;
221
222 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
223 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
224 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
225 private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
226
227 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
228
229 /// <summary>
230 /// A list of actors that should receive collision events.
231 /// </summary>
232 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
233
234 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
235 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
236 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
237
238 private float contactsurfacelayer = 0.002f;
239
240 private int contactsPerCollision = 80;
241 internal IntPtr ContactgeomsArray = IntPtr.Zero;
242 private IntPtr GlobalContactsArray = IntPtr.Zero;
243
244 const int maxContactsbeforedeath = 4000;
245 private volatile int m_global_contactcount = 0;
246
247
248 private readonly IntPtr contactgroup;
249
250 public ContactData[] m_materialContactsData = new ContactData[8];
251
252 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
253 private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
254 private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
255
256 private int m_physicsiterations = 10;
257 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
258 private readonly PhysicsActor PANull = new NullPhysicsActor();
259 private float step_time = 0.0f;
260
261 public IntPtr world;
262
263 private uint obj2LocalID = 0;
264 private OdeCharacter cc1;
265 private OdePrim cp1;
266 private OdeCharacter cc2;
267 private OdePrim cp2;
268
269 // split the spaces acording to contents type
270 // ActiveSpace contains characters and active prims
271 // StaticSpace contains land and other that is mostly static in enviroment
272 // this can contain subspaces, like the grid in staticspace
273 // as now space only contains this 2 top spaces
274
275 public IntPtr TopSpace; // the global space
276 public IntPtr ActiveSpace; // space for active prims
277 public IntPtr StaticSpace; // space for the static things around
278
279 // some speedup variables
280 private int spaceGridMaxX;
281 private int spaceGridMaxY;
282 private float spacesPerMeter;
283
284 // split static geometry collision into a grid as before
285 private IntPtr[,] staticPrimspace;
286
287 private Object OdeLock;
288 private static Object SimulationLock;
289
290 public IMesher mesher;
291
292 private IConfigSource m_config;
293
294 public bool physics_logging = false;
295 public int physics_logging_interval = 0;
296 public bool physics_logging_append_existing_logfile = false;
297
298 private Vector3 m_worldOffset = Vector3.Zero;
299 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
300 private PhysicsScene m_parentScene = null;
301
302 private ODERayCastRequestManager m_rayCastManager;
303
304
305/* maybe needed if ode uses tls
306 private void checkThread()
307 {
308
309 int th = Thread.CurrentThread.ManagedThreadId;
310 if(th != threadid)
311 {
312 threadid = th;
313 d.AllocateODEDataForThread(~0U);
314 }
315 }
316 */
317 /// <summary>
318 /// Initiailizes the scene
319 /// Sets many properties that ODE requires to be stable
320 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
321 /// </summary>
322 public OdeScene(string sceneIdentifier)
323 {
324 m_log
325 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
326
327// checkThread();
328 Name = sceneIdentifier;
329
330 OdeLock = new Object();
331 SimulationLock = new Object();
332
333 nearCallback = near;
334
335 m_rayCastManager = new ODERayCastRequestManager(this);
336 lock (OdeLock)
337 {
338 // Create the world and the first space
339 try
340 {
341 world = d.WorldCreate();
342 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
343
344 // now the major subspaces
345 ActiveSpace = d.HashSpaceCreate(TopSpace);
346 StaticSpace = d.HashSpaceCreate(TopSpace);
347 }
348 catch
349 {
350 // i must RtC#FM
351 }
352
353 d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does
354 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
355 d.HashSpaceSetLevels(StaticSpace, -2, 8);
356
357 // demote to second level
358 d.SpaceSetSublevel(ActiveSpace, 1);
359 d.SpaceSetSublevel(StaticSpace, 1);
360
361 contactgroup = d.JointGroupCreate(0);
362 //contactgroup
363
364 d.WorldSetAutoDisableFlag(world, false);
365 }
366
367 _watermap = new float[258 * 258];
368 }
369
370 // Initialize the mesh plugin
371// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
372 public override void Initialise(IMesher meshmerizer, IConfigSource config)
373 {
374// checkThread();
375 mesher = meshmerizer;
376 m_config = config;
377
378 string ode_config = d.GetConfiguration("ODE");
379 m_log.WarnFormat("ODE configuration: {0}", ode_config);
380
381 if (ode_config.Contains("ODE_Ubit"))
382 {
383 OdeUbitLib = true;
384 }
385
386 /*
387 if (region != null)
388 {
389 WorldExtents.X = region.RegionSizeX;
390 WorldExtents.Y = region.RegionSizeY;
391 }
392 */
393
394 // Defaults
395
396 int contactsPerCollision = 80;
397
398 if (m_config != null)
399 {
400 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
401 if (physicsconfig != null)
402 {
403 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
404 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
405 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
406
407 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
408
409 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
410
411 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
412 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
413
414 avDensity = physicsconfig.GetFloat("av_density", avDensity);
415 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
416 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
417 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
418
419 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
420
421 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
422 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
423// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
424
425 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
426 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
427
428 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
429 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
430
431 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
432 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
433 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
434 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
435
436 if (Environment.OSVersion.Platform == PlatformID.Unix)
437 {
438 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
439 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
440 }
441 else
442 {
443 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
444 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
445 }
446
447 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
448 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
449 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
450
451 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
452 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
453 }
454 }
455
456 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
457 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
458
459 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
460 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
461
462 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
463 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
464
465 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
466 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
467
468 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
469 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
470
471 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
472 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
473
474 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
475 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
476
477 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
478 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
479
480 m_materialContactsData[(int)Material.light].mu = 0.0f;
481 m_materialContactsData[(int)Material.light].bounce = 0.0f;
482
483 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
484
485 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
486 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
487
488 d.WorldSetLinearDamping(world, 0.001f);
489 d.WorldSetAngularDamping(world, 0.001f);
490 d.WorldSetAngularDampingThreshold(world, 0f);
491 d.WorldSetLinearDampingThreshold(world, 0f);
492 d.WorldSetMaxAngularSpeed(world, 256f);
493
494 d.WorldSetCFM(world,1e-6f); // a bit harder than default
495 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
496 d.WorldSetERP(world, 0.6f); // higher than original
497
498 // Set how many steps we go without running collision testing
499 // This is in addition to the step size.
500 // Essentially Steps * m_physicsiterations
501 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
502 d.WorldSetContactMaxCorrectingVel(world, 100.0f);
503
504 spacesPerMeter = 1 / metersInSpace;
505 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
506 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
507
508 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
509
510 // create all spaces now
511 int i, j;
512 IntPtr newspace;
513 for (i = 0; i < spaceGridMaxX; i++)
514 for (j = 0; j < spaceGridMaxY; j++)
515 {
516 newspace = d.HashSpaceCreate(StaticSpace);
517 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
518 waitForSpaceUnlock(newspace);
519 d.SpaceSetSublevel(newspace, 2);
520 d.HashSpaceSetLevels(newspace, -2, 8);
521 staticPrimspace[i, j] = newspace;
522 }
523 // let this now be real maximum values
524 spaceGridMaxX--;
525 spaceGridMaxY--;
526 }
527
528 internal void waitForSpaceUnlock(IntPtr space)
529 {
530 //if (space != IntPtr.Zero)
531 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
532 }
533
534 #region Collision Detection
535
536 // sets a global contact for a joint for contactgeom , and base contact description)
537
538 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
539 {
540 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
541 return IntPtr.Zero;
542
543 float erp = contactGeom.depth;
544 erp *= erpscale;
545 if (erp < minERP)
546 erp = minERP;
547 else if (erp > MaxERP)
548 erp = MaxERP;
549
550 float depth = contactGeom.depth * dscale;
551 if (depth > 0.5f)
552 depth = 0.5f;
553
554 d.Contact newcontact = new d.Contact();
555 newcontact.geom.depth = depth;
556 newcontact.geom.g1 = contactGeom.g1;
557 newcontact.geom.g2 = contactGeom.g2;
558 newcontact.geom.pos = contactGeom.pos;
559 newcontact.geom.normal = contactGeom.normal;
560 newcontact.geom.side1 = contactGeom.side1;
561 newcontact.geom.side2 = contactGeom.side2;
562
563 // this needs bounce also
564 newcontact.surface.mode = comumContactFlags;
565 newcontact.surface.mu = mu;
566 newcontact.surface.bounce = bounce;
567 newcontact.surface.soft_cfm = cfm;
568 newcontact.surface.soft_erp = erp;
569
570 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
571 Marshal.StructureToPtr(newcontact, contact, true);
572 return d.JointCreateContactPtr(world, contactgroup, contact);
573 }
574
575
576
577 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
578 {
579 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
580 return false;
581
582 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
583 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
584 return true;
585 }
586
587 /// <summary>
588 /// This is our near callback. A geometry is near a body
589 /// </summary>
590 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
591 /// <param name="g1">a geometry or space</param>
592 /// <param name="g2">another geometry or space</param>
593 ///
594
595 private void near(IntPtr space, IntPtr g1, IntPtr g2)
596 {
597 // no lock here! It's invoked from within Simulate(), which is thread-locked
598
599 if (m_global_contactcount >= maxContactsbeforedeath)
600 return;
601
602 // Test if we're colliding a geom with a space.
603 // If so we have to drill down into the space recursively
604
605 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
606 return;
607
608 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
609 {
610 // We'll be calling near recursivly if one
611 // of them is a space to find all of the
612 // contact points in the space
613 try
614 {
615 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
616 }
617 catch (AccessViolationException)
618 {
619 m_log.Warn("[PHYSICS]: Unable to collide test a space");
620 return;
621 }
622 //here one should check collisions of geoms inside a space
623 // but on each space we only should have geoms that not colide amoung each other
624 // so we don't dig inside spaces
625 return;
626 }
627
628 // get geom bodies to check if we already a joint contact
629 // guess this shouldn't happen now
630 IntPtr b1 = d.GeomGetBody(g1);
631 IntPtr b2 = d.GeomGetBody(g2);
632
633 // d.GeomClassID id = d.GeomGetClass(g1);
634
635 // Figure out how many contact points we have
636 int count = 0;
637 try
638 {
639 // Colliding Geom To Geom
640 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
641
642 if (g1 == g2)
643 return; // Can't collide with yourself
644
645 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
646 return;
647
648 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
649 }
650 catch (SEHException)
651 {
652 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.");
653// ode.drelease(world);
654 base.TriggerPhysicsBasedRestart();
655 }
656 catch (Exception e)
657 {
658 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
659 return;
660 }
661
662 // id contacts done
663 if (count == 0)
664 return;
665
666 // try get physical actors
667 PhysicsActor p1;
668 PhysicsActor p2;
669
670 if (!actor_name_map.TryGetValue(g1, out p1))
671 {
672 p1 = PANull;
673 }
674
675 if (!actor_name_map.TryGetValue(g2, out p2))
676 {
677 p2 = PANull;
678 }
679
680 // update actors collision score
681 if (p1.CollisionScore >= float.MaxValue - count)
682 p1.CollisionScore = 0;
683 p1.CollisionScore += count;
684
685 if (p2.CollisionScore >= float.MaxValue - count)
686 p2.CollisionScore = 0;
687 p2.CollisionScore += count;
688
689
690 // get first contact
691 d.ContactGeom curContact = new d.ContactGeom();
692 if (!GetCurContactGeom(0, ref curContact))
693 return;
694 // for now it's the one with max depth
695 ContactPoint maxDepthContact = new ContactPoint(
696 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
697 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
698 curContact.depth
699 );
700 // do volume detection case
701 if (
702 (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) ||
703 (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
704 {
705 collision_accounting_events(p1, p2, maxDepthContact);
706 return;
707 }
708
709 // big messy collision analises
710 float mu = 0;
711 float bounce = 0;
712 float cfm = 0.0001f;
713 float erp = 0.1f;
714 float erpscale = 1.0f;
715 float dscale = 1.0f;
716 bool IgnoreNegSides = false;
717
718
719 ContactData contactdata1 = new ContactData(0, 0, false);
720 ContactData contactdata2 = new ContactData(0, 0, false);
721
722 String name = null;
723 bool dop1foot = false;
724 bool dop2foot = false;
725 bool ignore = false;
726
727 switch (p1.PhysicsActorType)
728 {
729 case (int)ActorTypes.Agent:
730 {
731 bounce = 0;
732 mu = 0;
733 cfm = 0.0001f;
734
735 switch (p2.PhysicsActorType)
736 {
737 case (int)ActorTypes.Agent:
738/*
739 p1.getContactData(ref contactdata1);
740 p2.getContactData(ref contactdata2);
741
742 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
743
744 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
745 mu *= frictionMovementMult;
746*/
747 p1.CollidingObj = true;
748 p2.CollidingObj = true;
749 break;
750 case (int)ActorTypes.Prim:
751/*
752 p1.getContactData(ref contactdata1);
753 p2.getContactData(ref contactdata2);
754
755
756 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
757
758 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
759 mu *= frictionMovementMult;
760 */
761 if (p2.Velocity.LengthSquared() > 0.0f)
762 p2.CollidingObj = true;
763
764 dop1foot = true;
765 break;
766 default:
767 ignore = true; // avatar to terrain and water ignored
768 break;
769 }
770 break;
771 }
772
773 case (int)ActorTypes.Prim:
774 switch (p2.PhysicsActorType)
775 {
776 case (int)ActorTypes.Agent:
777// p1.getContactData(ref contactdata1);
778// p2.getContactData(ref contactdata2);
779
780 bounce = 0;
781 mu = 0;
782 cfm = 0.0001f;
783/*
784 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
785
786 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
787 mu *= frictionMovementMult;
788*/
789 dop2foot = true;
790 if (p1.Velocity.LengthSquared() > 0.0f)
791 p1.CollidingObj = true;
792 break;
793 case (int)ActorTypes.Prim:
794 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
795 {
796 p1.CollidingObj = true;
797 p2.CollidingObj = true;
798 }
799 p1.getContactData(ref contactdata1);
800 p2.getContactData(ref contactdata2);
801 bounce = contactdata1.bounce * contactdata2.bounce;
802 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
803
804 cfm = p1.Mass;
805 if (cfm > p2.Mass)
806 cfm = p2.Mass;
807 dscale = 10 / cfm;
808 dscale = (float)Math.Sqrt(dscale);
809 if (dscale > 1.0f)
810 dscale = 1.0f;
811 erpscale = cfm * 0.01f;
812 cfm = 0.0001f / cfm;
813 if (cfm > 0.01f)
814 cfm = 0.01f;
815
816 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
817 mu *= frictionMovementMult;
818
819 break;
820 default:
821 if (geom_name_map.TryGetValue(g2, out name))
822 {
823 if (name == "Terrain")
824 {
825 p1.getContactData(ref contactdata1);
826 bounce = contactdata1.bounce * TerrainBounce;
827 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
828 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
829 mu *= frictionMovementMult;
830 p1.CollidingGround = true;
831
832 cfm = p1.Mass;
833 dscale = 10 / cfm;
834 dscale = (float)Math.Sqrt(dscale);
835 if (dscale > 1.0f)
836 dscale = 1.0f;
837 erpscale = cfm * 0.01f;
838 cfm = 0.0001f / cfm;
839 if (cfm > 0.01f)
840 cfm = 0.01f;
841
842 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
843 {
844 if (curContact.side1 > 0)
845 IgnoreNegSides = true;
846 }
847
848 }
849 else if (name == "Water")
850 {
851 ignore = true;
852 }
853 }
854 else
855 ignore=true;
856 break;
857 }
858 break;
859
860 default:
861 if (geom_name_map.TryGetValue(g1, out name))
862 {
863 if (name == "Terrain")
864 {
865 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
866 {
867 p2.CollidingGround = true;
868 p2.getContactData(ref contactdata2);
869 bounce = contactdata2.bounce * TerrainBounce;
870 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
871
872 cfm = p2.Mass;
873 dscale = 10 / cfm;
874 dscale = (float)Math.Sqrt(dscale);
875
876 if (dscale > 1.0f)
877 dscale = 1.0f;
878
879 erpscale = cfm * 0.01f;
880 cfm = 0.0001f / cfm;
881 if (cfm > 0.01f)
882 cfm = 0.01f;
883
884 if (curContact.side1 > 0) // should be 2 ?
885 IgnoreNegSides = true;
886
887 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
888 mu *= frictionMovementMult;
889 }
890 else
891 ignore = true;
892
893 }
894 else if (name == "Water" &&
895 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
896 {
897 ignore = true;
898 }
899 }
900 else
901 ignore = true;
902 break;
903 }
904
905 if (ignore)
906 return;
907
908 IntPtr Joint;
909
910 int i = 0;
911 while(true)
912 {
913
914 if (IgnoreNegSides && curContact.side1 < 0)
915 {
916 if (++i >= count)
917 break;
918
919 if (!GetCurContactGeom(i, ref curContact))
920 break;
921 }
922 else
923
924 {
925 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
926 p1.IsColliding = true;
927 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
928 p2.IsColliding = true;
929
930 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
931 d.JointAttach(Joint, b1, b2);
932
933 if (++m_global_contactcount >= maxContactsbeforedeath)
934 break;
935
936 if (++i >= count)
937 break;
938
939 if (!GetCurContactGeom(i, ref curContact))
940 break;
941
942 if (curContact.depth > maxDepthContact.PenetrationDepth)
943 {
944 maxDepthContact.Position.X = curContact.pos.X;
945 maxDepthContact.Position.Y = curContact.pos.Y;
946 maxDepthContact.Position.Z = curContact.pos.Z;
947 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
948 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
949 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
950 maxDepthContact.PenetrationDepth = curContact.depth;
951 }
952 }
953 }
954
955 collision_accounting_events(p1, p2, maxDepthContact);
956
957/*
958 if (notskipedcount > geomContactPointsStartthrottle)
959 {
960 // If there are more then 3 contact points, it's likely
961 // that we've got a pile of objects, so ...
962 // We don't want to send out hundreds of terse updates over and over again
963 // so lets throttle them and send them again after it's somewhat sorted out.
964 this needs checking so out for now
965 if (b1 != IntPtr.Zero)
966 p1.ThrottleUpdates = true;
967 if (b2 != IntPtr.Zero)
968 p2.ThrottleUpdates = true;
969
970 }
971 */
972 }
973
974 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
975 {
976 // obj1LocalID = 0;
977 //returncollisions = false;
978 obj2LocalID = 0;
979 //ctype = 0;
980 //cStartStop = 0;
981 if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
982 return;
983
984 switch ((ActorTypes)p1.PhysicsActorType)
985 {
986 case ActorTypes.Agent:
987 cc1 = (OdeCharacter)p1;
988 switch ((ActorTypes)p2.PhysicsActorType)
989 {
990 case ActorTypes.Agent:
991 cc2 = (OdeCharacter)p2;
992 obj2LocalID = cc2.m_localID;
993 if (p2.SubscribedEvents())
994 cc2.AddCollisionEvent(cc1.m_localID, contact);
995 break;
996
997 case ActorTypes.Prim:
998 if (p2 is OdePrim)
999 {
1000 cp2 = (OdePrim)p2;
1001 obj2LocalID = cp2.m_localID;
1002 if (p2.SubscribedEvents())
1003 cp2.AddCollisionEvent(cc1.m_localID, contact);
1004 }
1005 break;
1006
1007 case ActorTypes.Ground:
1008 case ActorTypes.Unknown:
1009 default:
1010 obj2LocalID = 0;
1011 break;
1012 }
1013 if (p1.SubscribedEvents())
1014 {
1015 contact.SurfaceNormal = -contact.SurfaceNormal;
1016 cc1.AddCollisionEvent(obj2LocalID, contact);
1017 }
1018 break;
1019
1020 case ActorTypes.Prim:
1021
1022 if (p1 is OdePrim)
1023 {
1024 cp1 = (OdePrim)p1;
1025
1026 // obj1LocalID = cp2.m_localID;
1027 switch ((ActorTypes)p2.PhysicsActorType)
1028 {
1029 case ActorTypes.Agent:
1030 if (p2 is OdeCharacter)
1031 {
1032 cc2 = (OdeCharacter)p2;
1033 obj2LocalID = cc2.m_localID;
1034 if (p2.SubscribedEvents())
1035 cc2.AddCollisionEvent(cp1.m_localID, contact);
1036 }
1037 break;
1038 case ActorTypes.Prim:
1039
1040 if (p2 is OdePrim)
1041 {
1042 cp2 = (OdePrim)p2;
1043 obj2LocalID = cp2.m_localID;
1044 if (p2.SubscribedEvents())
1045 cp2.AddCollisionEvent(cp1.m_localID, contact);
1046 }
1047 break;
1048
1049 case ActorTypes.Ground:
1050 case ActorTypes.Unknown:
1051 default:
1052 obj2LocalID = 0;
1053 break;
1054 }
1055 if (p1.SubscribedEvents())
1056 {
1057 contact.SurfaceNormal = -contact.SurfaceNormal;
1058 cp1.AddCollisionEvent(obj2LocalID, contact);
1059 }
1060 }
1061 break;
1062 }
1063 }
1064
1065 /// <summary>
1066 /// This is our collision testing routine in ODE
1067 /// </summary>
1068 /// <param name="timeStep"></param>
1069 private void collision_optimized()
1070 {
1071 lock (_characters)
1072 {
1073 try
1074 {
1075 foreach (OdeCharacter chr in _characters)
1076 {
1077 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1078 continue;
1079
1080 chr.IsColliding = false;
1081 // chr.CollidingGround = false; not done here
1082 chr.CollidingObj = false;
1083 // do colisions with static space
1084 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1085 }
1086 }
1087 catch (AccessViolationException)
1088 {
1089 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1090 }
1091
1092 }
1093
1094 lock (_activeprims)
1095 {
1096 foreach (OdePrim aprim in _activeprims)
1097 {
1098 aprim.CollisionScore = 0;
1099 aprim.IsColliding = false;
1100 }
1101 }
1102
1103 // collide active prims with static enviroment
1104 lock (_activegroups)
1105 {
1106 try
1107 {
1108 foreach (OdePrim prm in _activegroups)
1109 {
1110 if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds)
1111 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1112 }
1113 }
1114 catch (AccessViolationException)
1115 {
1116 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1117 }
1118 }
1119 // finally colide active things amoung them
1120 try
1121 {
1122 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1123 }
1124 catch (AccessViolationException)
1125 {
1126 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1127 }
1128// _perloopContact.Clear();
1129 }
1130
1131 #endregion
1132
1133
1134
1135 /// <summary>
1136 /// Add actor to the list that should receive collision events in the simulate loop.
1137 /// </summary>
1138 /// <param name="obj"></param>
1139 public void AddCollisionEventReporting(PhysicsActor obj)
1140 {
1141 lock (_collisionEventPrim)
1142 {
1143 if (!_collisionEventPrim.Contains(obj))
1144 _collisionEventPrim.Add(obj);
1145 }
1146 }
1147
1148 /// <summary>
1149 /// Remove actor from the list that should receive collision events in the simulate loop.
1150 /// </summary>
1151 /// <param name="obj"></param>
1152 public void RemoveCollisionEventReporting(PhysicsActor obj)
1153 {
1154 lock (_collisionEventPrim)
1155 {
1156 if (_collisionEventPrim.Contains(obj))
1157 _collisionEventPrim.Remove(obj);
1158 }
1159 }
1160
1161 #region Add/Remove Entities
1162
1163 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1164 {
1165 Vector3 pos;
1166 pos.X = position.X;
1167 pos.Y = position.Y;
1168 pos.Z = position.Z;
1169 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1170 newAv.Flying = isFlying;
1171 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1172
1173 return newAv;
1174 }
1175
1176 public void AddCharacter(OdeCharacter chr)
1177 {
1178 lock (_characters)
1179 {
1180 if (!_characters.Contains(chr))
1181 {
1182 _characters.Add(chr);
1183 if (chr.bad)
1184 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1185 }
1186 }
1187 }
1188
1189 public void RemoveCharacter(OdeCharacter chr)
1190 {
1191 lock (_characters)
1192 {
1193 if (_characters.Contains(chr))
1194 {
1195 _characters.Remove(chr);
1196 }
1197 }
1198 }
1199
1200 public void BadCharacter(OdeCharacter chr)
1201 {
1202 lock (_badCharacter)
1203 {
1204 if (!_badCharacter.Contains(chr))
1205 _badCharacter.Add(chr);
1206 }
1207 }
1208
1209 public override void RemoveAvatar(PhysicsActor actor)
1210 {
1211 //m_log.Debug("[PHYSICS]:ODELOCK");
1212 ((OdeCharacter) actor).Destroy();
1213 }
1214
1215 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1216 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1217 {
1218 Vector3 pos = position;
1219 Vector3 siz = size;
1220 Quaternion rot = rotation;
1221
1222 OdePrim newPrim;
1223 lock (OdeLock)
1224 {
1225 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);
1226
1227 lock (_prims)
1228 _prims.Add(newPrim);
1229 }
1230 return newPrim;
1231 }
1232
1233 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1234 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
1235 {
1236 Vector3 pos = position;
1237 Vector3 siz = size;
1238 Quaternion rot = rotation;
1239
1240 OdePrim newPrim;
1241 lock (OdeLock)
1242 {
1243 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);
1244
1245 lock (_prims)
1246 _prims.Add(newPrim);
1247 }
1248 return newPrim;
1249 }
1250
1251 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1252 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1253 {
1254 Vector3 pos = position;
1255 Vector3 siz = size;
1256 Quaternion rot = rotation;
1257
1258 OdePrim newPrim;
1259 lock (OdeLock)
1260 {
1261 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);
1262
1263 lock (_prims)
1264 _prims.Add(newPrim);
1265 }
1266 return newPrim;
1267 }
1268
1269 public void addActivePrim(OdePrim activatePrim)
1270 {
1271 // adds active prim..
1272 lock (_activeprims)
1273 {
1274 if (!_activeprims.Contains(activatePrim))
1275 _activeprims.Add(activatePrim);
1276 }
1277 }
1278
1279 public void addActiveGroups(OdePrim activatePrim)
1280 {
1281 lock (_activegroups)
1282 {
1283 if (!_activegroups.Contains(activatePrim))
1284 _activegroups.Add(activatePrim);
1285 }
1286 }
1287
1288 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1289 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1290 {
1291 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid);
1292 }
1293
1294
1295 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1296 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1297 {
1298#if SPAM
1299 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1300#endif
1301
1302 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1303 }
1304
1305 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1306 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1307 {
1308#if SPAM
1309 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1310#endif
1311
1312 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1313 }
1314
1315 public override float TimeDilation
1316 {
1317 get { return m_timeDilation; }
1318 }
1319
1320 public override bool SupportsNINJAJoints
1321 {
1322 get { return false; }
1323 }
1324
1325
1326 public void remActivePrim(OdePrim deactivatePrim)
1327 {
1328 lock (_activeprims)
1329 {
1330 _activeprims.Remove(deactivatePrim);
1331 }
1332 }
1333 public void remActiveGroup(OdePrim deactivatePrim)
1334 {
1335 lock (_activegroups)
1336 {
1337 _activegroups.Remove(deactivatePrim);
1338 }
1339 }
1340
1341 public override void RemovePrim(PhysicsActor prim)
1342 {
1343 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1344 // removed in the next physics simulate pass.
1345 if (prim is OdePrim)
1346 {
1347// lock (OdeLock)
1348 {
1349 OdePrim p = (OdePrim)prim;
1350 p.setPrimForRemoval();
1351 }
1352 }
1353 }
1354 /// <summary>
1355 /// This is called from within simulate but outside the locked portion
1356 /// We need to do our own locking here
1357 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1358 /// Simulate() -- justincc).
1359 ///
1360 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1361 ///
1362 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1363 /// that the space was using.
1364 /// </summary>
1365 /// <param name="prim"></param>
1366 public void RemovePrimThreadLocked(OdePrim prim)
1367 {
1368 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1369 lock (prim)
1370 {
1371 RemoveCollisionEventReporting(prim);
1372 lock (_prims)
1373 _prims.Remove(prim);
1374 }
1375
1376 }
1377 #endregion
1378
1379 #region Space Separation Calculation
1380
1381 /// <summary>
1382 /// Called when a static prim moves or becomes static
1383 /// Places the prim in a space one the static sub-spaces grid
1384 /// </summary>
1385 /// <param name="geom">the pointer to the geom that moved</param>
1386 /// <param name="pos">the position that the geom moved to</param>
1387 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1388 /// <returns>a pointer to the new space it's in</returns>
1389 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1390 {
1391 // moves a prim into another static sub-space or from another space into a static sub-space
1392
1393 // Called ODEPrim so
1394 // it's already in locked space.
1395
1396 if (geom == IntPtr.Zero) // shouldn't happen
1397 return IntPtr.Zero;
1398
1399 // get the static sub-space for current position
1400 IntPtr newspace = calculateSpaceForGeom(pos);
1401
1402 if (newspace == currentspace) // if we are there all done
1403 return newspace;
1404
1405 // else remove it from its current space
1406 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1407 {
1408 if (d.GeomIsSpace(currentspace))
1409 {
1410 waitForSpaceUnlock(currentspace);
1411 d.SpaceRemove(currentspace, geom);
1412
1413 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1414 {
1415 d.SpaceDestroy(currentspace);
1416 }
1417 }
1418 else
1419 {
1420 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1421 " Geom:" + geom);
1422 }
1423 }
1424 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1425 {
1426 currentspace = d.GeomGetSpace(geom);
1427 if (currentspace != IntPtr.Zero)
1428 {
1429 if (d.GeomIsSpace(currentspace))
1430 {
1431 waitForSpaceUnlock(currentspace);
1432 d.SpaceRemove(currentspace, geom);
1433
1434 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1435 {
1436 d.SpaceDestroy(currentspace);
1437 }
1438
1439 }
1440 }
1441 }
1442
1443 // put the geom in the newspace
1444 waitForSpaceUnlock(newspace);
1445 d.SpaceAdd(newspace, geom);
1446
1447 // let caller know this newspace
1448 return newspace;
1449 }
1450
1451 /// <summary>
1452 /// Calculates the space the prim should be in by its position
1453 /// </summary>
1454 /// <param name="pos"></param>
1455 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1456 public IntPtr calculateSpaceForGeom(Vector3 pos)
1457 {
1458 int x, y;
1459 x = (int)(pos.X * spacesPerMeter);
1460 if (x < 0)
1461 x = 0;
1462 else if (x > spaceGridMaxX)
1463 x = spaceGridMaxX;
1464
1465 y = (int)(pos.Y * spacesPerMeter);
1466 if (y < 0)
1467 y = 0;
1468 else if (y >spaceGridMaxY)
1469 y = spaceGridMaxY;
1470
1471 IntPtr tmpSpace = staticPrimspace[x, y];
1472 return tmpSpace;
1473 }
1474
1475 #endregion
1476
1477 /// <summary>
1478 /// Routine to figure out if we need to mesh this prim with our mesher
1479 /// </summary>
1480 /// <param name="pbs"></param>
1481 /// <returns></returns>
1482 public bool needsMeshing(PrimitiveBaseShape pbs)
1483 {
1484 // most of this is redundant now as the mesher will return null if it cant mesh a prim
1485 // but we still need to check for sculptie meshing being enabled so this is the most
1486 // convenient place to do it for now...
1487
1488 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
1489 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
1490 int iPropertiesNotSupportedDefault = 0;
1491
1492 if (pbs.SculptEntry)
1493 {
1494 if(!meshSculptedPrim)
1495 return false;
1496 }
1497
1498 // 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
1499 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
1500 {
1501 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1502 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1503 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1504 {
1505
1506 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1507 && pbs.ProfileHollow == 0
1508 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1509 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1510 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1511 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1512 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1513 {
1514#if SPAM
1515 m_log.Warn("NonMesh");
1516#endif
1517 return false;
1518 }
1519 }
1520 }
1521
1522 // following code doesn't give meshs to boxes and spheres ever
1523 // and it's odd.. so for now just return true if asked to force meshs
1524 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1525
1526 if (forceSimplePrimMeshing)
1527 return true;
1528
1529 if (pbs.ProfileHollow != 0)
1530 iPropertiesNotSupportedDefault++;
1531
1532 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1533 iPropertiesNotSupportedDefault++;
1534
1535 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1536 iPropertiesNotSupportedDefault++;
1537
1538 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1539 iPropertiesNotSupportedDefault++;
1540
1541 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1542 iPropertiesNotSupportedDefault++;
1543
1544 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1545 iPropertiesNotSupportedDefault++;
1546
1547 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1548 iPropertiesNotSupportedDefault++;
1549
1550 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))
1551 iPropertiesNotSupportedDefault++;
1552
1553 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1554 iPropertiesNotSupportedDefault++;
1555
1556 // test for torus
1557 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1558 {
1559 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1560 {
1561 iPropertiesNotSupportedDefault++;
1562 }
1563 }
1564 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1565 {
1566 if (pbs.PathCurve == (byte)Extrusion.Straight)
1567 {
1568 iPropertiesNotSupportedDefault++;
1569 }
1570
1571 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1572 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1573 {
1574 iPropertiesNotSupportedDefault++;
1575 }
1576 }
1577 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1578 {
1579 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1580 {
1581 iPropertiesNotSupportedDefault++;
1582 }
1583 }
1584 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1585 {
1586 if (pbs.PathCurve == (byte)Extrusion.Straight)
1587 {
1588 iPropertiesNotSupportedDefault++;
1589 }
1590 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1591 {
1592 iPropertiesNotSupportedDefault++;
1593 }
1594 }
1595
1596 if (pbs.SculptEntry && meshSculptedPrim)
1597 iPropertiesNotSupportedDefault++;
1598
1599 if (iPropertiesNotSupportedDefault == 0)
1600 {
1601#if SPAM
1602 m_log.Warn("NonMesh");
1603#endif
1604 return false;
1605 }
1606#if SPAM
1607 m_log.Debug("Mesh");
1608#endif
1609 return true;
1610 }
1611
1612 /// <summary>
1613 /// Called to queue a change to a actor
1614 /// to use in place of old taint mechanism so changes do have a time sequence
1615 /// </summary>
1616
1617 public void AddChange(PhysicsActor actor, changes what, Object arg)
1618 {
1619 ODEchangeitem item = new ODEchangeitem();
1620 item.actor = actor;
1621 item.what = what;
1622 item.arg = arg;
1623 ChangesQueue.Enqueue(item);
1624 }
1625
1626 /// <summary>
1627 /// Called after our prim properties are set Scale, position etc.
1628 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1629 /// This assures us that we have no race conditions
1630 /// </summary>
1631 /// <param name="prim"></param>
1632 public override void AddPhysicsActorTaint(PhysicsActor prim)
1633 {
1634 }
1635
1636 /// <summary>
1637 /// This is our main simulate loop
1638 /// It's thread locked by a Mutex in the scene.
1639 /// It holds Collisions, it instructs ODE to step through the physical reactions
1640 /// It moves the objects around in memory
1641 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1642 /// </summary>
1643 /// <param name="timeStep"></param>
1644 /// <returns></returns>
1645 public override float Simulate(float timeStep)
1646 {
1647 int statstart;
1648 int statchanges = 0;
1649 int statchmove = 0;
1650 int statactmove = 0;
1651 int statray = 0;
1652 int statcol = 0;
1653 int statstep = 0;
1654 int statmovchar = 0;
1655 int statmovprim;
1656 int totjcontact = 0;
1657
1658 // acumulate time so we can reduce error
1659 step_time += timeStep;
1660
1661 if (step_time < ODE_STEPSIZE)
1662 return 0;
1663
1664 if (framecount >= int.MaxValue)
1665 framecount = 0;
1666
1667 framecount++;
1668
1669 int curphysiteractions = m_physicsiterations;
1670
1671 if (step_time >= m_SkipFramesAtms)
1672 {
1673 // if in trouble reduce step resolution
1674 curphysiteractions /= 2;
1675 }
1676
1677 int nodeframes = 0;
1678
1679// checkThread();
1680
1681 lock (SimulationLock)
1682 {
1683 // adjust number of iterations per step
1684 try
1685 {
1686 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1687 }
1688 catch (StackOverflowException)
1689 {
1690 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1691// ode.drelease(world);
1692 base.TriggerPhysicsBasedRestart();
1693 }
1694
1695
1696 while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
1697 {
1698 try
1699 {
1700 statstart = Util.EnvironmentTickCount();
1701
1702 // clear pointer/counter to contacts to pass into joints
1703 m_global_contactcount = 0;
1704
1705 ODEchangeitem item;
1706
1707 if(ChangesQueue.Count >0)
1708 {
1709 int ttmpstart = Util.EnvironmentTickCount();
1710 int ttmp;
1711 int ttmp2;
1712
1713 while(ChangesQueue.Dequeue(out item))
1714 {
1715 if (item.actor != null)
1716 {
1717 try
1718 {
1719 if (item.actor is OdeCharacter)
1720 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1721 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1722 RemovePrimThreadLocked((OdePrim)item.actor);
1723 }
1724 catch
1725 {
1726 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1727 };
1728 }
1729 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1730 if (ttmp > 20)
1731 break;
1732 }
1733
1734 ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart);
1735 if (ttmp2 > 50)
1736 ttmp2 = 0;
1737
1738 }
1739
1740 statchanges += Util.EnvironmentTickCountSubtract(statstart);
1741
1742 statactmove += Util.EnvironmentTickCountSubtract(statstart);
1743 //if ((framecount % m_randomizeWater) == 0)
1744 // randomizeWater(waterlevel);
1745
1746 m_rayCastManager.ProcessQueuedRequests();
1747
1748
1749
1750 statray += Util.EnvironmentTickCountSubtract(statstart);
1751 collision_optimized();
1752 statcol += Util.EnvironmentTickCountSubtract(statstart);
1753
1754 lock (_collisionEventPrim)
1755 {
1756 foreach (PhysicsActor obj in _collisionEventPrim)
1757 {
1758 if (obj == null)
1759 continue;
1760
1761 switch ((ActorTypes)obj.PhysicsActorType)
1762 {
1763 case ActorTypes.Agent:
1764 OdeCharacter cobj = (OdeCharacter)obj;
1765 cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f));
1766 cobj.SendCollisions();
1767 break;
1768
1769 case ActorTypes.Prim:
1770 OdePrim pobj = (OdePrim)obj;
1771 pobj.SendCollisions();
1772 break;
1773 }
1774 }
1775 }
1776
1777 d.WorldQuickStep(world, ODE_STEPSIZE);
1778 statstep += Util.EnvironmentTickCountSubtract(statstart);
1779
1780 // Move characters
1781 lock (_characters)
1782 {
1783 List<OdeCharacter> defects = new List<OdeCharacter>();
1784 foreach (OdeCharacter actor in _characters)
1785 {
1786 if (actor != null)
1787 actor.Move(ODE_STEPSIZE, defects);
1788 }
1789 if (defects.Count != 0)
1790 {
1791 foreach (OdeCharacter defect in defects)
1792 {
1793 RemoveCharacter(defect);
1794 }
1795 }
1796 }
1797 statchmove += Util.EnvironmentTickCountSubtract(statstart);
1798
1799 // Move other active objects
1800 lock (_activegroups)
1801 {
1802 foreach (OdePrim aprim in _activegroups)
1803 {
1804 aprim.Move();
1805 }
1806 }
1807
1808 //ode.dunlock(world);
1809 }
1810 catch (Exception e)
1811 {
1812 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1813// ode.dunlock(world);
1814 }
1815
1816 d.JointGroupEmpty(contactgroup);
1817 totjcontact += m_global_contactcount;
1818
1819 step_time -= ODE_STEPSIZE;
1820 nodeframes++;
1821 }
1822
1823 statstart = Util.EnvironmentTickCount();
1824
1825 lock (_characters)
1826 {
1827 foreach (OdeCharacter actor in _characters)
1828 {
1829 if (actor != null)
1830 {
1831 if (actor.bad)
1832 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1833
1834 actor.UpdatePositionAndVelocity();
1835 }
1836 }
1837 }
1838
1839 lock (_badCharacter)
1840 {
1841 if (_badCharacter.Count > 0)
1842 {
1843 foreach (OdeCharacter chr in _badCharacter)
1844 {
1845 RemoveCharacter(chr);
1846 }
1847
1848 _badCharacter.Clear();
1849 }
1850 }
1851 statmovchar = Util.EnvironmentTickCountSubtract(statstart);
1852
1853 lock (_activegroups)
1854 {
1855 {
1856 foreach (OdePrim actor in _activegroups)
1857 {
1858 if (actor.IsPhysical)
1859 {
1860 actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
1861 }
1862 }
1863 }
1864 }
1865
1866 statmovprim = Util.EnvironmentTickCountSubtract(statstart);
1867
1868 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1869 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1870 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1871 int nbodies = d.NTotalBodies;
1872 int ngeoms = d.NTotalGeoms;
1873
1874 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1875 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1876 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1877 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1878 {
1879 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1880 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1881
1882 if (physics_logging_append_existing_logfile)
1883 {
1884 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1885 TextWriter fwriter = File.AppendText(fname);
1886 fwriter.WriteLine(header);
1887 fwriter.Close();
1888 }
1889
1890 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1891 }
1892
1893 // think time dilation is not a physics issue alone.. but ok let's fake something
1894 if (step_time < ODE_STEPSIZE) // we did the required loops
1895 m_timeDilation = 1.0f;
1896 else
1897 { // we didn't forget the lost ones and let user know something
1898 m_timeDilation = 1 - step_time / timeStep;
1899 if (m_timeDilation < 0)
1900 m_timeDilation = 0;
1901 step_time = 0;
1902 }
1903 }
1904
1905// return nodeframes * ODE_STEPSIZE; // return real simulated time
1906 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
1907 }
1908
1909 /// <summary>
1910 public override void GetResults()
1911 {
1912 }
1913
1914 public override bool IsThreaded
1915 {
1916 // for now we won't be multithreaded
1917 get { return (false); }
1918 }
1919
1920 public float GetTerrainHeightAtXY(float x, float y)
1921 {
1922
1923
1924 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1925 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1926
1927
1928 IntPtr heightFieldGeom = IntPtr.Zero;
1929
1930 // get region map
1931 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
1932 return 0f;
1933
1934 if (heightFieldGeom == IntPtr.Zero)
1935 return 0f;
1936
1937 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1938 return 0f;
1939
1940 // TerrainHeightField for ODE as offset 1m
1941 x += 1f - offsetX;
1942 y += 1f - offsetY;
1943
1944 // make position fit into array
1945 if (x < 0)
1946 x = 0;
1947 if (y < 0)
1948 y = 0;
1949
1950 // integer indexs
1951 int ix;
1952 int iy;
1953 // interpolators offset
1954 float dx;
1955 float dy;
1956
1957 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
1958
1959 if (OdeUbitLib)
1960 {
1961 if (x < regsize - 1)
1962 {
1963 ix = (int)x;
1964 dx = x - (float)ix;
1965 }
1966 else // out world use external height
1967 {
1968 ix = regsize - 1;
1969 dx = 0;
1970 }
1971 if (y < regsize - 1)
1972 {
1973 iy = (int)y;
1974 dy = y - (float)iy;
1975 }
1976 else
1977 {
1978 iy = regsize - 1;
1979 dy = 0;
1980 }
1981 }
1982
1983 else
1984 {
1985 // we still have square fixed size regions
1986 // also flip x and y because of how map is done for ODE fliped axis
1987 // so ix,iy,dx and dy are inter exchanged
1988 if (x < regsize - 1)
1989 {
1990 iy = (int)x;
1991 dy = x - (float)iy;
1992 }
1993 else // out world use external height
1994 {
1995 iy = regsize - 1;
1996 dy = 0;
1997 }
1998 if (y < regsize - 1)
1999 {
2000 ix = (int)y;
2001 dx = y - (float)ix;
2002 }
2003 else
2004 {
2005 ix = regsize - 1;
2006 dx = 0;
2007 }
2008 }
2009
2010 float h0;
2011 float h1;
2012 float h2;
2013
2014 iy *= regsize;
2015 iy += ix; // all indexes have iy + ix
2016
2017 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2018
2019 if ((dx + dy) <= 1.0f)
2020 {
2021 h0 = ((float)heights[iy]); // 0,0 vertice
2022 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2023 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2024 }
2025 else
2026 {
2027 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2028 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2029 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2030 }
2031
2032 return h0 + h1 + h2;
2033 }
2034
2035
2036 public override void SetTerrain(float[] heightMap)
2037 {
2038 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2039 {
2040 if (m_parentScene is OdeScene)
2041 {
2042 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2043 }
2044 }
2045 else
2046 {
2047 SetTerrain(heightMap, m_worldOffset);
2048 }
2049 }
2050
2051 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2052 {
2053 SetTerrain(heightMap, pOffset);
2054 }
2055
2056 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2057 {
2058 if (OdeUbitLib)
2059 UbitSetTerrain(heightMap, pOffset);
2060 else
2061 OriSetTerrain(heightMap, pOffset);
2062 }
2063
2064 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2065 {
2066 // assumes 1m size grid and constante size square regions
2067 // needs to know about sims around in future
2068
2069 float[] _heightmap;
2070
2071 uint heightmapWidth = Constants.RegionSize + 2;
2072 uint heightmapHeight = Constants.RegionSize + 2;
2073
2074 uint heightmapWidthSamples = heightmapWidth + 1;
2075 uint heightmapHeightSamples = heightmapHeight + 1;
2076
2077 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2078
2079 const float scale = 1.0f;
2080 const float offset = 0.0f;
2081 const float thickness = 10f;
2082 const int wrap = 0;
2083
2084 uint regionsize = Constants.RegionSize;
2085
2086 float hfmin = float.MaxValue;
2087 float hfmax = float.MinValue;
2088 float val;
2089 uint xx;
2090 uint yy;
2091
2092 uint maxXXYY = regionsize - 1;
2093 // flipping map adding one margin all around so things don't fall in edges
2094
2095 uint xt = 0;
2096 xx = 0;
2097
2098 for (uint x = 0; x < heightmapWidthSamples; x++)
2099 {
2100 if (x > 1 && xx < maxXXYY)
2101 xx++;
2102 yy = 0;
2103 for (uint y = 0; y < heightmapHeightSamples; y++)
2104 {
2105 if (y > 1 && y < maxXXYY)
2106 yy += regionsize;
2107
2108 val = heightMap[yy + xx];
2109 if (val < 0.0f)
2110 val = 0.0f; // no neg terrain as in chode
2111 _heightmap[xt + y] = val;
2112
2113 if (hfmin > val)
2114 hfmin = val;
2115 if (hfmax < val)
2116 hfmax = val;
2117 }
2118 xt += heightmapHeightSamples;
2119 }
2120 lock (OdeLock)
2121 {
2122 IntPtr GroundGeom = IntPtr.Zero;
2123 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2124 {
2125 RegionTerrain.Remove(pOffset);
2126 if (GroundGeom != IntPtr.Zero)
2127 {
2128 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2129 {
2130 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2131 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2132 TerrainHeightFieldHeights.Remove(GroundGeom);
2133 }
2134 d.SpaceRemove(StaticSpace, GroundGeom);
2135 d.GeomDestroy(GroundGeom);
2136 }
2137 }
2138 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2139
2140 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2141
2142 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2143 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2144 offset, thickness, wrap);
2145
2146 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2147 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2148 if (GroundGeom != IntPtr.Zero)
2149 {
2150 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2151 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2152
2153 }
2154 geom_name_map[GroundGeom] = "Terrain";
2155
2156 d.Matrix3 R = new d.Matrix3();
2157
2158 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2159 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2160
2161
2162 q1 = q1 * q2;
2163
2164 Vector3 v3;
2165 float angle;
2166 q1.GetAxisAngle(out v3, out angle);
2167
2168 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2169 d.GeomSetRotation(GroundGeom, ref R);
2170 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2171 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2172// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2173 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2174 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2175
2176 }
2177 }
2178
2179 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2180 {
2181 // assumes 1m size grid and constante size square regions
2182 // needs to know about sims around in future
2183
2184 float[] _heightmap;
2185
2186 uint heightmapWidth = Constants.RegionSize + 2;
2187 uint heightmapHeight = Constants.RegionSize + 2;
2188
2189 uint heightmapWidthSamples = heightmapWidth + 1;
2190 uint heightmapHeightSamples = heightmapHeight + 1;
2191
2192 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2193
2194
2195 uint regionsize = Constants.RegionSize;
2196
2197 float hfmin = float.MaxValue;
2198// float hfmax = float.MinValue;
2199 float val;
2200
2201
2202 uint maxXXYY = regionsize - 1;
2203 // adding one margin all around so things don't fall in edges
2204
2205 uint xx;
2206 uint yy = 0;
2207 uint yt = 0;
2208
2209 for (uint y = 0; y < heightmapHeightSamples; y++)
2210 {
2211 if (y > 1 && y < maxXXYY)
2212 yy += regionsize;
2213 xx = 0;
2214 for (uint x = 0; x < heightmapWidthSamples; x++)
2215 {
2216 if (x > 1 && x < maxXXYY)
2217 xx++;
2218
2219 val = heightMap[yy + xx];
2220 if (val < 0.0f)
2221 val = 0.0f; // no neg terrain as in chode
2222 _heightmap[yt + x] = val;
2223
2224 if (hfmin > val)
2225 hfmin = val;
2226// if (hfmax < val)
2227// hfmax = val;
2228 }
2229 yt += heightmapWidthSamples;
2230 }
2231 lock (OdeLock)
2232 {
2233 IntPtr GroundGeom = IntPtr.Zero;
2234 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2235 {
2236 RegionTerrain.Remove(pOffset);
2237 if (GroundGeom != IntPtr.Zero)
2238 {
2239 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2240 {
2241 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2242 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2243 TerrainHeightFieldHeights.Remove(GroundGeom);
2244 }
2245 d.SpaceRemove(StaticSpace, GroundGeom);
2246 d.GeomDestroy(GroundGeom);
2247 }
2248 }
2249 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2250
2251 const int wrap = 0;
2252 float thickness = hfmin;
2253 if (thickness < 0)
2254 thickness = 1;
2255
2256 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2257
2258 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2259 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2260 thickness, wrap);
2261
2262// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2263 GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1);
2264 if (GroundGeom != IntPtr.Zero)
2265 {
2266 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2267 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2268
2269 }
2270 geom_name_map[GroundGeom] = "Terrain";
2271
2272 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2273 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2274 // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2275 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2276 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2277 }
2278 }
2279
2280
2281 public override void DeleteTerrain()
2282 {
2283 }
2284
2285 public float GetWaterLevel()
2286 {
2287 return waterlevel;
2288 }
2289
2290 public override bool SupportsCombining()
2291 {
2292 return true;
2293 }
2294/*
2295 public override void UnCombine(PhysicsScene pScene)
2296 {
2297 IntPtr localGround = IntPtr.Zero;
2298// float[] localHeightfield;
2299 bool proceed = false;
2300 List<IntPtr> geomDestroyList = new List<IntPtr>();
2301
2302 lock (OdeLock)
2303 {
2304 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2305 {
2306 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2307 {
2308 if (geom == localGround)
2309 {
2310// localHeightfield = TerrainHeightFieldHeights[geom];
2311 proceed = true;
2312 }
2313 else
2314 {
2315 geomDestroyList.Add(geom);
2316 }
2317 }
2318
2319 if (proceed)
2320 {
2321 m_worldOffset = Vector3.Zero;
2322 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2323 m_parentScene = null;
2324
2325 foreach (IntPtr g in geomDestroyList)
2326 {
2327 // removingHeightField needs to be done or the garbage collector will
2328 // collect the terrain data before we tell ODE to destroy it causing
2329 // memory corruption
2330 if (TerrainHeightFieldHeights.ContainsKey(g))
2331 {
2332// float[] removingHeightField = TerrainHeightFieldHeights[g];
2333 TerrainHeightFieldHeights.Remove(g);
2334
2335 if (RegionTerrain.ContainsKey(g))
2336 {
2337 RegionTerrain.Remove(g);
2338 }
2339
2340 d.GeomDestroy(g);
2341 //removingHeightField = new float[0];
2342 }
2343 }
2344
2345 }
2346 else
2347 {
2348 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2349 }
2350 }
2351 }
2352 }
2353*/
2354 public override void SetWaterLevel(float baseheight)
2355 {
2356 waterlevel = baseheight;
2357 randomizeWater(waterlevel);
2358 }
2359
2360 public void randomizeWater(float baseheight)
2361 {
2362 const uint heightmapWidth = m_regionWidth + 2;
2363 const uint heightmapHeight = m_regionHeight + 2;
2364 const uint heightmapWidthSamples = m_regionWidth + 2;
2365 const uint heightmapHeightSamples = m_regionHeight + 2;
2366 const float scale = 1.0f;
2367 const float offset = 0.0f;
2368 const float thickness = 2.9f;
2369 const int wrap = 0;
2370
2371 for (int i = 0; i < (258 * 258); i++)
2372 {
2373 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
2374 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
2375 }
2376
2377 lock (OdeLock)
2378 {
2379 if (WaterGeom != IntPtr.Zero)
2380 {
2381 d.SpaceRemove(StaticSpace, WaterGeom);
2382 }
2383 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2384 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
2385 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2386 offset, thickness, wrap);
2387 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
2388 WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2389 if (WaterGeom != IntPtr.Zero)
2390 {
2391 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
2392 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
2393
2394 }
2395 geom_name_map[WaterGeom] = "Water";
2396
2397 d.Matrix3 R = new d.Matrix3();
2398
2399 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2400 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2401
2402 q1 = q1 * q2;
2403 Vector3 v3;
2404 float angle;
2405 q1.GetAxisAngle(out v3, out angle);
2406
2407 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2408 d.GeomSetRotation(WaterGeom, ref R);
2409 d.GeomSetPosition(WaterGeom, 128, 128, 0);
2410
2411 }
2412
2413 }
2414
2415 public override void Dispose()
2416 {
2417 m_rayCastManager.Dispose();
2418 m_rayCastManager = null;
2419
2420 lock (OdeLock)
2421 {
2422 lock (_prims)
2423 {
2424 foreach (OdePrim prm in _prims)
2425 {
2426 RemovePrim(prm);
2427 }
2428 }
2429
2430 if (ContactgeomsArray != IntPtr.Zero)
2431 Marshal.FreeHGlobal(ContactgeomsArray);
2432 if (GlobalContactsArray != IntPtr.Zero)
2433 Marshal.FreeHGlobal(GlobalContactsArray);
2434
2435 d.WorldDestroy(world);
2436 //d.CloseODE();
2437 }
2438 }
2439
2440 public override Dictionary<uint, float> GetTopColliders()
2441 {
2442 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2443 int cnt = 0;
2444 lock (_prims)
2445 {
2446 foreach (OdePrim prm in _prims)
2447 {
2448 if (prm.CollisionScore > 0)
2449 {
2450 returncolliders.Add(prm.m_localID, prm.CollisionScore);
2451 cnt++;
2452 prm.CollisionScore = 0f;
2453 if (cnt > 25)
2454 {
2455 break;
2456 }
2457 }
2458 }
2459 }
2460 return returncolliders;
2461 }
2462
2463 public override bool SupportsRayCast()
2464 {
2465 return true;
2466 }
2467
2468 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2469 {
2470 if (retMethod != null)
2471 {
2472 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2473 }
2474 }
2475
2476 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2477 {
2478 if (retMethod != null)
2479 {
2480 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2481 }
2482 }
2483
2484 // don't like this
2485 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2486 {
2487 ContactResult[] ourResults = null;
2488 RayCallback retMethod = delegate(List<ContactResult> results)
2489 {
2490 ourResults = new ContactResult[results.Count];
2491 results.CopyTo(ourResults, 0);
2492 };
2493 int waitTime = 0;
2494 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2495 while (ourResults == null && waitTime < 1000)
2496 {
2497 Thread.Sleep(1);
2498 waitTime++;
2499 }
2500 if (ourResults == null)
2501 return new List<ContactResult>();
2502 return new List<ContactResult>(ourResults);
2503 }
2504
2505 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2506 {
2507 if (retMethod != null && actor !=null)
2508 {
2509 IntPtr geom;
2510 if (actor is OdePrim)
2511 geom = ((OdePrim)actor).prim_geom;
2512 else if (actor is OdeCharacter)
2513 geom = ((OdePrim)actor).prim_geom;
2514 else
2515 return;
2516 if (geom == IntPtr.Zero)
2517 return;
2518 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2519 }
2520 }
2521
2522 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2523 {
2524 if (retMethod != null && actor != null)
2525 {
2526 IntPtr geom;
2527 if (actor is OdePrim)
2528 geom = ((OdePrim)actor).prim_geom;
2529 else if (actor is OdeCharacter)
2530 geom = ((OdePrim)actor).prim_geom;
2531 else
2532 return;
2533 if (geom == IntPtr.Zero)
2534 return;
2535
2536 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2537 }
2538 }
2539
2540 // don't like this
2541 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2542 {
2543 if (actor != null)
2544 {
2545 IntPtr geom;
2546 if (actor is OdePrim)
2547 geom = ((OdePrim)actor).prim_geom;
2548 else if (actor is OdeCharacter)
2549 geom = ((OdePrim)actor).prim_geom;
2550 else
2551 return new List<ContactResult>();
2552 if (geom == IntPtr.Zero)
2553 return new List<ContactResult>();
2554
2555 ContactResult[] ourResults = null;
2556 RayCallback retMethod = delegate(List<ContactResult> results)
2557 {
2558 ourResults = new ContactResult[results.Count];
2559 results.CopyTo(ourResults, 0);
2560 };
2561 int waitTime = 0;
2562 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2563 while (ourResults == null && waitTime < 1000)
2564 {
2565 Thread.Sleep(1);
2566 waitTime++;
2567 }
2568 if (ourResults == null)
2569 return new List<ContactResult>();
2570 return new List<ContactResult>(ourResults);
2571 }
2572 return new List<ContactResult>();
2573 }
2574 }
2575}
generated by cgit v1.1 at 2024-12-16 16:57:45 +0000