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