aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin
diff options
context:
space:
mode:
authorRobert Adams2015-09-08 06:15:46 -0700
committerRobert Adams2015-09-08 06:15:46 -0700
commit4dd17c4117ea413fb0c4418511956cb3abfe258c (patch)
tree6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/Physics/UbitOdePlugin
parentMerge of ubitworkvarnew with opensim/master as of 20150905. (diff)
downloadopensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz
More 'everything is a module' merging.
Have most of UbitOde converted. There are compile errors in OpenSimBase as the new modules stuff is not all there. Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1847
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs1096
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs933
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs3901
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs683
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs356
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs2025
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs90
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2869
10 files changed, 0 insertions, 13858 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
deleted file mode 100644
index d46341b..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
+++ /dev/null
@@ -1,58 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System.Reflection;
29using System.Runtime.InteropServices;
30
31// Information about this assembly is defined by the following
32// attributes.
33//
34// change them to the information which is associated with the assembly
35// you compile.
36
37[assembly : AssemblyTitle("OdePlugin")]
38[assembly : AssemblyDescription("Ubit Variation")]
39[assembly : AssemblyConfiguration("")]
40[assembly : AssemblyCompany("http://opensimulator.org")]
41[assembly : AssemblyProduct("OdePlugin")]
42[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
43[assembly : AssemblyTrademark("")]
44[assembly : AssemblyCulture("")]
45
46// This sets the default COM visibility of types in the assembly to invisible.
47// If you need to expose a type to COM, use [ComVisible(true)] on that type.
48
49[assembly : ComVisible(false)]
50
51// The assembly version has following format :
52//
53// Major.Minor.Build.Revision
54//
55// You can specify all values by your own or you can build default build and revision
56// numbers with the '*' character (the default):
57
58[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
deleted file mode 100644
index 0e2cbd2..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ /dev/null
@@ -1,1847 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28
29// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private Vector3 _velocity;
78 private Vector3 _target_velocity;
79 private Vector3 _acceleration;
80 private Vector3 m_rotationalVelocity;
81 private Vector3 m_size;
82 private 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/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
deleted file mode 100644
index 3c952ae..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ /dev/null
@@ -1,1096 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30 * ODEPrim.cs contains methods dealing with Prim editing, Prim
31 * characteristics and Kinetic motion.
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use.
39 */
40
41// Extensive change Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager;
52
53namespace OpenSim.Region.Physics.OdePlugin
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private OdeScene _pParentScene;
64
65 // Vehicle properties
66 // WARNING this are working copies for internel use
67 // their values may not be the corresponding parameter
68
69 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71
72 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73
74 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75 // HOVER_TERRAIN_ONLY
76 // HOVER_GLOBAL_HEIGHT
77 // NO_DEFLECTION_UP
78 // HOVER_WATER_ONLY
79 // HOVER_UP_ONLY
80 // LIMIT_MOTOR_UP
81 // LIMIT_ROLL_ONLY
82 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83
84 // Linear properties
85 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87 private float m_linearMotorDecayTimescale = 120;
88 private float m_linearMotorTimescale = 1000;
89 private Vector3 m_linearMotorOffset = Vector3.Zero;
90
91 //Angular properties
92 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96
97 //Deflection properties
98 private float m_angularDeflectionEfficiency = 0;
99 private float m_angularDeflectionTimescale = 1000;
100 private float m_linearDeflectionEfficiency = 0;
101 private float m_linearDeflectionTimescale = 1000;
102
103 //Banking properties
104 private float m_bankingEfficiency = 0;
105 private float m_bankingMix = 0;
106 private float m_bankingTimescale = 1000;
107
108 //Hover and Buoyancy properties
109 private float m_VhoverHeight = 0f;
110 private float m_VhoverEfficiency = 0f;
111 private float m_VhoverTimescale = 1000f;
112 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116
117 //Attractor properties
118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120
121
122 // auxiliar
123 private float m_lmEfect = 0f; // current linear motor eficiency
124 private float m_lmDecay = 0f; // current linear decay
125
126 private float m_amEfect = 0; // current angular motor eficiency
127 private float m_amDecay = 0f; // current linear decay
128
129 private float m_ffactor = 1.0f;
130
131 private float m_timestep = 0.02f;
132 private float m_invtimestep = 50;
133
134
135 float m_ampwr;
136 float m_amdampX;
137 float m_amdampY;
138 float m_amdampZ;
139
140 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/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
deleted file mode 100644
index a6bdaa0..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
+++ /dev/null
@@ -1,933 +0,0 @@
1/*
2 * AJLDuarte 2012
3 */
4
5using System;
6using System.Threading;
7using System.Collections.Generic;
8using System.IO;
9using System.Reflection;
10using System.Runtime.InteropServices;
11using System.Text;
12using OpenSim.Framework;
13using OpenSim.Region.Physics.Manager;
14using OdeAPI;
15using log4net;
16using Nini.Config;
17using OpenMetaverse;
18
19namespace OpenSim.Region.Physics.OdePlugin
20{
21 public enum MeshState : byte
22 {
23 noNeed = 0,
24
25 loadingAsset = 1,
26
27 AssetOK = 0x0f, // 00001111
28
29 NeedMask = 0x30, // 00110000
30 needMesh = 0x10, // 00010000
31 needAsset = 0x20, // 00100000
32
33 FailMask = 0xC0, // 11000000
34 AssetFailed = 0x40, // 01000000
35 MeshFailed = 0x80, // 10000000
36
37 MeshNoColide = FailMask | needAsset
38 }
39
40 public enum meshWorkerCmnds : byte
41 {
42 nop = 0,
43 addnew,
44 changefull,
45 changesize,
46 changeshapetype,
47 getmesh,
48 }
49
50 public class ODEPhysRepData
51 {
52 public PhysicsActor actor;
53 public PrimitiveBaseShape pbs;
54 public IMesh mesh;
55
56 public Vector3 size;
57 public Vector3 OBB;
58 public Vector3 OBBOffset;
59
60 public float volume;
61
62 public byte shapetype;
63 public bool hasOBB;
64 public bool hasMeshVolume;
65 public MeshState meshState;
66 public UUID? assetID;
67 public meshWorkerCmnds comand;
68 }
69
70 public class ODEMeshWorker
71 {
72
73 private ILog m_log;
74 private OdeScene m_scene;
75 private IMesher m_mesher;
76
77 public bool meshSculptedPrim = true;
78 public bool forceSimplePrimMeshing = false;
79 public float meshSculptLOD = 32;
80 public float MeshSculptphysicalLOD = 32;
81
82
83 private OpenSim.Framework.BlockingQueue<ODEPhysRepData> createqueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>();
84 private bool m_running;
85
86 private Thread m_thread;
87
88 public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig)
89 {
90 m_scene = pScene;
91 m_log = pLog;
92 m_mesher = pMesher;
93
94 if (pConfig != null)
95 {
96 forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
97 meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
98 meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD);
99 MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
100 }
101 m_running = true;
102 m_thread = new Thread(DoWork);
103 m_thread.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} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
deleted file mode 100644
index 8bcdd89..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ /dev/null
@@ -1,3901 +0,0 @@
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.Physics.Manager;
59
60namespace OpenSim.Region.Physics.OdePlugin
61{
62 public class OdePrim : PhysicsActor
63 {
64 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
65
66 private bool m_isphysical;
67 private bool m_fakeisphysical;
68 private bool m_isphantom;
69 private bool m_fakeisphantom;
70 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
71 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72
73 protected bool m_building;
74 protected bool m_forcePosOrRotation;
75 private bool m_iscolliding;
76
77 internal bool m_isSelected;
78 private bool m_delaySelect;
79 private bool m_lastdoneSelected;
80 internal bool m_outbounds;
81
82 private Quaternion m_lastorientation;
83 private Quaternion _orientation;
84
85 private Vector3 _position;
86 private Vector3 _velocity;
87 private Vector3 m_torque;
88 private Vector3 m_lastVelocity;
89 private Vector3 m_lastposition;
90 private Vector3 m_rotationalVelocity;
91 private Vector3 _size;
92 private Vector3 _acceleration;
93 private Vector3 m_angularlock = Vector3.One;
94 private IntPtr Amotor;
95
96 private Vector3 m_force;
97 private Vector3 m_forceacc;
98 private Vector3 m_angularForceacc;
99
100 private float m_invTimeStep;
101 private float m_timeStep;
102
103 private Vector3 m_PIDTarget;
104 private float m_PIDTau;
105 private bool m_usePID;
106
107 private float m_PIDHoverHeight;
108 private float m_PIDHoverTau;
109 private bool m_useHoverPID;
110 private PIDHoverType m_PIDHoverType;
111 private float m_targetHoverHeight;
112 private float m_groundHeight;
113 private float m_waterHeight;
114 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
115
116 private int body_autodisable_frames;
117 public int bodydisablecontrol;
118 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/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
deleted file mode 100644
index 73ababa..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
+++ /dev/null
@@ -1,683 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// Processes raycast requests as ODE is in a state to be able to do them.
43 /// This ensures that it's thread safe and there will be no conflicts.
44 /// Requests get returned by a different thread then they were requested by.
45 /// </summary>
46 public class ODERayCastRequestManager
47 {
48 /// <summary>
49 /// Pending ray requests
50 /// </summary>
51 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
52
53 /// <summary>
54 /// Scene that created this object.
55 /// </summary>
56 private OdeScene m_scene;
57
58 IntPtr ray; // the ray. we only need one for our lifetime
59 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} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
deleted file mode 100644
index 672212f..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
+++ /dev/null
@@ -1,356 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27// Ubit 2012
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// </summary>
43 public class ODESitAvatar
44 {
45 private OdeScene m_scene;
46 private ODERayCastRequestManager m_raymanager;
47
48 public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
49 {
50 m_scene = pScene;
51 m_raymanager = raymanager;
52 }
53
54 private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
55 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} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
deleted file mode 100644
index 10d7d50..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
+++ /dev/null
@@ -1,2025 +0,0 @@
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/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
deleted file mode 100644
index d32188e..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
+++ /dev/null
@@ -1,90 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Threading;
33using System.IO;
34using System.Diagnostics;
35using log4net;
36using Nini.Config;
37using OdeAPI;
38using OpenSim.Framework;
39using OpenSim.Region.Physics.Manager;
40using OpenMetaverse;
41
42namespace OpenSim.Region.Physics.OdePlugin
43{
44 /// <summary>
45 /// ODE plugin
46 /// </summary>
47 public class OdePlugin : IPhysicsPlugin
48 {
49 //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
50
51 private OdeScene m_scene;
52
53 public bool Init()
54 {
55 if (Util.IsWindows())
56 Util.LoadArchSpecificWindowsDll("ode.dll");
57
58 if (d.InitODE2(0) != 0)
59 {
60 if (d.AllocateODEDataForThread(~0U) == 0)
61 {
62 d.CloseODE();
63 return false;
64 }
65 return true;
66 }
67 return false;
68 }
69
70 public PhysicsScene GetScene(String sceneIdentifier)
71 {
72 if (m_scene == null)
73 {
74
75 m_scene = new OdeScene(sceneIdentifier);
76 }
77 return (m_scene);
78 }
79
80 public string GetName()
81 {
82 return ("UbitODE");
83 }
84
85 public void Dispose()
86 {
87 d.CloseODE();
88 }
89 }
90} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
deleted file mode 100644
index e8c219c..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ /dev/null
@@ -1,2869 +0,0 @@
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.Physics.Manager;
43using OpenMetaverse;
44
45namespace OpenSim.Region.Physics.OdePlugin
46{
47 // colision flags of things others can colide with
48 // rays, sensors, probes removed since can't be colided with
49 // The top space where things are placed provided further selection
50 // ie physical are in active space nonphysical in static
51 // this should be exclusive as possible
52
53 [Flags]
54 public enum CollisionCategories : uint
55 {
56 Disabled = 0,
57 //by 'things' types
58 Space = 0x01,
59 Geom = 0x02, // aka prim/part
60 Character = 0x04,
61 Land = 0x08,
62 Water = 0x010,
63
64 // by state
65 Phantom = 0x01000,
66 VolumeDtc = 0x02000,
67 Selected = 0x04000,
68 NoShape = 0x08000,
69
70
71 All = 0xffffffff
72 }
73
74 /// <summary>
75 /// Material type for a primitive
76 /// </summary>
77 public enum Material : int
78 {
79 /// <summary></summary>
80 Stone = 0,
81 /// <summary></summary>
82 Metal = 1,
83 /// <summary></summary>
84 Glass = 2,
85 /// <summary></summary>
86 Wood = 3,
87 /// <summary></summary>
88 Flesh = 4,
89 /// <summary></summary>
90 Plastic = 5,
91 /// <summary></summary>
92 Rubber = 6,
93
94 light = 7 // compatibility with old viewers
95 }
96
97 public enum changes : int
98 {
99 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
100 Remove,
101 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
102 // or removes from a object if arg is null
103 DeLink,
104 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
105 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
106 PosOffset, // not in use
107 // arg Vector3 new position in local coords. Changes prim position in object
108 OriOffset, // not in use
109 // arg Vector3 new position in local coords. Changes prim position in object
110 Velocity,
111 AngVelocity,
112 Acceleration,
113 Force,
114 Torque,
115 Momentum,
116
117 AddForce,
118 AddAngForce,
119 AngLock,
120
121 Buoyancy,
122
123 PIDTarget,
124 PIDTau,
125 PIDActive,
126
127 PIDHoverHeight,
128 PIDHoverType,
129 PIDHoverTau,
130 PIDHoverActive,
131
132 Size,
133 AvatarSize,
134 Shape,
135 PhysRepData,
136 AddPhysRep,
137
138 CollidesWater,
139 VolumeDtc,
140
141 Physical,
142 Phantom,
143 Selected,
144 disabled,
145 building,
146
147 VehicleType,
148 VehicleFloatParam,
149 VehicleVectorParam,
150 VehicleRotationParam,
151 VehicleFlags,
152 SetVehicle,
153
154 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
155 }
156
157 public struct ODEchangeitem
158 {
159 public PhysicsActor actor;
160 public OdeCharacter character;
161 public changes what;
162 public Object arg;
163 }
164
165
166
167 public class OdeScene : PhysicsScene
168 {
169 private readonly ILog m_log;
170 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
171
172 public bool OdeUbitLib = false;
173 public bool m_suportCombine = false; // mega suport not tested
174
175// private int threadid = 0;
176// private Random fluidRandomizer = new Random(Environment.TickCount);
177
178// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
179
180 const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
181 const float comumContactERP = 0.7f;
182 const float comumContactCFM = 0.0001f;
183 const float comumContactSLIP = 0f;
184
185 float frictionMovementMult = 0.8f;
186
187 float TerrainBounce = 0.1f;
188 float TerrainFriction = 0.3f;
189
190 public float AvatarFriction = 0;// 0.9f * 0.5f;
191
192 // this netx dimensions are only relevant for terrain partition (mega regions)
193 // WorldExtents below has the simulation dimensions
194 // they should be identical except on mega regions
195 private uint m_regionWidth = Constants.RegionSize;
196 private uint m_regionHeight = Constants.RegionSize;
197
198 public float ODE_STEPSIZE = 0.020f;
199 public float HalfOdeStep = 0.01f;
200 public int odetimestepMS = 20; // rounded
201 private float metersInSpace = 25.6f;
202 private float m_timeDilation = 1.0f;
203
204 private DateTime m_lastframe;
205 private DateTime m_lastMeshExpire;
206
207 public float gravityx = 0f;
208 public float gravityy = 0f;
209 public float gravityz = -9.8f;
210
211 private float waterlevel = 0f;
212 private int framecount = 0;
213
214// private int m_meshExpireCntr;
215
216 private float avDensity = 3f;
217 private float avMovementDivisorWalk = 1.3f;
218 private float avMovementDivisorRun = 0.8f;
219 private float minimumGroundFlightOffset = 3f;
220 public float maximumMassObject = 10000.01f;
221
222
223 public float geomDefaultDensity = 10.000006836f;
224
225 public float bodyPIDD = 35f;
226 public float bodyPIDG = 25;
227
228 public int bodyFramesAutoDisable = 5;
229
230 private d.NearCallback nearCallback;
231
232 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
233 private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
234 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
235 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
236
237 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
238
239 /// <summary>
240 /// A list of actors that should receive collision events.
241 /// </summary>
242 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
243 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
244
245 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
246// public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
247 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
248
249 private float contactsurfacelayer = 0.001f;
250
251 private int contactsPerCollision = 80;
252 internal IntPtr ContactgeomsArray = IntPtr.Zero;
253 private IntPtr GlobalContactsArray = IntPtr.Zero;
254 private d.Contact SharedTmpcontact = new d.Contact();
255
256 const int maxContactsbeforedeath = 4000;
257 private volatile int m_global_contactcount = 0;
258
259 private IntPtr contactgroup;
260
261 public ContactData[] m_materialContactsData = new ContactData[8];
262
263 private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
264 private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
265 private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
266
267 private int m_physicsiterations = 15;
268 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
269// private PhysicsActor PANull = new NullPhysicsActor();
270 private float step_time = 0.0f;
271
272 public IntPtr world;
273
274
275 // split the spaces acording to contents type
276 // ActiveSpace contains characters and active prims
277 // StaticSpace contains land and other that is mostly static in enviroment
278 // this can contain subspaces, like the grid in staticspace
279 // as now space only contains this 2 top spaces
280
281 public IntPtr TopSpace; // the global space
282 public IntPtr ActiveSpace; // space for active prims
283 public IntPtr CharsSpace; // space for active prims
284 public IntPtr StaticSpace; // space for the static things around
285 public IntPtr GroundSpace; // space for ground
286
287 // some speedup variables
288 private int spaceGridMaxX;
289 private int spaceGridMaxY;
290 private float spacesPerMeterX;
291 private float spacesPerMeterY;
292
293 // split static geometry collision into a grid as before
294 private IntPtr[,] staticPrimspace;
295 private IntPtr[] staticPrimspaceOffRegion;
296
297 public Object OdeLock;
298 public static Object SimulationLock;
299
300 public IMesher mesher;
301
302 private IConfigSource m_config;
303
304 public bool physics_logging = false;
305 public int physics_logging_interval = 0;
306 public bool physics_logging_append_existing_logfile = false;
307
308 private Vector3 m_worldOffset = Vector3.Zero;
309 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
310 private PhysicsScene m_parentScene = null;
311
312 private ODERayCastRequestManager m_rayCastManager;
313 public ODEMeshWorker m_meshWorker;
314
315/* maybe needed if ode uses tls
316 private void checkThread()
317 {
318
319 int th = Thread.CurrentThread.ManagedThreadId;
320 if(th != threadid)
321 {
322 threadid = th;
323 d.AllocateODEDataForThread(~0U);
324 }
325 }
326 */
327 /// <summary>
328 /// Initiailizes the scene
329 /// Sets many properties that ODE requires to be stable
330 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
331 /// </summary>
332 public OdeScene(string sceneIdentifier)
333 {
334 m_log
335 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
336
337// checkThread();
338 Name = sceneIdentifier;
339
340 OdeLock = new Object();
341 SimulationLock = new Object();
342
343 nearCallback = near;
344
345 m_rayCastManager = new ODERayCastRequestManager(this);
346
347 lock (OdeLock)
348 {
349 // Create the world and the first space
350 try
351 {
352 world = d.WorldCreate();
353 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
354
355 // now the major subspaces
356 ActiveSpace = d.HashSpaceCreate(TopSpace);
357 CharsSpace = d.HashSpaceCreate(TopSpace);
358 StaticSpace = d.HashSpaceCreate(TopSpace);
359 GroundSpace = d.HashSpaceCreate(TopSpace);
360 }
361 catch
362 {
363 // i must RtC#FM
364 // i did!
365 }
366
367 d.HashSpaceSetLevels(TopSpace, -2, 8);
368 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
369 d.HashSpaceSetLevels(CharsSpace, -4, 3);
370 d.HashSpaceSetLevels(StaticSpace, -2, 8);
371 d.HashSpaceSetLevels(GroundSpace, 0, 8);
372
373 // demote to second level
374 d.SpaceSetSublevel(ActiveSpace, 1);
375 d.SpaceSetSublevel(CharsSpace, 1);
376 d.SpaceSetSublevel(StaticSpace, 1);
377 d.SpaceSetSublevel(GroundSpace, 1);
378
379 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
380 CollisionCategories.Geom |
381 CollisionCategories.Character |
382 CollisionCategories.Phantom |
383 CollisionCategories.VolumeDtc
384 ));
385 d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
386 CollisionCategories.Geom |
387 CollisionCategories.Character |
388 CollisionCategories.Phantom |
389 CollisionCategories.VolumeDtc
390 ));
391 d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
392 CollisionCategories.Geom |
393 CollisionCategories.Character |
394 CollisionCategories.Phantom |
395 CollisionCategories.VolumeDtc
396 ));
397 d.GeomSetCollideBits(CharsSpace, 0);
398
399 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
400 CollisionCategories.Geom |
401// CollisionCategories.Land |
402// CollisionCategories.Water |
403 CollisionCategories.Phantom |
404 CollisionCategories.VolumeDtc
405 ));
406 d.GeomSetCollideBits(StaticSpace, 0);
407
408 d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
409 d.GeomSetCollideBits(GroundSpace, 0);
410
411 contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1);
412 //contactgroup
413
414 d.WorldSetAutoDisableFlag(world, false);
415 }
416 }
417
418 public override void Initialise(IMesher meshmerizer, IConfigSource config, Vector3 regionExtent)
419 {
420 WorldExtents.X = regionExtent.X;
421 m_regionWidth = (uint)regionExtent.X;
422 WorldExtents.Y = regionExtent.Y;
423 m_regionHeight = (uint)regionExtent.Y;
424
425 m_suportCombine = false;
426 Initialise(meshmerizer, config);
427 }
428
429
430 public override void Initialise(IMesher meshmerizer, IConfigSource config)
431 {
432// checkThread();
433 mesher = meshmerizer;
434 m_config = config;
435
436 string ode_config = d.GetConfiguration();
437 if (ode_config != null && ode_config != "")
438 {
439 m_log.WarnFormat("ODE configuration: {0}", ode_config);
440
441 if (ode_config.Contains("ODE_Ubit"))
442 {
443 OdeUbitLib = true;
444 }
445 }
446
447 // Defaults
448
449 int contactsPerCollision = 80;
450
451 IConfig physicsconfig = null;
452
453 if (m_config != null)
454 {
455 physicsconfig = m_config.Configs["ODEPhysicsSettings"];
456 if (physicsconfig != null)
457 {
458 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
459 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
460 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
461
462 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
463
464// contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
465
466 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
467
468 avDensity = physicsconfig.GetFloat("av_density", avDensity);
469 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
470 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
471
472 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
473
474 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
475 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
476
477 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
478 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
479 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
480
481 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
482 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
483 }
484 }
485
486
487 d.WorldSetCFM(world, comumContactCFM);
488 d.WorldSetERP(world, comumContactERP);
489
490 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
491
492 d.WorldSetLinearDamping(world, 0.002f);
493 d.WorldSetAngularDamping(world, 0.002f);
494 d.WorldSetAngularDampingThreshold(world, 0f);
495 d.WorldSetLinearDampingThreshold(world, 0f);
496 d.WorldSetMaxAngularSpeed(world, 100f);
497
498 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
499
500 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
501 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
502
503 m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig);
504
505 HalfOdeStep = ODE_STEPSIZE * 0.5f;
506 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
507
508 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
509 GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
510
511 SharedTmpcontact.geom.g1 = IntPtr.Zero;
512 SharedTmpcontact.geom.g2 = IntPtr.Zero;
513
514 SharedTmpcontact.geom.side1 = -1;
515 SharedTmpcontact.geom.side2 = -1;
516
517 SharedTmpcontact.surface.mode = comumContactFlags;
518 SharedTmpcontact.surface.mu = 0;
519 SharedTmpcontact.surface.bounce = 0;
520 SharedTmpcontact.surface.soft_cfm = comumContactCFM;
521 SharedTmpcontact.surface.soft_erp = comumContactERP;
522 SharedTmpcontact.surface.slip1 = comumContactSLIP;
523 SharedTmpcontact.surface.slip2 = comumContactSLIP;
524
525 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
526 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
527
528 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
529 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
530
531 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
532 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
533
534 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
535 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
536
537 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
538 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
539
540 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
541 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
542
543 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
544 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
545
546 m_materialContactsData[(int)Material.light].mu = 0.0f;
547 m_materialContactsData[(int)Material.light].bounce = 0.0f;
548
549
550 spacesPerMeterX = 1.0f / metersInSpace;
551 spacesPerMeterY = spacesPerMeterX;
552 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
553 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
554
555 if (spaceGridMaxX > 24)
556 {
557 spaceGridMaxX = 24;
558 spacesPerMeterX = spaceGridMaxX / WorldExtents.X ;
559 }
560
561 if (spaceGridMaxY > 24)
562 {
563 spaceGridMaxY = 24;
564 spacesPerMeterY = spaceGridMaxY / WorldExtents.Y ;
565 }
566
567 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
568
569 // create all spaces now
570 int i, j;
571 IntPtr newspace;
572
573 for (i = 0; i < spaceGridMaxX; i++)
574 for (j = 0; j < spaceGridMaxY; j++)
575 {
576 newspace = d.HashSpaceCreate(StaticSpace);
577 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
578 waitForSpaceUnlock(newspace);
579 d.SpaceSetSublevel(newspace, 2);
580 d.HashSpaceSetLevels(newspace, -2, 8);
581 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
582 CollisionCategories.Geom |
583 CollisionCategories.Land |
584 CollisionCategories.Water |
585 CollisionCategories.Phantom |
586 CollisionCategories.VolumeDtc
587 ));
588 d.GeomSetCollideBits(newspace, 0);
589
590 staticPrimspace[i, j] = newspace;
591 }
592
593 // let this now be index limit
594 spaceGridMaxX--;
595 spaceGridMaxY--;
596
597 // create 4 off world spaces (x<0,x>max,y<0,y>max)
598 staticPrimspaceOffRegion = new IntPtr[4];
599
600 for (i = 0; i < 4; i++)
601 {
602 newspace = d.HashSpaceCreate(StaticSpace);
603 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
604 waitForSpaceUnlock(newspace);
605 d.SpaceSetSublevel(newspace, 2);
606 d.HashSpaceSetLevels(newspace, -2, 8);
607 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
608 CollisionCategories.Geom |
609 CollisionCategories.Land |
610 CollisionCategories.Water |
611 CollisionCategories.Phantom |
612 CollisionCategories.VolumeDtc
613 ));
614 d.GeomSetCollideBits(newspace, 0);
615
616 staticPrimspaceOffRegion[i] = newspace;
617 }
618
619 m_lastframe = DateTime.UtcNow;
620 m_lastMeshExpire = m_lastframe;
621 }
622
623 internal void waitForSpaceUnlock(IntPtr space)
624 {
625 //if (space != IntPtr.Zero)
626 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
627 }
628
629 #region Collision Detection
630
631 // sets a global contact for a joint for contactgeom , and base contact description)
632
633
634
635 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom)
636 {
637 if (m_global_contactcount >= maxContactsbeforedeath)
638 return IntPtr.Zero;
639
640 m_global_contactcount++;
641
642 SharedTmpcontact.geom.depth = contactGeom.depth;
643 SharedTmpcontact.geom.pos = contactGeom.pos;
644 SharedTmpcontact.geom.normal = contactGeom.normal;
645
646 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
647 Marshal.StructureToPtr(SharedTmpcontact, contact, true);
648 return d.JointCreateContactPtr(world, contactgroup, contact);
649 }
650
651 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
652 {
653 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
654 return false;
655
656 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
657 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
658 return true;
659 }
660
661 /// <summary>
662 /// This is our near callback. A geometry is near a body
663 /// </summary>
664 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
665 /// <param name="g1">a geometry or space</param>
666 /// <param name="g2">another geometry or space</param>
667 ///
668
669 private void near(IntPtr space, IntPtr g1, IntPtr g2)
670 {
671 // no lock here! It's invoked from within Simulate(), which is thread-locked
672
673 if (m_global_contactcount >= maxContactsbeforedeath)
674 return;
675
676 // Test if we're colliding a geom with a space.
677 // If so we have to drill down into the space recursively
678
679 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
680 return;
681
682 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
683 {
684 // We'll be calling near recursivly if one
685 // of them is a space to find all of the
686 // contact points in the space
687 try
688 {
689 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
690 }
691 catch (AccessViolationException)
692 {
693 m_log.Warn("[PHYSICS]: Unable to collide test a space");
694 return;
695 }
696 //here one should check collisions of geoms inside a space
697 // but on each space we only should have geoms that not colide amoung each other
698 // so we don't dig inside spaces
699 return;
700 }
701
702 // get geom bodies to check if we already a joint contact
703 // guess this shouldn't happen now
704 IntPtr b1 = d.GeomGetBody(g1);
705 IntPtr b2 = d.GeomGetBody(g2);
706
707 // d.GeomClassID id = d.GeomGetClass(g1);
708
709 // Figure out how many contact points we have
710 int count = 0;
711 try
712 {
713 // Colliding Geom To Geom
714 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
715
716 if (g1 == g2)
717 return; // Can't collide with yourself
718
719 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
720 return;
721 /*
722 // debug
723 PhysicsActor dp2;
724 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
725 {
726 d.AABB aabb;
727 d.GeomGetAABB(g2, out aabb);
728 float x = aabb.MaxX - aabb.MinX;
729 float y = aabb.MaxY - aabb.MinY;
730 float z = aabb.MaxZ - aabb.MinZ;
731 if (x > 60.0f || y > 60.0f || z > 60.0f)
732 {
733 if (!actor_name_map.TryGetValue(g2, out dp2))
734 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
735 else
736 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
737 dp2.Name, dp2.Size, x, y, z,
738 dp2.Position.ToString(),
739 dp2.Orientation.ToString(),
740 dp2.Orientation.Length());
741 return;
742 }
743 }
744 //
745 */
746
747
748 if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
749 d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
750 {
751 int cflags;
752 unchecked
753 {
754 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
755 }
756 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
757 }
758 else
759 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
760 }
761 catch (SEHException)
762 {
763 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.");
764 // ode.drelease(world);
765 base.TriggerPhysicsBasedRestart();
766 }
767 catch (Exception e)
768 {
769 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
770 return;
771 }
772
773 // contacts done
774 if (count == 0)
775 return;
776
777 // try get physical actors
778 PhysicsActor p1;
779 PhysicsActor p2;
780
781 if (!actor_name_map.TryGetValue(g1, out p1))
782 {
783 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
784 return;
785 }
786
787 if (!actor_name_map.TryGetValue(g2, out p2))
788 {
789 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
790 return;
791 }
792
793 // update actors collision score
794 if (p1.CollisionScore >= float.MaxValue - count)
795 p1.CollisionScore = 0;
796 p1.CollisionScore += count;
797
798 if (p2.CollisionScore >= float.MaxValue - count)
799 p2.CollisionScore = 0;
800 p2.CollisionScore += count;
801
802 // get first contact
803 d.ContactGeom curContact = new d.ContactGeom();
804
805 if (!GetCurContactGeom(0, ref curContact))
806 return;
807
808 ContactPoint maxDepthContact = new ContactPoint();
809
810 // do volume detection case
811 if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
812 {
813 maxDepthContact = new ContactPoint(
814 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
815 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
816 curContact.depth, false
817 );
818
819 collision_accounting_events(p1, p2, maxDepthContact);
820 return;
821 }
822
823 // big messy collision analises
824
825 float mu = 0;
826 float bounce = 0;
827// bool IgnoreNegSides = false;
828
829 ContactData contactdata1 = new ContactData(0, 0, false);
830 ContactData contactdata2 = new ContactData(0, 0, false);
831
832 bool dop1ava = false;
833 bool dop2ava = false;
834 bool ignore = false;
835
836 switch (p1.PhysicsActorType)
837 {
838 case (int)ActorTypes.Agent:
839 {
840 dop1ava = true;
841 switch (p2.PhysicsActorType)
842 {
843 case (int)ActorTypes.Agent:
844 case (int)ActorTypes.Prim:
845 break;
846
847 default:
848 ignore = true; // avatar to terrain and water ignored
849 break;
850 }
851 break;
852 }
853
854 case (int)ActorTypes.Prim:
855 {
856 switch (p2.PhysicsActorType)
857 {
858 case (int)ActorTypes.Agent:
859 dop2ava = true;
860 break;
861
862 case (int)ActorTypes.Prim:
863 Vector3 relV = p1.Velocity - p2.Velocity;
864 float relVlenSQ = relV.LengthSquared();
865 if (relVlenSQ > 0.0001f)
866 {
867 p1.CollidingObj = true;
868 p2.CollidingObj = true;
869 }
870 p1.getContactData(ref contactdata1);
871 p2.getContactData(ref contactdata2);
872 bounce = contactdata1.bounce * contactdata2.bounce;
873 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
874
875 if (relVlenSQ > 0.01f)
876 mu *= frictionMovementMult;
877
878 break;
879
880 case (int)ActorTypes.Ground:
881 p1.getContactData(ref contactdata1);
882 bounce = contactdata1.bounce * TerrainBounce;
883 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
884
885 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
886 mu *= frictionMovementMult;
887 p1.CollidingGround = true;
888 /*
889 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
890 {
891 if (curContact.side1 > 0)
892 IgnoreNegSides = true;
893 }
894 */
895 break;
896
897 case (int)ActorTypes.Water:
898 default:
899 ignore = true;
900 break;
901 }
902 }
903 break;
904
905 case (int)ActorTypes.Ground:
906 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
907 {
908 p2.CollidingGround = true;
909 p2.getContactData(ref contactdata2);
910 bounce = contactdata2.bounce * TerrainBounce;
911 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
912
913// if (curContact.side1 > 0) // should be 2 ?
914// IgnoreNegSides = true;
915
916 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
917 mu *= frictionMovementMult;
918 }
919 else
920 ignore = true;
921 break;
922
923 case (int)ActorTypes.Water:
924 default:
925 break;
926 }
927
928 if (ignore)
929 return;
930
931 IntPtr Joint;
932 bool FeetCollision = false;
933 int ncontacts = 0;
934
935 int i = 0;
936
937 maxDepthContact = new ContactPoint();
938 maxDepthContact.PenetrationDepth = float.MinValue;
939 ContactPoint minDepthContact = new ContactPoint();
940 minDepthContact.PenetrationDepth = float.MaxValue;
941
942 SharedTmpcontact.geom.depth = 0;
943 SharedTmpcontact.surface.mu = mu;
944 SharedTmpcontact.surface.bounce = bounce;
945
946 d.ContactGeom altContact = new d.ContactGeom();
947 bool useAltcontact = false;
948 bool noskip = true;
949
950 while (true)
951 {
952// if (!(IgnoreNegSides && curContact.side1 < 0))
953 {
954 noskip = true;
955 useAltcontact = false;
956
957 if (dop1ava)
958 {
959 if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
960 {
961 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
962 {
963 p1.CollidingObj = true;
964 p2.CollidingObj = true;
965 }
966 else if (p2.Velocity.LengthSquared() > 0.0f)
967 p2.CollidingObj = true;
968 }
969 else
970 noskip = false;
971 }
972 else if (dop2ava)
973 {
974 if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
975 {
976 if (p1.PhysicsActorType == (int)ActorTypes.Agent)
977 {
978 p1.CollidingObj = true;
979 p2.CollidingObj = true;
980 }
981 else if (p2.Velocity.LengthSquared() > 0.0f)
982 p1.CollidingObj = true;
983 }
984 else
985 noskip = false;
986 }
987
988 if (noskip)
989 {
990 if(useAltcontact)
991 Joint = CreateContacJoint(ref altContact);
992 else
993 Joint = CreateContacJoint(ref curContact);
994
995 if (Joint == IntPtr.Zero)
996 break;
997
998 d.JointAttach(Joint, b1, b2);
999
1000 ncontacts++;
1001
1002 if (curContact.depth > maxDepthContact.PenetrationDepth)
1003 {
1004 maxDepthContact.Position.X = curContact.pos.X;
1005 maxDepthContact.Position.Y = curContact.pos.Y;
1006 maxDepthContact.Position.Z = curContact.pos.Z;
1007 maxDepthContact.PenetrationDepth = curContact.depth;
1008 maxDepthContact.CharacterFeet = FeetCollision;
1009 }
1010
1011 if (curContact.depth < minDepthContact.PenetrationDepth)
1012 {
1013 minDepthContact.PenetrationDepth = curContact.depth;
1014 minDepthContact.SurfaceNormal.X = curContact.normal.X;
1015 minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1016 minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1017 }
1018 }
1019 }
1020 if (++i >= count)
1021 break;
1022
1023 if (!GetCurContactGeom(i, ref curContact))
1024 break;
1025 }
1026
1027 if (ncontacts > 0)
1028 {
1029 maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
1030 maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
1031 maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
1032
1033 collision_accounting_events(p1, p2, maxDepthContact);
1034 }
1035 }
1036
1037 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1038 {
1039 uint obj2LocalID = 0;
1040
1041 bool p1events = p1.SubscribedEvents();
1042 bool p2events = p2.SubscribedEvents();
1043
1044 if (p1.IsVolumeDtc)
1045 p2events = false;
1046 if (p2.IsVolumeDtc)
1047 p1events = false;
1048
1049 if (!p2events && !p1events)
1050 return;
1051
1052 Vector3 vel = Vector3.Zero;
1053 if (p2 != null && p2.IsPhysical)
1054 vel = p2.Velocity;
1055
1056 if (p1 != null && p1.IsPhysical)
1057 vel -= p1.Velocity;
1058
1059 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1060
1061 switch ((ActorTypes)p1.PhysicsActorType)
1062 {
1063 case ActorTypes.Agent:
1064 case ActorTypes.Prim:
1065 {
1066 switch ((ActorTypes)p2.PhysicsActorType)
1067 {
1068 case ActorTypes.Agent:
1069 case ActorTypes.Prim:
1070 if (p2events)
1071 {
1072 AddCollisionEventReporting(p2);
1073 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
1074 }
1075 obj2LocalID = p2.ParentActor.LocalID;
1076 break;
1077
1078 case ActorTypes.Ground:
1079 case ActorTypes.Unknown:
1080 default:
1081 obj2LocalID = 0;
1082 break;
1083 }
1084 if (p1events)
1085 {
1086 contact.SurfaceNormal = -contact.SurfaceNormal;
1087 AddCollisionEventReporting(p1);
1088 p1.AddCollisionEvent(obj2LocalID, contact);
1089 }
1090 break;
1091 }
1092 case ActorTypes.Ground:
1093 case ActorTypes.Unknown:
1094 default:
1095 {
1096 if (p2events && !p2.IsVolumeDtc)
1097 {
1098 AddCollisionEventReporting(p2);
1099 p2.AddCollisionEvent(0, contact);
1100 }
1101 break;
1102 }
1103 }
1104 }
1105
1106 /// <summary>
1107 /// This is our collision testing routine in ODE
1108 /// </summary>
1109 /// <param name="timeStep"></param>
1110 private void collision_optimized()
1111 {
1112 lock (_characters)
1113 {
1114 try
1115 {
1116 foreach (OdeCharacter chr in _characters)
1117 {
1118 if (chr == null || chr.Body == IntPtr.Zero)
1119 continue;
1120
1121 chr.IsColliding = false;
1122 // chr.CollidingGround = false; not done here
1123 chr.CollidingObj = false;
1124 // do colisions with static space
1125 d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
1126
1127 // no coll with gnd
1128 }
1129 // chars with chars
1130 d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
1131
1132 }
1133 catch (AccessViolationException)
1134 {
1135 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1136 }
1137
1138 }
1139
1140 lock (_activeprims)
1141 {
1142 foreach (OdePrim aprim in _activeprims)
1143 {
1144 aprim.CollisionScore = 0;
1145 aprim.IsColliding = false;
1146 }
1147 }
1148
1149 // collide active prims with static enviroment
1150 lock (_activegroups)
1151 {
1152 try
1153 {
1154 foreach (OdePrim prm in _activegroups)
1155 {
1156 if (!prm.m_outbounds)
1157 {
1158 if (d.BodyIsEnabled(prm.Body))
1159 {
1160 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1161 d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1162 }
1163 }
1164 }
1165 }
1166 catch (AccessViolationException)
1167 {
1168 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1169 }
1170 }
1171 // colide active amoung them
1172 try
1173 {
1174 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1175 }
1176 catch (AccessViolationException)
1177 {
1178 m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space");
1179 }
1180 // and with chars
1181 try
1182 {
1183 d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
1184 }
1185 catch (AccessViolationException)
1186 {
1187 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1188 }
1189 // _perloopContact.Clear();
1190 }
1191
1192 #endregion
1193 /// <summary>
1194 /// Add actor to the list that should receive collision events in the simulate loop.
1195 /// </summary>
1196 /// <param name="obj"></param>
1197 public void AddCollisionEventReporting(PhysicsActor obj)
1198 {
1199 if (!_collisionEventPrim.Contains(obj))
1200 _collisionEventPrim.Add(obj);
1201 }
1202
1203 /// <summary>
1204 /// Remove actor from the list that should receive collision events in the simulate loop.
1205 /// </summary>
1206 /// <param name="obj"></param>
1207 public void RemoveCollisionEventReporting(PhysicsActor obj)
1208 {
1209 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1210 _collisionEventPrimRemove.Add(obj);
1211 }
1212
1213 public override float TimeDilation
1214 {
1215 get { return m_timeDilation; }
1216 }
1217
1218 public override bool SupportsNINJAJoints
1219 {
1220 get { return false; }
1221 }
1222
1223 #region Add/Remove Entities
1224
1225 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
1226 {
1227 return null;
1228 }
1229
1230 public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
1231 {
1232 OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
1233 size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1234 newAv.Flying = isFlying;
1235 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1236
1237 return newAv;
1238 }
1239
1240 public void AddCharacter(OdeCharacter chr)
1241 {
1242 lock (_characters)
1243 {
1244 if (!_characters.Contains(chr))
1245 {
1246 _characters.Add(chr);
1247 if (chr.bad)
1248 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1249 }
1250 }
1251 }
1252
1253 public void RemoveCharacter(OdeCharacter chr)
1254 {
1255 lock (_characters)
1256 {
1257 if (_characters.Contains(chr))
1258 {
1259 _characters.Remove(chr);
1260 }
1261 }
1262 }
1263
1264 public void BadCharacter(OdeCharacter chr)
1265 {
1266 lock (_badCharacter)
1267 {
1268 if (!_badCharacter.Contains(chr))
1269 _badCharacter.Add(chr);
1270 }
1271 }
1272
1273 public override void RemoveAvatar(PhysicsActor actor)
1274 {
1275 //m_log.Debug("[PHYSICS]:ODELOCK");
1276 ((OdeCharacter) actor).Destroy();
1277 }
1278
1279
1280 public void addActivePrim(OdePrim activatePrim)
1281 {
1282 // adds active prim..
1283 lock (_activeprims)
1284 {
1285 if (!_activeprims.Contains(activatePrim))
1286 _activeprims.Add(activatePrim);
1287 }
1288 }
1289
1290 public void addActiveGroups(OdePrim activatePrim)
1291 {
1292 lock (_activegroups)
1293 {
1294 if (!_activegroups.Contains(activatePrim))
1295 _activegroups.Add(activatePrim);
1296 }
1297 }
1298
1299 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1300 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1301 {
1302 OdePrim newPrim;
1303 lock (OdeLock)
1304 {
1305 newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
1306 lock (_prims)
1307 _prims.Add(newPrim);
1308 }
1309 return newPrim;
1310 }
1311
1312 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1313 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1314 {
1315 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
1316 }
1317
1318
1319 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1320 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1321 {
1322 return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
1323 }
1324
1325 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1326 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1327 {
1328
1329 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1330 }
1331
1332 public void remActivePrim(OdePrim deactivatePrim)
1333 {
1334 lock (_activeprims)
1335 {
1336 _activeprims.Remove(deactivatePrim);
1337 }
1338 }
1339 public void remActiveGroup(OdePrim deactivatePrim)
1340 {
1341 lock (_activegroups)
1342 {
1343 _activegroups.Remove(deactivatePrim);
1344 }
1345 }
1346
1347 public override void RemovePrim(PhysicsActor prim)
1348 {
1349 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1350 // removed in the next physics simulate pass.
1351 if (prim is OdePrim)
1352 {
1353// lock (OdeLock)
1354 {
1355
1356 OdePrim p = (OdePrim)prim;
1357 p.setPrimForRemoval();
1358 }
1359 }
1360 }
1361
1362 public void RemovePrimThreadLocked(OdePrim prim)
1363 {
1364 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1365 lock (prim)
1366 {
1367// RemoveCollisionEventReporting(prim);
1368 lock (_prims)
1369 _prims.Remove(prim);
1370 }
1371
1372 }
1373
1374 public bool havePrim(OdePrim prm)
1375 {
1376 lock (_prims)
1377 return _prims.Contains(prm);
1378 }
1379
1380 public bool haveActor(PhysicsActor actor)
1381 {
1382 if (actor is OdePrim)
1383 {
1384 lock (_prims)
1385 return _prims.Contains((OdePrim)actor);
1386 }
1387 else if (actor is OdeCharacter)
1388 {
1389 lock (_characters)
1390 return _characters.Contains((OdeCharacter)actor);
1391 }
1392 return false;
1393 }
1394
1395 #endregion
1396
1397 #region Space Separation Calculation
1398
1399 /// <summary>
1400 /// Called when a static prim moves or becomes static
1401 /// Places the prim in a space one the static sub-spaces grid
1402 /// </summary>
1403 /// <param name="geom">the pointer to the geom that moved</param>
1404 /// <param name="pos">the position that the geom moved to</param>
1405 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1406 /// <returns>a pointer to the new space it's in</returns>
1407 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1408 {
1409 // moves a prim into another static sub-space or from another space into a static sub-space
1410
1411 // Called ODEPrim so
1412 // it's already in locked space.
1413
1414 if (geom == IntPtr.Zero) // shouldn't happen
1415 return IntPtr.Zero;
1416
1417 // get the static sub-space for current position
1418 IntPtr newspace = calculateSpaceForGeom(pos);
1419
1420 if (newspace == currentspace) // if we are there all done
1421 return newspace;
1422
1423 // else remove it from its current space
1424 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1425 {
1426 if (d.GeomIsSpace(currentspace))
1427 {
1428 waitForSpaceUnlock(currentspace);
1429 d.SpaceRemove(currentspace, geom);
1430
1431 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1432 {
1433 d.SpaceDestroy(currentspace);
1434 }
1435 }
1436 else
1437 {
1438 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1439 " Geom:" + geom);
1440 }
1441 }
1442 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1443 {
1444 currentspace = d.GeomGetSpace(geom);
1445 if (currentspace != IntPtr.Zero)
1446 {
1447 if (d.GeomIsSpace(currentspace))
1448 {
1449 waitForSpaceUnlock(currentspace);
1450 d.SpaceRemove(currentspace, geom);
1451
1452 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1453 {
1454 d.SpaceDestroy(currentspace);
1455 }
1456
1457 }
1458 }
1459 }
1460
1461 // put the geom in the newspace
1462 waitForSpaceUnlock(newspace);
1463 d.SpaceAdd(newspace, geom);
1464
1465 // let caller know this newspace
1466 return newspace;
1467 }
1468
1469 /// <summary>
1470 /// Calculates the space the prim should be in by its position
1471 /// </summary>
1472 /// <param name="pos"></param>
1473 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1474 public IntPtr calculateSpaceForGeom(Vector3 pos)
1475 {
1476 int x, y;
1477
1478 if (pos.X < 0)
1479 return staticPrimspaceOffRegion[0];
1480
1481 if (pos.Y < 0)
1482 return staticPrimspaceOffRegion[2];
1483
1484 x = (int)(pos.X * spacesPerMeterX);
1485 if (x > spaceGridMaxX)
1486 return staticPrimspaceOffRegion[1];
1487
1488 y = (int)(pos.Y * spacesPerMeterY);
1489 if (y > spaceGridMaxY)
1490 return staticPrimspaceOffRegion[3];
1491
1492 return staticPrimspace[x, y];
1493 }
1494
1495 #endregion
1496
1497
1498 /// <summary>
1499 /// Called to queue a change to a actor
1500 /// to use in place of old taint mechanism so changes do have a time sequence
1501 /// </summary>
1502
1503 public void AddChange(PhysicsActor actor, changes what, Object arg)
1504 {
1505 ODEchangeitem item = new ODEchangeitem();
1506 item.actor = actor;
1507 item.what = what;
1508 item.arg = arg;
1509 ChangesQueue.Enqueue(item);
1510 }
1511
1512 /// <summary>
1513 /// Called after our prim properties are set Scale, position etc.
1514 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1515 /// This assures us that we have no race conditions
1516 /// </summary>
1517 /// <param name="prim"></param>
1518 public override void AddPhysicsActorTaint(PhysicsActor prim)
1519 {
1520 }
1521
1522 // does all pending changes generated during region load process
1523 public override void PrepareSimulation()
1524 {
1525 lock (OdeLock)
1526 {
1527 if (world == IntPtr.Zero)
1528 {
1529 ChangesQueue.Clear();
1530 return;
1531 }
1532
1533 ODEchangeitem item;
1534
1535 int donechanges = 0;
1536 if (ChangesQueue.Count > 0)
1537 {
1538 m_log.InfoFormat("[ODE] start processing pending actor operations");
1539 int tstart = Util.EnvironmentTickCount();
1540
1541 while (ChangesQueue.Dequeue(out item))
1542 {
1543 if (item.actor != null)
1544 {
1545 try
1546 {
1547 if (item.actor is OdeCharacter)
1548 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1549 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1550 RemovePrimThreadLocked((OdePrim)item.actor);
1551 }
1552 catch
1553 {
1554 m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
1555 item.actor.Name, item.what.ToString());
1556 }
1557 }
1558 donechanges++;
1559 }
1560 int time = Util.EnvironmentTickCountSubtract(tstart);
1561 m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time);
1562 }
1563 }
1564 }
1565
1566 /// <summary>
1567 /// This is our main simulate loop
1568 /// It's thread locked by a Mutex in the scene.
1569 /// It holds Collisions, it instructs ODE to step through the physical reactions
1570 /// It moves the objects around in memory
1571 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1572 /// </summary>
1573 /// <param name="timeStep"></param>
1574 /// <returns></returns>
1575 public override float Simulate(float timeStep)
1576 {
1577 DateTime now = DateTime.UtcNow;
1578 TimeSpan timedif = now - m_lastframe;
1579 timeStep = (float)timedif.TotalSeconds;
1580 m_lastframe = now;
1581
1582 // acumulate time so we can reduce error
1583 step_time += timeStep;
1584
1585 if (step_time < HalfOdeStep)
1586 return 0;
1587
1588 if (framecount < 0)
1589 framecount = 0;
1590
1591 framecount++;
1592
1593// int curphysiteractions;
1594
1595 // if in trouble reduce step resolution
1596// if (step_time >= m_SkipFramesAtms)
1597// curphysiteractions = m_physicsiterations / 2;
1598// else
1599// curphysiteractions = m_physicsiterations;
1600
1601// checkThread();
1602 int nodeframes = 0;
1603
1604 lock (SimulationLock)
1605 lock(OdeLock)
1606 {
1607 if (world == IntPtr.Zero)
1608 {
1609 ChangesQueue.Clear();
1610 return 0;
1611 }
1612
1613 ODEchangeitem item;
1614
1615// d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1616
1617 int loopstartMS = Util.EnvironmentTickCount();
1618 int looptimeMS = 0;
1619
1620
1621 while (step_time > HalfOdeStep)
1622 {
1623 try
1624 {
1625 // clear pointer/counter to contacts to pass into joints
1626 m_global_contactcount = 0;
1627
1628 if (ChangesQueue.Count > 0)
1629 {
1630 int changestartMS = Util.EnvironmentTickCount();
1631 int ttmp;
1632 while (ChangesQueue.Dequeue(out item))
1633 {
1634 if (item.actor != null)
1635 {
1636 try
1637 {
1638 if (item.actor is OdeCharacter)
1639 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1640 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1641 RemovePrimThreadLocked((OdePrim)item.actor);
1642 }
1643 catch
1644 {
1645 m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
1646 item.actor.Name, item.what.ToString());
1647 }
1648 }
1649 ttmp = Util.EnvironmentTickCountSubtract(changestartMS);
1650 if (ttmp > 20)
1651 break;
1652 }
1653 }
1654
1655 // Move characters
1656 lock (_characters)
1657 {
1658 List<OdeCharacter> defects = new List<OdeCharacter>();
1659 foreach (OdeCharacter actor in _characters)
1660 {
1661 if (actor != null)
1662 actor.Move(defects);
1663 }
1664 if (defects.Count != 0)
1665 {
1666 foreach (OdeCharacter defect in defects)
1667 {
1668 RemoveCharacter(defect);
1669 }
1670 defects.Clear();
1671 }
1672 }
1673
1674 // Move other active objects
1675 lock (_activegroups)
1676 {
1677 foreach (OdePrim aprim in _activegroups)
1678 {
1679 aprim.Move();
1680 }
1681 }
1682
1683 m_rayCastManager.ProcessQueuedRequests();
1684
1685 collision_optimized();
1686
1687 foreach (PhysicsActor obj in _collisionEventPrim)
1688 {
1689 if (obj == null)
1690 continue;
1691
1692 switch ((ActorTypes)obj.PhysicsActorType)
1693 {
1694 case ActorTypes.Agent:
1695 OdeCharacter cobj = (OdeCharacter)obj;
1696 cobj.AddCollisionFrameTime((int)(odetimestepMS));
1697 cobj.SendCollisions();
1698 break;
1699
1700 case ActorTypes.Prim:
1701 OdePrim pobj = (OdePrim)obj;
1702 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1703 if (!pobj.m_outbounds)
1704 {
1705 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1706 pobj.SendCollisions();
1707 }
1708 break;
1709 }
1710 }
1711
1712 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1713 _collisionEventPrim.Remove(obj);
1714
1715 _collisionEventPrimRemove.Clear();
1716
1717 // do a ode simulation step
1718 d.WorldQuickStep(world, ODE_STEPSIZE);
1719// d.WorldStep(world, ODE_STEPSIZE);
1720 d.JointGroupEmpty(contactgroup);
1721
1722 // update managed ideia of physical data and do updates to core
1723 /*
1724 lock (_characters)
1725 {
1726 foreach (OdeCharacter actor in _characters)
1727 {
1728 if (actor != null)
1729 {
1730 if (actor.bad)
1731 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1732
1733 actor.UpdatePositionAndVelocity();
1734 }
1735 }
1736 }
1737 */
1738
1739 lock (_activegroups)
1740 {
1741 {
1742 foreach (OdePrim actor in _activegroups)
1743 {
1744 if (actor.IsPhysical)
1745 {
1746 actor.UpdatePositionAndVelocity(framecount);
1747 }
1748 }
1749 }
1750 }
1751 }
1752 catch (Exception e)
1753 {
1754 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1755// ode.dunlock(world);
1756 }
1757
1758 step_time -= ODE_STEPSIZE;
1759 nodeframes++;
1760
1761 looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
1762 if (looptimeMS > 100)
1763 break;
1764 }
1765
1766 lock (_badCharacter)
1767 {
1768 if (_badCharacter.Count > 0)
1769 {
1770 foreach (OdeCharacter chr in _badCharacter)
1771 {
1772 RemoveCharacter(chr);
1773 }
1774
1775 _badCharacter.Clear();
1776 }
1777 }
1778
1779 timedif = now - m_lastMeshExpire;
1780
1781 if (timedif.Seconds > 10)
1782 {
1783 mesher.ExpireReleaseMeshs();
1784 m_lastMeshExpire = now;
1785 }
1786
1787// information block running in debug only
1788/*
1789 int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1790 int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1791 int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
1792
1793 int nactivegeoms = 0;
1794 int nactivespaces = 0;
1795
1796 int nstaticgeoms = 0;
1797 int nstaticspaces = 0;
1798 IntPtr sp;
1799
1800 for (int i = 0; i < ntopactivegeoms; i++)
1801 {
1802 sp = d.SpaceGetGeom(ActiveSpace, i);
1803 if (d.GeomIsSpace(sp))
1804 {
1805 nactivespaces++;
1806 nactivegeoms += d.SpaceGetNumGeoms(sp);
1807 }
1808 else
1809 nactivegeoms++;
1810 }
1811
1812 for (int i = 0; i < ntopstaticgeoms; i++)
1813 {
1814 sp = d.SpaceGetGeom(StaticSpace, i);
1815 if (d.GeomIsSpace(sp))
1816 {
1817 nstaticspaces++;
1818 nstaticgeoms += d.SpaceGetNumGeoms(sp);
1819 }
1820 else
1821 nstaticgeoms++;
1822 }
1823
1824 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1825
1826 int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
1827 int nbodies = d.NTotalBodies;
1828 int ngeoms = d.NTotalGeoms;
1829*/
1830 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1831 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1832 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1833 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1834 {
1835 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1836 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1837
1838 if (physics_logging_append_existing_logfile)
1839 {
1840 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1841 TextWriter fwriter = File.AppendText(fname);
1842 fwriter.WriteLine(header);
1843 fwriter.Close();
1844 }
1845
1846 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1847 }
1848
1849 // think time dilation as to do with dinamic step size that we dont' have
1850 // even so tell something to world
1851 if (looptimeMS < 100) // we did the requested loops
1852 m_timeDilation = 1.0f;
1853 else if (step_time > 0)
1854 {
1855 m_timeDilation = timeStep / step_time;
1856 if (m_timeDilation > 1)
1857 m_timeDilation = 1;
1858 if (step_time > m_SkipFramesAtms)
1859 step_time = 0;
1860 m_lastframe = DateTime.UtcNow; // skip also the time lost
1861 }
1862 }
1863 return (float)nodeframes * ODE_STEPSIZE / timeStep;
1864 }
1865
1866 /// <summary>
1867 public override void GetResults()
1868 {
1869 }
1870
1871 public override bool IsThreaded
1872 {
1873 // for now we won't be multithreaded
1874 get { return (false); }
1875 }
1876
1877 public float GetTerrainHeightAtXY(float x, float y)
1878 {
1879
1880 int offsetX = 0;
1881 int offsetY = 0;
1882
1883 if (m_suportCombine)
1884 {
1885 offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1886 offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1887 }
1888
1889 // get region map
1890 IntPtr heightFieldGeom = IntPtr.Zero;
1891 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
1892 return 0f;
1893
1894 if (heightFieldGeom == IntPtr.Zero)
1895 return 0f;
1896
1897 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1898 return 0f;
1899
1900 // TerrainHeightField for ODE as offset 1m
1901 x += 1f - offsetX;
1902 y += 1f - offsetY;
1903
1904 // make position fit into array
1905 if (x < 0)
1906 x = 0;
1907 if (y < 0)
1908 y = 0;
1909
1910 // integer indexs
1911 int ix;
1912 int iy;
1913 // interpolators offset
1914 float dx;
1915 float dy;
1916
1917 int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
1918 int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
1919 int regsize = regsizeX;
1920
1921 if (OdeUbitLib)
1922 {
1923 if (x < regsizeX - 1)
1924 {
1925 ix = (int)x;
1926 dx = x - (float)ix;
1927 }
1928 else // out world use external height
1929 {
1930 ix = regsizeX - 2;
1931 dx = 0;
1932 }
1933 if (y < regsizeY - 1)
1934 {
1935 iy = (int)y;
1936 dy = y - (float)iy;
1937 }
1938 else
1939 {
1940 iy = regsizeY - 2;
1941 dy = 0;
1942 }
1943 }
1944 else
1945 {
1946 // we still have square fixed size regions
1947 // also flip x and y because of how map is done for ODE fliped axis
1948 // so ix,iy,dx and dy are inter exchanged
1949
1950 regsize = regsizeY;
1951
1952 if (x < regsizeX - 1)
1953 {
1954 iy = (int)x;
1955 dy = x - (float)iy;
1956 }
1957 else // out world use external height
1958 {
1959 iy = regsizeX - 2;
1960 dy = 0;
1961 }
1962 if (y < regsizeY - 1)
1963 {
1964 ix = (int)y;
1965 dx = y - (float)ix;
1966 }
1967 else
1968 {
1969 ix = regsizeY - 2;
1970 dx = 0;
1971 }
1972 }
1973
1974 float h0;
1975 float h1;
1976 float h2;
1977
1978 iy *= regsize;
1979 iy += ix; // all indexes have iy + ix
1980
1981 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
1982 /*
1983 if ((dx + dy) <= 1.0f)
1984 {
1985 h0 = ((float)heights[iy]); // 0,0 vertice
1986 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
1987 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
1988 }
1989 else
1990 {
1991 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
1992 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
1993 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
1994 }
1995 */
1996 h0 = ((float)heights[iy]); // 0,0 vertice
1997
1998 if (dy>dx)
1999 {
2000 iy += regsize;
2001 h2 = (float)heights[iy]; // 0,1 vertice
2002 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2003 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2004 }
2005 else
2006 {
2007 iy++;
2008 h2 = (float)heights[iy]; // vertice 1,0
2009 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2010 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2011 }
2012
2013 return h0 + h1 + h2;
2014 }
2015
2016 public Vector3 GetTerrainNormalAtXY(float x, float y)
2017 {
2018 int offsetX = 0;
2019 int offsetY = 0;
2020
2021 if (m_suportCombine)
2022 {
2023 offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2024 offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2025 }
2026
2027 // get region map
2028 IntPtr heightFieldGeom = IntPtr.Zero;
2029 Vector3 norm = new Vector3(0, 0, 1);
2030
2031 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
2032 return norm; ;
2033
2034 if (heightFieldGeom == IntPtr.Zero)
2035 return norm;
2036
2037 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
2038 return norm;
2039
2040 // TerrainHeightField for ODE as offset 1m
2041 x += 1f - offsetX;
2042 y += 1f - offsetY;
2043
2044 // make position fit into array
2045 if (x < 0)
2046 x = 0;
2047 if (y < 0)
2048 y = 0;
2049
2050 // integer indexs
2051 int ix;
2052 int iy;
2053 // interpolators offset
2054 float dx;
2055 float dy;
2056
2057 int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
2058 int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
2059 int regsize = regsizeX;
2060
2061 int xstep = 1;
2062 int ystep = regsizeX;
2063 bool firstTri = false;
2064
2065 if (OdeUbitLib)
2066 {
2067 if (x < regsizeX - 1)
2068 {
2069 ix = (int)x;
2070 dx = x - (float)ix;
2071 }
2072 else // out world use external height
2073 {
2074 ix = regsizeX - 2;
2075 dx = 0;
2076 }
2077 if (y < regsizeY - 1)
2078 {
2079 iy = (int)y;
2080 dy = y - (float)iy;
2081 }
2082 else
2083 {
2084 iy = regsizeY - 2;
2085 dy = 0;
2086 }
2087 firstTri = dy > dx;
2088 }
2089
2090 else
2091 {
2092 xstep = regsizeY;
2093 ystep = 1;
2094 regsize = regsizeY;
2095
2096 // we still have square fixed size regions
2097 // also flip x and y because of how map is done for ODE fliped axis
2098 // so ix,iy,dx and dy are inter exchanged
2099 if (x < regsizeX - 1)
2100 {
2101 iy = (int)x;
2102 dy = x - (float)iy;
2103 }
2104 else // out world use external height
2105 {
2106 iy = regsizeX - 2;
2107 dy = 0;
2108 }
2109 if (y < regsizeY - 1)
2110 {
2111 ix = (int)y;
2112 dx = y - (float)ix;
2113 }
2114 else
2115 {
2116 ix = regsizeY - 2;
2117 dx = 0;
2118 }
2119 firstTri = dx > dy;
2120 }
2121
2122 float h0;
2123 float h1;
2124 float h2;
2125
2126 iy *= regsize;
2127 iy += ix; // all indexes have iy + ix
2128
2129 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2130
2131 if (firstTri)
2132 {
2133 h1 = ((float)heights[iy]); // 0,0 vertice
2134 iy += ystep;
2135 h0 = (float)heights[iy]; // 0,1
2136 h2 = (float)heights[iy+xstep]; // 1,1 vertice
2137 norm.X = h0 - h2;
2138 norm.Y = h1 - h0;
2139 }
2140 else
2141 {
2142 h2 = ((float)heights[iy]); // 0,0 vertice
2143 iy += xstep;
2144 h0 = ((float)heights[iy]); // 1,0 vertice
2145 h1 = (float)heights[iy+ystep]; // vertice 1,1
2146 norm.X = h2 - h0;
2147 norm.Y = h0 - h1;
2148 }
2149 norm.Z = 1;
2150 norm.Normalize();
2151 return norm;
2152 }
2153
2154 public override void SetTerrain(float[] heightMap)
2155 {
2156 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2157 {
2158 if (m_parentScene is OdeScene)
2159 {
2160 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2161 }
2162 }
2163 else
2164 {
2165 SetTerrain(heightMap, m_worldOffset);
2166 }
2167 }
2168
2169 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2170 {
2171 if(m_suportCombine)
2172 SetTerrain(heightMap, pOffset);
2173 }
2174
2175 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2176 {
2177 if (OdeUbitLib)
2178 UbitSetTerrain(heightMap, pOffset);
2179 else
2180 OriSetTerrain(heightMap, pOffset);
2181 }
2182
2183 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2184 {
2185 // assumes 1m size grid and constante size square regions
2186 // needs to know about sims around in future
2187
2188 float[] _heightmap;
2189
2190 uint regionsizeX = m_regionWidth;
2191 uint regionsizeY = m_regionHeight;
2192
2193 // map is rotated
2194 uint heightmapWidth = regionsizeY + 2;
2195 uint heightmapHeight = regionsizeX + 2;
2196
2197 uint heightmapWidthSamples = heightmapWidth + 1;
2198 uint heightmapHeightSamples = heightmapHeight + 1;
2199
2200 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2201
2202 const float scale = 1.0f;
2203 const float offset = 0.0f;
2204 const float thickness = 10f;
2205 const int wrap = 0;
2206
2207
2208 float hfmin = float.MaxValue;
2209 float hfmax = float.MinValue;
2210 float val;
2211 uint xx;
2212 uint yy;
2213
2214 uint maxXX = regionsizeX - 1;
2215 uint maxYY = regionsizeY - 1;
2216 // flipping map adding one margin all around so things don't fall in edges
2217
2218 uint xt = 0;
2219 xx = 0;
2220
2221 for (uint x = 0; x < heightmapWidthSamples; x++)
2222 {
2223 if (x > 1 && xx < maxXX)
2224 xx++;
2225 yy = 0;
2226 for (uint y = 0; y < heightmapHeightSamples; y++)
2227 {
2228 if (y > 1 && y < maxYY)
2229 yy += regionsizeX;
2230
2231 val = heightMap[yy + xx];
2232 if (val < 0.0f)
2233 val = 0.0f; // no neg terrain as in chode
2234 _heightmap[xt + y] = val;
2235
2236 if (hfmin > val)
2237 hfmin = val;
2238 if (hfmax < val)
2239 hfmax = val;
2240 }
2241 xt += heightmapHeightSamples;
2242 }
2243
2244 lock (OdeLock)
2245 {
2246 IntPtr GroundGeom = IntPtr.Zero;
2247 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2248 {
2249 RegionTerrain.Remove(pOffset);
2250 if (GroundGeom != IntPtr.Zero)
2251 {
2252 actor_name_map.Remove(GroundGeom);
2253 d.GeomDestroy(GroundGeom);
2254
2255 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2256 {
2257 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2258 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2259 TerrainHeightFieldHeights.Remove(GroundGeom);
2260 }
2261 }
2262 }
2263 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2264
2265 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2266
2267 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2268 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2269 offset, thickness, wrap);
2270
2271 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2272
2273 GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
2274
2275 if (GroundGeom != IntPtr.Zero)
2276 {
2277 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2278 d.GeomSetCollideBits(GroundGeom, 0);
2279
2280 PhysicsActor pa = new NullPhysicsActor();
2281 pa.Name = "Terrain";
2282 pa.PhysicsActorType = (int)ActorTypes.Ground;
2283 actor_name_map[GroundGeom] = pa;
2284
2285// geom_name_map[GroundGeom] = "Terrain";
2286
2287 d.Matrix3 R = new d.Matrix3();
2288
2289 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2290 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2291
2292
2293 q1 = q1 * q2;
2294
2295 Vector3 v3;
2296 float angle;
2297 q1.GetAxisAngle(out v3, out angle);
2298
2299 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2300 d.GeomSetRotation(GroundGeom, ref R);
2301 d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0);
2302 RegionTerrain.Add(pOffset, GroundGeom);
2303 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2304 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2305 }
2306 }
2307 }
2308
2309 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2310 {
2311 // assumes 1m size grid and constante size square regions
2312 // needs to know about sims around in future
2313
2314 float[] _heightmap;
2315
2316 uint regionsizeX = m_regionWidth;
2317 uint regionsizeY = m_regionHeight;
2318
2319 uint heightmapWidth = regionsizeX + 2;
2320 uint heightmapHeight = regionsizeY + 2;
2321
2322 uint heightmapWidthSamples = heightmapWidth + 1;
2323 uint heightmapHeightSamples = heightmapHeight + 1;
2324
2325 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2326
2327
2328 float hfmin = float.MaxValue;
2329// float hfmax = float.MinValue;
2330 float val;
2331
2332
2333 uint maxXX = regionsizeX - 1;
2334 uint maxYY = regionsizeY - 1;
2335 // adding one margin all around so things don't fall in edges
2336
2337 uint xx;
2338 uint yy = 0;
2339 uint yt = 0;
2340
2341 for (uint y = 0; y < heightmapHeightSamples; y++)
2342 {
2343 if (y > 1 && y < maxYY)
2344 yy += regionsizeX;
2345 xx = 0;
2346 for (uint x = 0; x < heightmapWidthSamples; x++)
2347 {
2348 if (x > 1 && x < maxXX)
2349 xx++;
2350
2351 val = heightMap[yy + xx];
2352 if (val < 0.0f)
2353 val = 0.0f; // no neg terrain as in chode
2354 _heightmap[yt + x] = val;
2355
2356 if (hfmin > val)
2357 hfmin = val;
2358// if (hfmax < val)
2359// hfmax = val;
2360 }
2361 yt += heightmapWidthSamples;
2362 }
2363 lock (OdeLock)
2364 {
2365 IntPtr GroundGeom = IntPtr.Zero;
2366 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2367 {
2368 RegionTerrain.Remove(pOffset);
2369 if (GroundGeom != IntPtr.Zero)
2370 {
2371 actor_name_map.Remove(GroundGeom);
2372 d.GeomDestroy(GroundGeom);
2373
2374 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2375 {
2376 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2377 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2378 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2379 TerrainHeightFieldHeights.Remove(GroundGeom);
2380 }
2381 }
2382 }
2383 IntPtr HeightmapData = d.GeomUbitTerrainDataCreate();
2384
2385 const int wrap = 0;
2386 float thickness = hfmin;
2387 if (thickness < 0)
2388 thickness = 1;
2389
2390 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2391
2392 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2393 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2394 thickness, wrap);
2395
2396// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2397 GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1);
2398 if (GroundGeom != IntPtr.Zero)
2399 {
2400 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2401 d.GeomSetCollideBits(GroundGeom, 0);
2402
2403
2404 PhysicsActor pa = new NullPhysicsActor();
2405 pa.Name = "Terrain";
2406 pa.PhysicsActorType = (int)ActorTypes.Ground;
2407 actor_name_map[GroundGeom] = pa;
2408
2409// geom_name_map[GroundGeom] = "Terrain";
2410
2411 d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0);
2412 RegionTerrain.Add(pOffset, GroundGeom);
2413 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2414 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2415 }
2416 }
2417 }
2418
2419
2420 public override void DeleteTerrain()
2421 {
2422 }
2423
2424 public float GetWaterLevel()
2425 {
2426 return waterlevel;
2427 }
2428
2429 public override bool SupportsCombining()
2430 {
2431 return m_suportCombine;
2432 }
2433/*
2434 public override void UnCombine(PhysicsScene pScene)
2435 {
2436 IntPtr localGround = IntPtr.Zero;
2437// float[] localHeightfield;
2438 bool proceed = false;
2439 List<IntPtr> geomDestroyList = new List<IntPtr>();
2440
2441 lock (OdeLock)
2442 {
2443 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2444 {
2445 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2446 {
2447 if (geom == localGround)
2448 {
2449// localHeightfield = TerrainHeightFieldHeights[geom];
2450 proceed = true;
2451 }
2452 else
2453 {
2454 geomDestroyList.Add(geom);
2455 }
2456 }
2457
2458 if (proceed)
2459 {
2460 m_worldOffset = Vector3.Zero;
2461 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2462 m_parentScene = null;
2463
2464 foreach (IntPtr g in geomDestroyList)
2465 {
2466 // removingHeightField needs to be done or the garbage collector will
2467 // collect the terrain data before we tell ODE to destroy it causing
2468 // memory corruption
2469 if (TerrainHeightFieldHeights.ContainsKey(g))
2470 {
2471// float[] removingHeightField = TerrainHeightFieldHeights[g];
2472 TerrainHeightFieldHeights.Remove(g);
2473
2474 if (RegionTerrain.ContainsKey(g))
2475 {
2476 RegionTerrain.Remove(g);
2477 }
2478
2479 d.GeomDestroy(g);
2480 //removingHeightField = new float[0];
2481 }
2482 }
2483
2484 }
2485 else
2486 {
2487 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2488 }
2489 }
2490 }
2491 }
2492*/
2493 public override void SetWaterLevel(float baseheight)
2494 {
2495 waterlevel = baseheight;
2496 }
2497
2498 public override void Dispose()
2499 {
2500 if (m_meshWorker != null)
2501 m_meshWorker.Stop();
2502
2503 lock (OdeLock)
2504 {
2505 m_rayCastManager.Dispose();
2506 m_rayCastManager = null;
2507
2508 lock (_prims)
2509 {
2510 ChangesQueue.Clear();
2511 foreach (OdePrim prm in _prims)
2512 {
2513 prm.DoAChange(changes.Remove, null);
2514 _collisionEventPrim.Remove(prm);
2515 }
2516 _prims.Clear();
2517 }
2518
2519 OdeCharacter[] chtorem;
2520 lock (_characters)
2521 {
2522 chtorem = new OdeCharacter[_characters.Count];
2523 _characters.CopyTo(chtorem);
2524 }
2525
2526 ChangesQueue.Clear();
2527 foreach (OdeCharacter ch in chtorem)
2528 ch.DoAChange(changes.Remove, null);
2529
2530
2531 foreach (IntPtr GroundGeom in RegionTerrain.Values)
2532 {
2533 if (GroundGeom != IntPtr.Zero)
2534 d.GeomDestroy(GroundGeom);
2535 }
2536
2537
2538 RegionTerrain.Clear();
2539
2540 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2541 {
2542 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2543 {
2544 if (gch.IsAllocated)
2545 gch.Free();
2546 }
2547 }
2548
2549 TerrainHeightFieldHeightsHandlers.Clear();
2550 TerrainHeightFieldHeights.Clear();
2551
2552 if (ContactgeomsArray != IntPtr.Zero)
2553 Marshal.FreeHGlobal(ContactgeomsArray);
2554 if (GlobalContactsArray != IntPtr.Zero)
2555 Marshal.FreeHGlobal(GlobalContactsArray);
2556
2557
2558 d.WorldDestroy(world);
2559 world = IntPtr.Zero;
2560 //d.CloseODE();
2561 }
2562 }
2563
2564 public override Dictionary<uint, float> GetTopColliders()
2565 {
2566 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2567 int cnt = 0;
2568 lock (_prims)
2569 {
2570 foreach (OdePrim prm in _prims)
2571 {
2572 if (prm.CollisionScore > 0)
2573 {
2574 returncolliders.Add(prm.LocalID, prm.CollisionScore);
2575 cnt++;
2576 prm.CollisionScore = 0f;
2577 if (cnt > 25)
2578 {
2579 break;
2580 }
2581 }
2582 }
2583 }
2584 return returncolliders;
2585 }
2586
2587 public override bool SupportsRayCast()
2588 {
2589 return true;
2590 }
2591
2592 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2593 {
2594 if (retMethod != null)
2595 {
2596 ODERayRequest req = new ODERayRequest();
2597 req.actor = null;
2598 req.callbackMethod = retMethod;
2599 req.length = length;
2600 req.Normal = direction;
2601 req.Origin = position;
2602 req.Count = 0;
2603 req.filter = RayFilterFlags.AllPrims;
2604
2605 m_rayCastManager.QueueRequest(req);
2606 }
2607 }
2608
2609 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2610 {
2611 if (retMethod != null)
2612 {
2613 ODERayRequest req = new ODERayRequest();
2614 req.actor = null;
2615 req.callbackMethod = retMethod;
2616 req.length = length;
2617 req.Normal = direction;
2618 req.Origin = position;
2619 req.Count = Count;
2620 req.filter = RayFilterFlags.AllPrims;
2621
2622 m_rayCastManager.QueueRequest(req);
2623 }
2624 }
2625
2626
2627 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2628 {
2629 List<ContactResult> ourresults = new List<ContactResult>();
2630 object SyncObject = new object();
2631
2632 RayCallback retMethod = delegate(List<ContactResult> results)
2633 {
2634 lock (SyncObject)
2635 {
2636 ourresults = results;
2637 Monitor.PulseAll(SyncObject);
2638 }
2639 };
2640
2641 ODERayRequest req = new ODERayRequest();
2642 req.actor = null;
2643 req.callbackMethod = retMethod;
2644 req.length = length;
2645 req.Normal = direction;
2646 req.Origin = position;
2647 req.Count = Count;
2648 req.filter = RayFilterFlags.AllPrims;
2649
2650 lock (SyncObject)
2651 {
2652 m_rayCastManager.QueueRequest(req);
2653 if (!Monitor.Wait(SyncObject, 500))
2654 return null;
2655 else
2656 return ourresults;
2657 }
2658 }
2659
2660 public override bool SupportsRaycastWorldFiltered()
2661 {
2662 return true;
2663 }
2664
2665 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2666 {
2667 object SyncObject = new object();
2668 List<ContactResult> ourresults = new List<ContactResult>();
2669
2670 RayCallback retMethod = delegate(List<ContactResult> results)
2671 {
2672 lock (SyncObject)
2673 {
2674 ourresults = results;
2675 Monitor.PulseAll(SyncObject);
2676 }
2677 };
2678
2679 ODERayRequest req = new ODERayRequest();
2680 req.actor = null;
2681 req.callbackMethod = retMethod;
2682 req.length = length;
2683 req.Normal = direction;
2684 req.Origin = position;
2685 req.Count = Count;
2686 req.filter = filter;
2687
2688 lock (SyncObject)
2689 {
2690 m_rayCastManager.QueueRequest(req);
2691 if (!Monitor.Wait(SyncObject, 500))
2692 return null;
2693 else
2694 return ourresults;
2695 }
2696 }
2697
2698 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
2699 {
2700 if (actor == null)
2701 return new List<ContactResult>();
2702
2703 IntPtr geom;
2704 if (actor is OdePrim)
2705 geom = ((OdePrim)actor).prim_geom;
2706 else if (actor is OdeCharacter)
2707 geom = ((OdePrim)actor).prim_geom;
2708 else
2709 return new List<ContactResult>();
2710
2711 if (geom == IntPtr.Zero)
2712 return new List<ContactResult>();
2713
2714 List<ContactResult> ourResults = null;
2715 object SyncObject = new object();
2716
2717 RayCallback retMethod = delegate(List<ContactResult> results)
2718 {
2719 lock (SyncObject)
2720 {
2721 ourResults = results;
2722 Monitor.PulseAll(SyncObject);
2723 }
2724 };
2725
2726 ODERayRequest req = new ODERayRequest();
2727 req.actor = actor;
2728 req.callbackMethod = retMethod;
2729 req.length = length;
2730 req.Normal = direction;
2731 req.Origin = position;
2732 req.Count = Count;
2733 req.filter = flags;
2734
2735 lock (SyncObject)
2736 {
2737 m_rayCastManager.QueueRequest(req);
2738 if (!Monitor.Wait(SyncObject, 500))
2739 return new List<ContactResult>();
2740 }
2741
2742 if (ourResults == null)
2743 return new List<ContactResult>();
2744 return ourResults;
2745 }
2746
2747 public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
2748 {
2749 List<ContactResult> ourResults = null;
2750 object SyncObject = new object();
2751
2752 ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
2753 {
2754 lock (SyncObject)
2755 {
2756 ourResults = results;
2757 Monitor.PulseAll(SyncObject);
2758 }
2759 };
2760
2761 ODERayRequest req = new ODERayRequest();
2762 req.actor = null;
2763 req.callbackMethod = retMethod;
2764 req.Normal = size;
2765 req.Origin = position;
2766 req.orientation = orientation;
2767 req.Count = Count;
2768 req.filter = flags;
2769
2770 lock (SyncObject)
2771 {
2772 m_rayCastManager.QueueRequest(req);
2773 if (!Monitor.Wait(SyncObject, 500))
2774 return new List<ContactResult>();
2775 }
2776
2777 if (ourResults == null)
2778 return new List<ContactResult>();
2779 return ourResults;
2780 }
2781
2782 public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
2783 {
2784 List<ContactResult> ourResults = null;
2785 object SyncObject = new object();
2786
2787 ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
2788 {
2789 ourResults = results;
2790 Monitor.PulseAll(SyncObject);
2791 };
2792
2793 ODERayRequest req = new ODERayRequest();
2794 req.actor = null;
2795 req.callbackMethod = retMethod;
2796 req.length = radius;
2797 req.Origin = position;
2798 req.Count = Count;
2799 req.filter = flags;
2800
2801
2802 lock (SyncObject)
2803 {
2804 m_rayCastManager.QueueRequest(req);
2805 if (!Monitor.Wait(SyncObject, 500))
2806 return new List<ContactResult>();
2807 }
2808
2809 if (ourResults == null)
2810 return new List<ContactResult>();
2811 return ourResults;
2812 }
2813
2814 public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
2815 {
2816 IntPtr geom = IntPtr.Zero;;
2817
2818 if (actor != null)
2819 {
2820 if (actor is OdePrim)
2821 geom = ((OdePrim)actor).prim_geom;
2822 else if (actor is OdeCharacter)
2823 geom = ((OdePrim)actor).prim_geom;
2824 }
2825
2826 List<ContactResult> ourResults = null;
2827 object SyncObject = new object();
2828
2829 ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
2830 {
2831 ourResults = results;
2832 Monitor.PulseAll(SyncObject);
2833 };
2834
2835 ODERayRequest req = new ODERayRequest();
2836 req.actor = null;
2837 req.callbackMethod = retMethod;
2838 req.length = plane.W;
2839 req.Normal.X = plane.X;
2840 req.Normal.Y = plane.Y;
2841 req.Normal.Z = plane.Z;
2842 req.Count = Count;
2843 req.filter = flags;
2844
2845 lock (SyncObject)
2846 {
2847 m_rayCastManager.QueueRequest(req);
2848 if (!Monitor.Wait(SyncObject, 500))
2849 return new List<ContactResult>();
2850 }
2851
2852 if (ourResults == null)
2853 return new List<ContactResult>();
2854 return ourResults;
2855 }
2856
2857 public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
2858 {
2859 Util.FireAndForget( delegate
2860 {
2861 ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
2862 if(sitAvatar != null)
2863 sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
2864 });
2865 return 1;
2866 }
2867
2868 }
2869}
generated by cgit v1.1 at 2024-12-14 08:45:10 +0000