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