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