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