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