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