aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs6
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs1467
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs4124
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs384
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs48
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs3887
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs353
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs122
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs98
-rw-r--r--OpenSim/Region/Physics/Manager/IMesher.cs28
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsActor.cs228
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsScene.cs61
-rw-r--r--OpenSim/Region/Physics/Manager/VehicleConstants.cs45
-rw-r--r--OpenSim/Region/Physics/Manager/ZeroMesher.cs14
-rw-r--r--OpenSim/Region/Physics/Meshing/Mesh.cs79
-rw-r--r--OpenSim/Region/Physics/Meshing/Meshmerizer.cs13
-rw-r--r--OpenSim/Region/Physics/Meshing/SculptMap.cs62
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs22
-rw-r--r--OpenSim/Region/Physics/POSPlugin/POSPrim.cs2
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs340
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/Mesh.cs614
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs1410
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs2324
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/SculptMap.cs244
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs220
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1950
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs1087
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs932
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs3844
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs683
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs356
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs2025
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs90
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2859
36 files changed, 30009 insertions, 128 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index b16bc10..05c147d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -632,8 +632,10 @@ public sealed class BSShapeCollection : IDisposable
632 else 632 else
633 { 633 {
634 IMesh meshData = PhysicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, 634 IMesh meshData = PhysicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod,
635 true,
635 false, // say it is not physical so a bounding box is not built 636 false, // say it is not physical so a bounding box is not built
636 false // do not cache the mesh and do not use previously built versions 637 false, // do not cache the mesh and do not use previously built versions
638 false // It's NOT for ODE
637 ); 639 );
638 640
639 if (meshData != null) 641 if (meshData != null)
@@ -740,7 +742,7 @@ public sealed class BSShapeCollection : IDisposable
740 { 742 {
741 // Build a new hull in the physical world 743 // Build a new hull in the physical world
742 // Pass true for physicalness as this creates some sort of bounding box which we don't need 744 // Pass true for physicalness as this creates some sort of bounding box which we don't need
743 IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false); 745 IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false, false, false);
744 if (meshData != null) 746 if (meshData != null)
745 { 747 {
746 748
diff --git a/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d65929a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/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("")]
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/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..ec717d7
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1467 @@
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 OpenMetaverse;
32using Ode.NET;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using log4net;
36
37namespace OpenSim.Region.Physics.OdePlugin
38{
39 /// <summary>
40 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
41 /// </summary>
42
43 public enum dParam : int
44 {
45 LowStop = 0,
46 HiStop = 1,
47 Vel = 2,
48 FMax = 3,
49 FudgeFactor = 4,
50 Bounce = 5,
51 CFM = 6,
52 StopERP = 7,
53 StopCFM = 8,
54 LoStop2 = 256,
55 HiStop2 = 257,
56 Vel2 = 258,
57 FMax2 = 259,
58 StopERP2 = 7 + 256,
59 StopCFM2 = 8 + 256,
60 LoStop3 = 512,
61 HiStop3 = 513,
62 Vel3 = 514,
63 FMax3 = 515,
64 StopERP3 = 7 + 512,
65 StopCFM3 = 8 + 512
66 }
67 public class OdeCharacter : PhysicsActor
68 {
69 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
70
71 private Vector3 _position;
72 private d.Vector3 _zeroPosition;
73 // private d.Matrix3 m_StandUpRotation;
74 private bool _zeroFlag = false;
75 private bool m_lastUpdateSent = false;
76 private Vector3 _velocity;
77 private Vector3 _target_velocity;
78 private Vector3 _acceleration;
79 private Vector3 m_rotationalVelocity;
80 private float m_mass = 80f;
81 public float m_density = 60f;
82 private bool m_pidControllerActive = true;
83 public float PID_D = 800.0f;
84 public float PID_P = 900.0f;
85 //private static float POSTURE_SERVO = 10000.0f;
86 public float CAPSULE_RADIUS = 0.37f;
87 public float CAPSULE_LENGTH = 2.140599f;
88 public float m_tensor = 3800000f;
89 public float heightFudgeFactor = 0.52f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool jumping = false; // add for jumping
94 private bool m_iscolliding = false;
95 private bool m_iscollidingGround = false;
96 private bool m_wascolliding = false;
97 private bool m_wascollidingGround = false;
98 private bool m_iscollidingObj = false;
99 private bool m_alwaysRun = false;
100 private bool m_hackSentFall = false;
101 private bool m_hackSentFly = false;
102 private int m_requestedUpdateFrequency = 0;
103 private Vector3 m_taintPosition = Vector3.Zero;
104 public uint m_localID = 0;
105 public bool m_returnCollisions = false;
106 // taints and their non-tainted counterparts
107 public bool m_isPhysical = false; // the current physical status
108 public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
109 public float MinimumGroundFlightOffset = 3f;
110
111 private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
112 private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
113
114
115 private float m_buoyancy = 0f;
116
117 // private CollisionLocker ode;
118
119 private string m_name = String.Empty;
120
121 private bool[] m_colliderarr = new bool[11];
122 private bool[] m_colliderGroundarr = new bool[11];
123
124 // Default we're a Character
125 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
126
127 // Default, Collide with Other Geometries, spaces, bodies and characters.
128 private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
129 | CollisionCategories.Space
130 | CollisionCategories.Body
131 | CollisionCategories.Character
132 | CollisionCategories.Land);
133 public IntPtr Body = IntPtr.Zero;
134 private OdeScene _parent_scene;
135 public IntPtr Shell = IntPtr.Zero;
136 public IntPtr Amotor = IntPtr.Zero;
137 public d.Mass ShellMass;
138 public bool collidelock = false;
139
140 public int m_eventsubscription = 0;
141 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
142
143 private Vector3 m_taintMomentum = Vector3.Zero;
144 private bool m_haveTaintMomentum = false;
145
146
147 // unique UUID of this character object
148 public UUID m_uuid;
149 public bool bad = false;
150 private Object m_syncRoot = new Object();
151
152 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
153 {
154 m_uuid = UUID.Random();
155
156 if (pos.IsFinite())
157 {
158 if (pos.Z > 9999999f)
159 {
160 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
161 }
162 if (pos.Z < -90000f)
163 {
164 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
165 }
166 _position = pos;
167 m_taintPosition.X = pos.X;
168 m_taintPosition.Y = pos.Y;
169 m_taintPosition.Z = pos.Z;
170 }
171 else
172 {
173 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
174 m_taintPosition.X = _position.X;
175 m_taintPosition.Y = _position.Y;
176 m_taintPosition.Z = _position.Z;
177 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
178 }
179
180 _parent_scene = parent_scene;
181
182 PID_D = pid_d;
183 PID_P = pid_p;
184 CAPSULE_RADIUS = capsule_radius;
185 m_tensor = tensor;
186 m_density = density;
187 heightFudgeFactor = height_fudge_factor;
188 walkDivisor = walk_divisor;
189 runDivisor = rundivisor;
190
191 // m_StandUpRotation =
192 // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
193 // 0.5f);
194
195 for (int i = 0; i < 11; i++)
196 {
197 m_colliderarr[i] = false;
198 }
199 CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
200 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
201 m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
202
203 m_isPhysical = false; // current status: no ODE information exists
204 m_tainted_isPhysical = true; // new tainted status: need to create ODE information
205
206 _parent_scene.AddPhysicsActorTaint(this);
207
208 m_name = avName;
209 }
210
211 public override int PhysicsActorType
212 {
213 get { return (int) ActorTypes.Agent; }
214 set { return; }
215 }
216
217 /// <summary>
218 /// If this is set, the avatar will move faster
219 /// </summary>
220 public override bool SetAlwaysRun
221 {
222 get { return m_alwaysRun; }
223 set { m_alwaysRun = value; }
224 }
225
226 public override uint LocalID
227 {
228 set { m_localID = value; }
229 }
230
231 public override bool Grabbed
232 {
233 set { return; }
234 }
235
236 public override bool Selected
237 {
238// set { return; }
239 set { jumping = value; } // add for jumping flag
240 }
241
242 public override float Buoyancy
243 {
244 get { return m_buoyancy; }
245 set { m_buoyancy = value; }
246 }
247
248 public override bool FloatOnWater
249 {
250 set { return; }
251 }
252
253 public override bool IsPhysical
254 {
255 get { return false; }
256 set { return; }
257 }
258
259 public override bool ThrottleUpdates
260 {
261 get { return false; }
262 set { return; }
263 }
264
265 public override bool Flying
266 {
267 get { return flying; }
268 set { flying = value; }
269 }
270
271 /// <summary>
272 /// Returns if the avatar is colliding in general.
273 /// This includes the ground and objects and avatar.
274 /// </summary>
275 public override bool IsColliding
276 {
277//#@ get { return m_iscolliding; }
278 get { //##
279//Console.WriteLine(">>>>>>>>>>>> IC get = {0}", m_iscolliding); //##
280 return m_iscolliding; } //##
281 set
282 {
283 int i;
284 int truecount = 0;
285 int falsecount = 0;
286
287 if (m_colliderarr.Length >= 10)
288 {
289 for (i = 0; i < 10; i++)
290 {
291 m_colliderarr[i] = m_colliderarr[i + 1];
292 }
293 }
294 m_colliderarr[10] = value;
295
296 for (i = 0; i < 11; i++)
297 {
298 if (m_colliderarr[i])
299 {
300 truecount++;
301 }
302 else
303 {
304 falsecount++;
305 }
306 }
307
308 // Equal truecounts and false counts means we're colliding with something.
309
310 if (falsecount > 1.2*truecount)
311 {
312 m_iscolliding = false;
313 }
314 else
315 {
316 m_iscolliding = true;
317 }
318// ## Console.WriteLine("IC SET = {0} t{1} f{2} i {3}", value, truecount, falsecount, m_iscolliding);
319 if (m_wascolliding != m_iscolliding)
320 {
321 //base.SendCollisionUpdate(new CollisionEventUpdate());
322 }
323 m_wascolliding = m_iscolliding;
324 }
325 }
326
327 /// <summary>
328 /// Returns if an avatar is colliding with the ground
329 /// </summary>
330 public override bool CollidingGround
331 {
332 get { return m_iscollidingGround; }
333 set
334 {
335 // Collisions against the ground are not really reliable
336 // So, to get a consistant value we have to average the current result over time
337 // Currently we use 1 second = 10 calls to this.
338 int i;
339 int truecount = 0;
340 int falsecount = 0;
341
342 if (m_colliderGroundarr.Length >= 10)
343 {
344 for (i = 0; i < 10; i++)
345 {
346 m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
347 }
348 }
349 m_colliderGroundarr[10] = value;
350
351 for (i = 0; i < 11; i++)
352 {
353 if (m_colliderGroundarr[i])
354 {
355 truecount++;
356 }
357 else
358 {
359 falsecount++;
360 }
361 }
362
363 // Equal truecounts and false counts means we're colliding with something.
364
365 if (falsecount > 1.2*truecount)
366 {
367 m_iscollidingGround = false;
368 }
369 else
370 {
371 m_iscollidingGround = true;
372 }
373 if (m_wascollidingGround != m_iscollidingGround)
374 {
375 //base.SendCollisionUpdate(new CollisionEventUpdate());
376 }
377 m_wascollidingGround = m_iscollidingGround;
378 }
379 }
380
381 /// <summary>
382 /// Returns if the avatar is colliding with an object
383 /// </summary>
384 public override bool CollidingObj
385 {
386 get { return m_iscollidingObj; }
387 set
388 {
389 m_iscollidingObj = value;
390 if (value)
391 m_pidControllerActive = false;
392 else
393 m_pidControllerActive = true;
394 }
395 }
396
397 /// <summary>
398 /// turn the PID controller on or off.
399 /// The PID Controller will turn on all by itself in many situations
400 /// </summary>
401 /// <param name="status"></param>
402 public void SetPidStatus(bool status)
403 {
404 m_pidControllerActive = status;
405 }
406
407 public override bool Stopped
408 {
409 get { return _zeroFlag; }
410 }
411
412 /// <summary>
413 /// This 'puts' an avatar somewhere in the physics space.
414 /// Not really a good choice unless you 'know' it's a good
415 /// spot otherwise you're likely to orbit the avatar.
416 /// </summary>
417 public override Vector3 Position
418 {
419 get { return _position; }
420 set
421 {
422 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
423 {
424 if (value.IsFinite())
425 {
426 if (value.Z > 9999999f)
427 {
428 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
429 }
430 if (value.Z < -90000f)
431 {
432 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
433 }
434
435 _position.X = value.X;
436 _position.Y = value.Y;
437 _position.Z = value.Z;
438
439 m_taintPosition.X = value.X;
440 m_taintPosition.Y = value.Y;
441 m_taintPosition.Z = value.Z;
442 _parent_scene.AddPhysicsActorTaint(this);
443 }
444 else
445 {
446 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
447 }
448 }
449 }
450 }
451
452 public override Vector3 RotationalVelocity
453 {
454 get { return m_rotationalVelocity; }
455 set { m_rotationalVelocity = value; }
456 }
457
458 /// <summary>
459 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
460 /// and use it to offset landings properly
461 /// </summary>
462 public override Vector3 Size
463 {
464 get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
465 set
466 {
467 if (value.IsFinite())
468 {
469 m_pidControllerActive = true;
470
471 Vector3 SetSize = value;
472 m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
473 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
474
475 Velocity = Vector3.Zero;
476 m_taintPosition = _position; // update the stale taint position
477 _parent_scene.AddPhysicsActorTaint(this);
478 }
479 else
480 {
481 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
482 }
483 }
484 }
485
486 private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
487 {
488 movementVector.Z = 0f;
489 float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
490 if (magnitude < 0.1f) return;
491
492 // normalize the velocity vector
493 float invMagnitude = 1.0f / magnitude;
494 movementVector.X *= invMagnitude;
495 movementVector.Y *= invMagnitude;
496
497 // if we change the capsule heading too often, the capsule can fall down
498 // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
499 // meaning only 4 possible capsule tilt orientations
500 if (movementVector.X > 0)
501 {
502 // east
503 if (movementVector.Y > 0)
504 {
505 // northeast
506 movementVector.X = (float)Math.Sqrt(2.0);
507 movementVector.Y = (float)Math.Sqrt(2.0);
508 }
509 else
510 {
511 // southeast
512 movementVector.X = (float)Math.Sqrt(2.0);
513 movementVector.Y = -(float)Math.Sqrt(2.0);
514 }
515 }
516 else
517 {
518 // west
519 if (movementVector.Y > 0)
520 {
521 // northwest
522 movementVector.X = -(float)Math.Sqrt(2.0);
523 movementVector.Y = (float)Math.Sqrt(2.0);
524 }
525 else
526 {
527 // southwest
528 movementVector.X = -(float)Math.Sqrt(2.0);
529 movementVector.Y = -(float)Math.Sqrt(2.0);
530 }
531 }
532
533
534 // movementVector.Z is zero
535
536 // calculate tilt components based on desired amount of tilt and current (snapped) heading.
537 // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
538 float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
539 float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
540
541 //m_log.Debug("[PHYSICS] changing avatar tilt");
542 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
543 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
544 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
545 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
546 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
547 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
548 }
549
550 /// <summary>
551 /// This creates the Avatar's physical Surrogate at the position supplied
552 /// </summary>
553 /// <param name="npositionX"></param>
554 /// <param name="npositionY"></param>
555 /// <param name="npositionZ"></param>
556
557 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
558 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
559 // place that is safe to call this routine AvatarGeomAndBodyCreation.
560 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
561 {
562 //CAPSULE_LENGTH = -5;
563 //CAPSULE_RADIUS = -5;
564 int dAMotorEuler = 1;
565 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
566 if (CAPSULE_LENGTH <= 0)
567 {
568 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
569 CAPSULE_LENGTH = 0.01f;
570
571 }
572
573 if (CAPSULE_RADIUS <= 0)
574 {
575 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
576 CAPSULE_RADIUS = 0.01f;
577
578 }
579
580 if(Shell != IntPtr.Zero)
581 {
582 try
583 {
584 d.GeomDestroy(Shell);
585 }
586 catch (System.AccessViolationException)
587 {
588 m_log.Error("[PHYSICS]: PrimGeom dead");
589 }
590 // Remove any old entries
591//string tShell;
592//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
593//Console.WriteLine("**** Remove {0}", tShell);
594 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
595 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
596 }
597
598 Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
599 _parent_scene.geom_name_map[Shell] = m_name;
600 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
601//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1} height={3} rad={4}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, m_name, CAPSULE_LENGTH, CAPSULE_RADIUS);
602
603 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
604 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
605
606 d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
607 Body = d.BodyCreate(_parent_scene.world);
608 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
609
610 _position.X = npositionX;
611 _position.Y = npositionY;
612 _position.Z = npositionZ;
613
614
615 m_taintPosition.X = npositionX;
616 m_taintPosition.Y = npositionY;
617 m_taintPosition.Z = npositionZ;
618
619 d.BodySetMass(Body, ref ShellMass);
620 d.Matrix3 m_caprot;
621 // 90 Stand up on the cap of the capped cyllinder
622 if (_parent_scene.IsAvCapsuleTilted)
623 {
624 d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
625 }
626 else
627 {
628 d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
629 }
630
631
632 d.GeomSetRotation(Shell, ref m_caprot);
633 d.BodySetRotation(Body, ref m_caprot);
634
635 d.GeomSetBody(Shell, Body);
636
637
638 // The purpose of the AMotor here is to keep the avatar's physical
639 // surrogate from rotating while moving
640 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
641 d.JointAttach(Amotor, Body, IntPtr.Zero);
642 d.JointSetAMotorMode(Amotor, dAMotorEuler);
643 d.JointSetAMotorNumAxes(Amotor, 3);
644 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
645 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
646 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
647 d.JointSetAMotorAngle(Amotor, 0, 0);
648 d.JointSetAMotorAngle(Amotor, 1, 0);
649 d.JointSetAMotorAngle(Amotor, 2, 0);
650
651 // These lowstops and high stops are effectively (no wiggle room)
652 if (_parent_scene.IsAvCapsuleTilted)
653 {
654 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
655 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
656 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
657 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
658 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
659 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
660 }
661 else
662 {
663 #region Documentation of capsule motor LowStop and HighStop parameters
664 // Intentionally introduce some tilt into the capsule by setting
665 // the motor stops to small epsilon values. This small tilt prevents
666 // the capsule from falling into the terrain; a straight-up capsule
667 // (with -0..0 motor stops) falls into the terrain for reasons yet
668 // to be comprehended in their entirety.
669 #endregion
670 AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
671 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
672 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
673 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
674 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
675 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
676 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
677 }
678
679 // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the
680 // capped cyllinder will fall over
681 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
682 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
683
684 //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
685 //d.QfromR(
686 //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
687 //
688 //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
689 //standupStraight();
690 }
691
692 //
693 /// <summary>
694 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
695 /// This may be used in calculations in the scene/scenepresence
696 /// </summary>
697 public override float Mass
698 {
699 get
700 {
701 float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
702 return m_density*AVvolume;
703 }
704 }
705 public override void link(PhysicsActor obj)
706 {
707
708 }
709
710 public override void delink()
711 {
712
713 }
714
715 public override void LockAngularMotion(Vector3 axis)
716 {
717
718 }
719
720// This code is very useful. Written by DanX0r. We're just not using it right now.
721// Commented out to prevent a warning.
722//
723// private void standupStraight()
724// {
725// // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
726// // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
727// // change appearance and when you enter the simulator
728// // After this routine is done, the amotor stabilizes much quicker
729// d.Vector3 feet;
730// d.Vector3 head;
731// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
732// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
733// float posture = head.Z - feet.Z;
734
735// // restoring force proportional to lack of posture:
736// float servo = (2.5f - posture) * POSTURE_SERVO;
737// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
738// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
739// //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
740// //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
741// }
742
743 public override Vector3 Force
744 {
745 get { return _target_velocity; }
746 set { return; }
747 }
748
749 public override int VehicleType
750 {
751 get { return 0; }
752 set { return; }
753 }
754
755 public override void VehicleFloatParam(int param, float value)
756 {
757
758 }
759
760 public override void VehicleVectorParam(int param, Vector3 value)
761 {
762
763 }
764
765 public override void VehicleRotationParam(int param, Quaternion rotation)
766 {
767
768 }
769
770 public override void VehicleFlags(int flags, bool remove)
771 {
772 }
773
774 public override void SetVolumeDetect(int param)
775 {
776
777 }
778
779 public override Vector3 CenterOfMass
780 {
781 get { return Vector3.Zero; }
782 }
783
784 public override Vector3 GeometricCenter
785 {
786 get { return Vector3.Zero; }
787 }
788
789 public override PrimitiveBaseShape Shape
790 {
791 set { return; }
792 }
793
794 public override Vector3 Velocity
795 {
796 get {
797 // There's a problem with Vector3.Zero! Don't Use it Here!
798 if (_zeroFlag)
799 return Vector3.Zero;
800 m_lastUpdateSent = false;
801 return _velocity;
802 }
803 set
804 {
805 if (value.IsFinite())
806 {
807 _target_velocity = value;
808 m_pidControllerActive = true;
809 }
810 else
811 {
812 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
813 }
814 }
815 }
816
817 public override Vector3 Torque
818 {
819 get { return Vector3.Zero; }
820 set { return; }
821 }
822
823 public override float CollisionScore
824 {
825 get { return 0f; }
826 set { }
827 }
828
829 public override bool Kinematic
830 {
831 get { return false; }
832 set { }
833 }
834
835 public override Quaternion Orientation
836 {
837 get { return Quaternion.Identity; }
838 set {
839 //Matrix3 or = Orientation.ToRotationMatrix();
840 //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
841 //d.BodySetRotation(Body, ref ord);
842 }
843 }
844
845 public override Vector3 Acceleration
846 {
847 get { return _acceleration; }
848 set { _acceleration = value; }
849 }
850
851 public void SetAcceleration(Vector3 accel)
852 {
853 m_pidControllerActive = true;
854 _acceleration = accel;
855 }
856
857 /// <summary>
858 /// Adds the force supplied to the Target Velocity
859 /// The PID controller takes this target velocity and tries to make it a reality
860 /// </summary>
861 /// <param name="force"></param>
862 public override void AddForce(Vector3 force, bool pushforce)
863 {
864 if (force.IsFinite())
865 {
866 if (pushforce)
867 {
868 m_pidControllerActive = false;
869 force *= 100f;
870//Console.WriteLine("DF 1"); // ##
871 if (!force.ApproxEquals(Vector3.Zero, 0.01f))
872 doForce(force);
873 // If uncommented, things get pushed off world
874 //
875 // m_log.Debug("Push!");
876 // _target_velocity.X += force.X;
877 // _target_velocity.Y += force.Y;
878 // _target_velocity.Z += force.Z;
879 }
880 else
881 {
882 m_pidControllerActive = true;
883 _target_velocity.X += force.X;
884 _target_velocity.Y += force.Y;
885 _target_velocity.Z += force.Z;
886 }
887 }
888 else
889 {
890 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
891 }
892 //m_lastUpdateSent = false;
893 }
894
895 public override void AddAngularForce(Vector3 force, bool pushforce)
896 {
897
898 }
899
900 /// <summary>
901 /// After all of the forces add up with 'add force' we apply them with doForce
902 /// </summary>
903 /// <param name="force"></param>
904 public void doForce(Vector3 force)
905 {
906 if (!collidelock)
907 {
908 d.BodyAddForce(Body, force.X, force.Y, force.Z);
909 //d.BodySetRotation(Body, ref m_StandUpRotation);
910 //standupStraight();
911 d.Vector3 vel = d.BodyGetLinearVel(Body); //##
912//Console.WriteLine("AvVel <{0},{1},{2}>", vel.X, vel.Y, vel.Z); //##
913 }
914 }
915
916 public override void SetMomentum(Vector3 momentum)
917 {
918 if (momentum.IsFinite())
919 {
920 m_taintMomentum = momentum;
921 m_haveTaintMomentum = true;
922 _parent_scene.AddPhysicsActorTaint(this);
923 }
924 else
925 m_log.Warn("[PHYSICS] !isFinite momentum");
926 }
927
928
929 /// <summary>
930 /// Called from Simulate
931 /// This is the avatar's movement control + PID Controller
932 /// </summary>
933 /// <param name="timeStep"></param>
934 public void Move(float timeStep, List<OdeCharacter> defects)
935 {
936 // no lock; for now it's only called from within Simulate()
937
938 // If the PID Controller isn't active then we set our force
939 // calculating base velocity to the current position
940
941 if (Body == IntPtr.Zero)
942 return;
943
944 if (m_pidControllerActive == false)
945 {
946 _zeroPosition = d.BodyGetPosition(Body);
947 }
948 //PidStatus = true;
949
950 d.Vector3 localpos = d.BodyGetPosition(Body);
951 Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
952
953 if (!localPos.IsFinite())
954 {
955
956 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
957 defects.Add(this);
958 // _parent_scene.RemoveCharacter(this);
959
960 // destroy avatar capsule and related ODE data
961 if (Amotor != IntPtr.Zero)
962 {
963 // Kill the Amotor
964 d.JointDestroy(Amotor);
965 Amotor = IntPtr.Zero;
966 }
967
968 //kill the Geometry
969 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
970
971 if (Body != IntPtr.Zero)
972 {
973 //kill the body
974 d.BodyDestroy(Body);
975
976 Body = IntPtr.Zero;
977 }
978
979 if(Shell != IntPtr.Zero)
980 {
981 try
982 {
983 d.GeomDestroy(Shell);
984 }
985 catch (System.AccessViolationException)
986 {
987 m_log.Error("[PHYSICS]: PrimGeom dead");
988 }
989 // Remove any old entries
990//string tShell;
991//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
992//Console.WriteLine("**** Remove {0}", tShell);
993
994 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
995 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
996 Shell = IntPtr.Zero;
997 }
998
999 return;
1000 }
1001
1002 Vector3 vec = Vector3.Zero;
1003 d.Vector3 vel = d.BodyGetLinearVel(Body);
1004
1005 float movementdivisor = 1f;
1006
1007 if (!m_alwaysRun)
1008 {
1009 movementdivisor = walkDivisor;
1010 }
1011 else
1012 {
1013 movementdivisor = runDivisor;
1014 }
1015
1016 // if velocity is zero, use position control; otherwise, velocity control
1017 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
1018 {
1019 // keep track of where we stopped. No more slippin' & slidin'
1020 if (!_zeroFlag)
1021 {
1022 _zeroFlag = true;
1023 _zeroPosition = d.BodyGetPosition(Body);
1024 }
1025 if (m_pidControllerActive)
1026 {
1027 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1028 // react to the physics scene by moving it's position.
1029 // Avatar to Avatar collisions
1030 // Prim to avatar collisions
1031
1032 d.Vector3 pos = d.BodyGetPosition(Body);
1033 float errX = _zeroPosition.X - pos.X;
1034 float errY = _zeroPosition.Y - pos.Y;
1035 if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) )
1036 {
1037 vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2);
1038 vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2);
1039 }
1040 else
1041 { // close, jump to lateral destination
1042 d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z);
1043 }
1044// if (flying)
1045 if (flying || jumping) // add for jumping
1046 {
1047 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1048 }
1049 }
1050 //PidStatus = true;
1051 }
1052 else
1053 {
1054 m_pidControllerActive = true;
1055 _zeroFlag = false;
1056 if (m_iscolliding && !flying)
1057 {
1058 // We're standing on something
1059 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
1060 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
1061 }
1062 else if (m_iscolliding && flying)
1063 {
1064 // We're flying and colliding with something
1065 vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
1066 vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
1067 }
1068 else if (!m_iscolliding && flying)
1069 {
1070 // we're in mid air suspended
1071 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
1072 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
1073 }
1074
1075 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
1076 {
1077 // We're colliding with something and we're not flying but we're moving
1078 // This means we're walking or running.
1079 d.Vector3 pos = d.BodyGetPosition(Body);
1080 vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
1081 if (_target_velocity.X > 0)
1082 {
1083 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1084 }
1085 if (_target_velocity.Y > 0)
1086 {
1087 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1088 }
1089 }
1090 else if (!m_iscolliding && !flying)
1091 {
1092 // we're not colliding and we're not flying so that means we're falling!
1093 // m_iscolliding includes collisions with the ground.
1094
1095 // d.Vector3 pos = d.BodyGetPosition(Body);
1096 if (Math.Abs(_target_velocity.X) > 0)
1097 {
1098 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1099 }
1100 if (Math.Abs(_target_velocity.Y) > 0)
1101 {
1102 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1103 }
1104 }
1105
1106 if (flying)
1107 {
1108 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
1109 }
1110 }
1111 if (flying)
1112 {
1113 vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
1114
1115 //Added for auto fly height. Kitto Flora
1116 //d.Vector3 pos = d.BodyGetPosition(Body);
1117 float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
1118
1119 if (_position.Z < target_altitude)
1120 {
1121 vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
1122 }
1123 // end add Kitto Flora
1124 }
1125
1126 if (vel.X * vel.X + vel.Y * vel.Y + vel.Z * vel.Z > 2500.0f) // 50ms apply breaks
1127 {
1128 float breakfactor = 0.16f * m_mass; // will give aprox 60m/s terminal velocity at free fall
1129 vec.X -= breakfactor * vel.X;
1130 vec.Y -= breakfactor * vel.Y;
1131 vec.Z -= breakfactor * vel.Z;
1132 }
1133
1134 if (vec.IsFinite())
1135 {
1136 if (vec.LengthSquared() > 0.0004f) // 0.01 allows 0.002 !!
1137 {
1138//Console.WriteLine("DF 2"); // ##
1139
1140 doForce(vec);
1141 if (!_zeroFlag)
1142 {
1143// AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
1144 }
1145 }
1146 }
1147 else
1148 {
1149 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1150 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1151 defects.Add(this);
1152 // _parent_scene.RemoveCharacter(this);
1153 // destroy avatar capsule and related ODE data
1154 if (Amotor != IntPtr.Zero)
1155 {
1156 // Kill the Amotor
1157 d.JointDestroy(Amotor);
1158 Amotor = IntPtr.Zero;
1159 }
1160 //kill the Geometry
1161 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1162
1163 if (Body != IntPtr.Zero)
1164 {
1165 //kill the body
1166 d.BodyDestroy(Body);
1167
1168 Body = IntPtr.Zero;
1169 }
1170
1171 if(Shell != IntPtr.Zero)
1172 {
1173 try
1174 {
1175 d.GeomDestroy(Shell);
1176 }
1177 catch (System.AccessViolationException)
1178 {
1179 m_log.Error("[PHYSICS]: PrimGeom dead");
1180 }
1181 // Remove any old entries
1182//string tShell;
1183//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1184//Console.WriteLine("**** Remove {0}", tShell);
1185
1186 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1187 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1188 Shell = IntPtr.Zero;
1189 }
1190 }
1191 }
1192
1193 /// <summary>
1194 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1195 /// </summary>
1196 public void UpdatePositionAndVelocity()
1197 {
1198 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1199 d.Vector3 vec;
1200 try
1201 {
1202 vec = d.BodyGetPosition(Body);
1203 }
1204 catch (NullReferenceException)
1205 {
1206 bad = true;
1207 _parent_scene.BadCharacter(this);
1208 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1209 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1210 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1211 }
1212
1213
1214 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
1215 if (vec.X < 0.0f) vec.X = 0.0f;
1216 if (vec.Y < 0.0f) vec.Y = 0.0f;
1217 if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
1218 if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
1219
1220 _position.X = vec.X;
1221 _position.Y = vec.Y;
1222 _position.Z = vec.Z;
1223
1224 // Did we move last? = zeroflag
1225 // This helps keep us from sliding all over
1226
1227 if (_zeroFlag)
1228 {
1229 _velocity.X = 0.0f;
1230 _velocity.Y = 0.0f;
1231 _velocity.Z = 0.0f;
1232
1233 // Did we send out the 'stopped' message?
1234 if (!m_lastUpdateSent)
1235 {
1236 m_lastUpdateSent = true;
1237 //base.RequestPhysicsterseUpdate();
1238
1239 }
1240 }
1241 else
1242 {
1243 m_lastUpdateSent = false;
1244 try
1245 {
1246 vec = d.BodyGetLinearVel(Body);
1247 }
1248 catch (NullReferenceException)
1249 {
1250 vec.X = _velocity.X;
1251 vec.Y = _velocity.Y;
1252 vec.Z = _velocity.Z;
1253 }
1254 _velocity.X = (vec.X);
1255 _velocity.Y = (vec.Y);
1256
1257 _velocity.Z = (vec.Z);
1258
1259 if (_velocity.Z < -6 && !m_hackSentFall)
1260 {
1261 m_hackSentFall = true;
1262 m_pidControllerActive = false;
1263 }
1264 else if (flying && !m_hackSentFly)
1265 {
1266 //m_hackSentFly = true;
1267 //base.SendCollisionUpdate(new CollisionEventUpdate());
1268 }
1269 else
1270 {
1271 m_hackSentFly = false;
1272 m_hackSentFall = false;
1273 }
1274 }
1275 }
1276
1277 /// <summary>
1278 /// Cleanup the things we use in the scene.
1279 /// </summary>
1280 public void Destroy()
1281 {
1282 m_tainted_isPhysical = false;
1283 _parent_scene.AddPhysicsActorTaint(this);
1284 }
1285
1286 public override void CrossingFailure()
1287 {
1288 }
1289
1290 public override Vector3 PIDTarget { set { return; } }
1291 public override bool PIDActive { set { return; } }
1292 public override float PIDTau { set { return; } }
1293
1294 public override float PIDHoverHeight { set { return; } }
1295 public override bool PIDHoverActive { set { return; } }
1296 public override PIDHoverType PIDHoverType { set { return; } }
1297 public override float PIDHoverTau { set { return; } }
1298
1299 public override Quaternion APIDTarget{ set { return; } }
1300
1301 public override bool APIDActive{ set { return; } }
1302
1303 public override float APIDStrength{ set { return; } }
1304
1305 public override float APIDDamping{ set { return; } }
1306
1307
1308 public override void SubscribeEvents(int ms)
1309 {
1310 m_requestedUpdateFrequency = ms;
1311 m_eventsubscription = ms;
1312 _parent_scene.addCollisionEventReporting(this);
1313 }
1314 public override void UnSubscribeEvents()
1315 {
1316 _parent_scene.remCollisionEventReporting(this);
1317 m_requestedUpdateFrequency = 0;
1318 m_eventsubscription = 0;
1319 }
1320 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1321 {
1322 if (m_eventsubscription > 0)
1323 {
1324 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1325 }
1326 }
1327
1328 public void SendCollisions()
1329 {
1330 if (m_eventsubscription > m_requestedUpdateFrequency)
1331 {
1332 if (CollisionEventsThisFrame != null)
1333 {
1334 base.SendCollisionUpdate(CollisionEventsThisFrame);
1335 }
1336 CollisionEventsThisFrame = new CollisionEventUpdate();
1337 m_eventsubscription = 0;
1338 }
1339 }
1340 public override bool SubscribedEvents()
1341 {
1342 if (m_eventsubscription > 0)
1343 return true;
1344 return false;
1345 }
1346
1347 public void ProcessTaints(float timestep)
1348 {
1349 lock (m_syncRoot)
1350 {
1351 if (m_tainted_isPhysical != m_isPhysical)
1352 {
1353 if (m_tainted_isPhysical)
1354 {
1355 // Create avatar capsule and related ODE data
1356 if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
1357 {
1358 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1359 + (Shell!=IntPtr.Zero ? "Shell ":"")
1360 + (Body!=IntPtr.Zero ? "Body ":"")
1361 + (Amotor!=IntPtr.Zero ? "Amotor ":""));
1362 }
1363 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
1364 _parent_scene.AddCharacter(this);
1365 }
1366 else
1367 {
1368 _parent_scene.RemoveCharacter(this);
1369 // destroy avatar capsule and related ODE data
1370 if (Amotor != IntPtr.Zero)
1371 {
1372 // Kill the Amotor
1373 d.JointDestroy(Amotor);
1374 Amotor = IntPtr.Zero;
1375 }
1376 //kill the Geometry
1377 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1378
1379 if (Body != IntPtr.Zero)
1380 {
1381 //kill the body
1382 d.BodyDestroy(Body);
1383 Body = IntPtr.Zero;
1384 }
1385
1386 if(Shell != IntPtr.Zero)
1387 {
1388 try
1389 {
1390 d.GeomDestroy(Shell);
1391 }
1392 catch (Exception e)
1393 {
1394 m_log.ErrorFormat("[PHYSICS]: Failed to destroy character shell {0}",e.Message);
1395 }
1396 // Remove any old entries
1397 //string tShell;
1398 //_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1399 //Console.WriteLine("**** Remove {0}", tShell);
1400
1401 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1402 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1403 Shell = IntPtr.Zero;
1404 }
1405 }
1406
1407 m_isPhysical = m_tainted_isPhysical;
1408 }
1409
1410 if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
1411 {
1412 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1413 {
1414
1415 m_pidControllerActive = true;
1416 // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
1417 d.JointDestroy(Amotor);
1418 float prevCapsule = CAPSULE_LENGTH;
1419 CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
1420 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
1421 d.BodyDestroy(Body);
1422 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1423 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
1424 Velocity = Vector3.Zero;
1425 }
1426 else
1427 {
1428 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1429 + (Shell==IntPtr.Zero ? "Shell ":"")
1430 + (Body==IntPtr.Zero ? "Body ":"")
1431 + (Amotor==IntPtr.Zero ? "Amotor ":""));
1432 }
1433 }
1434
1435 if (!m_taintPosition.ApproxEquals(_position, 0.05f))
1436 {
1437 if (Body != IntPtr.Zero)
1438 {
1439 d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
1440
1441 }
1442 _position.X = m_taintPosition.X;
1443 _position.Y = m_taintPosition.Y;
1444 _position.Z = m_taintPosition.Z;
1445 }
1446
1447 if (m_haveTaintMomentum)
1448 {
1449 m_haveTaintMomentum = false;
1450 _velocity = m_taintMomentum;
1451 _target_velocity = m_taintMomentum;
1452 m_pidControllerActive = true;
1453 if (Body != IntPtr.Zero)
1454 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1455 }
1456 }
1457 }
1458
1459 internal void AddCollisionFrameTime(int p)
1460 {
1461 // protect it from overflow crashing
1462 if (m_eventsubscription + p >= int.MaxValue)
1463 m_eventsubscription = 0;
1464 m_eventsubscription += p;
1465 }
1466 }
1467}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..ba24aa7
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,4124 @@
1/* Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders.
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright
6 * notice, this list of conditions and the following disclaimer.
7 * * Redistributions in binary form must reproduce the above copyright
8 * notice, this list of conditions and the following disclaimer in the
9 * documentation and/or other materials provided with the distribution.
10 * * Neither the name of the OpenSimulator Project nor the
11 * names of its contributors may be used to endorse or promote products
12 * derived from this software without specific prior written permission.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
15 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
16 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
18 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
19 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
21 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 *
25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * Ubit 2012
27 * rolled into ODEPrim.cs
28 */
29
30using System;
31using System.IO;
32using System.Collections.Generic;
33using System.Reflection;
34using System.Runtime.InteropServices;
35using System.Threading;
36using log4net;
37using OpenMetaverse;
38using Ode.NET;
39using OpenSim.Framework;
40using OpenSim.Region.Physics.Manager;
41
42namespace OpenSim.Region.Physics.OdePlugin
43{
44 /// <summary>
45 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
46 /// </summary>
47
48 public class OdePrim : PhysicsActor
49 {
50 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
51
52 public class SerialControl
53 {
54 public object alock = new object();
55 public byte[] data = new byte[0];
56 }
57 private Vector3 _position;
58 private Vector3 _velocity;
59 private Vector3 _torque;
60 private Vector3 m_lastVelocity;
61 private Vector3 m_lastposition;
62 private Quaternion m_lastorientation = new Quaternion();
63 private Vector3 m_rotationalVelocity;
64 private Vector3 _size;
65 private Vector3 _acceleration;
66 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
67 private Quaternion _orientation;
68 private Vector3 m_taintposition;
69 private Vector3 m_taintsize;
70 private Vector3 m_taintVelocity;
71 private Vector3 m_taintTorque;
72 private Quaternion m_taintrot;
73 private Vector3 m_rotateEnable = Vector3.One; // Current setting
74 private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL
75 private bool m_rotateEnableUpdate = false;
76 private Vector3 m_lockX;
77 private Vector3 m_lockY;
78 private Vector3 m_lockZ;
79 private IntPtr Amotor = IntPtr.Zero;
80 private IntPtr AmotorX = IntPtr.Zero;
81 private IntPtr AmotorY = IntPtr.Zero;
82 private IntPtr AmotorZ = IntPtr.Zero;
83
84 private Vector3 m_PIDTarget;
85 private float m_PIDTau;
86 private float PID_D = 35f;
87 private float PID_G = 25f;
88 private bool m_usePID = false;
89
90 private Quaternion m_APIDTarget = new Quaternion();
91 private float m_APIDStrength = 0.5f;
92 private float m_APIDDamping = 0.5f;
93 private bool m_useAPID = false;
94 private float m_APIDdamper = 1.0f;
95
96 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
97 // do not confuse with VEHICLE HOVER
98
99 private float m_PIDHoverHeight;
100 private float m_PIDHoverTau;
101 private bool m_useHoverPID;
102 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
103 private float m_targetHoverHeight;
104 private float m_groundHeight;
105 private float m_waterHeight;
106 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
107
108 // private float m_tensor = 5f;
109 private int body_autodisable_frames = 20;
110
111
112 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
113 | CollisionCategories.Space
114 | CollisionCategories.Body
115 | CollisionCategories.Character
116 );
117 private bool m_taintshape;
118 private bool m_taintPhysics;
119 private bool m_collidesLand = true;
120 private bool m_collidesWater;
121 // public bool m_returnCollisions;
122
123 // Default we're a Geometry
124 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
125
126 // Default, Collide with Other Geometries, spaces and Bodies
127 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
128
129 public bool m_taintremove;
130 public bool m_taintdisable;
131 public bool m_disabled;
132 public bool m_taintadd;
133 public bool m_taintselected;
134 public bool m_taintphantom;
135 public bool m_taintCollidesWater;
136
137 public uint m_localID;
138
139 //public GCHandle gc;
140 private CollisionLocker ode;
141
142 private bool m_meshfailed = false;
143 private bool m_taintforce = false;
144 private bool m_taintaddangularforce = false;
145 private Vector3 m_force;
146 private List<Vector3> m_forcelist = new List<Vector3>();
147 private List<Vector3> m_angularforcelist = new List<Vector3>();
148
149 private IMesh _mesh;
150 private PrimitiveBaseShape _pbs;
151 private OdeScene _parent_scene;
152 public IntPtr m_targetSpace = IntPtr.Zero;
153 public IntPtr prim_geom;
154 // public IntPtr prev_geom;
155 public IntPtr _triMeshData;
156
157 private IntPtr _linkJointGroup = IntPtr.Zero;
158 private PhysicsActor _parent;
159 private PhysicsActor m_taintparent;
160
161 private List<OdePrim> childrenPrim = new List<OdePrim>();
162
163 private bool iscolliding;
164 private bool m_isphysical;
165 private bool m_isphantom;
166 private bool m_isSelected;
167
168 private bool m_NoColide; // for now only for internal use for bad meshs
169
170 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
171
172 private bool m_throttleUpdates;
173 private int throttleCounter;
174 public int m_interpenetrationcount;
175 public float m_collisionscore;
176 // public int m_roundsUnderMotionThreshold;
177 // private int m_crossingfailures;
178
179 public bool m_outofBounds;
180 private float m_density = 10.000006836f; // Aluminum g/cm3;
181
182 private float m_primMass = 10.000006836f; // Aluminum g/cm3;
183
184 private byte m_shapetype;
185 private byte m_taintshapetype;
186
187 public bool _zeroFlag; // if body has been stopped
188 private bool m_lastUpdateSent;
189
190 public IntPtr Body = IntPtr.Zero;
191 public String m_primName;
192 private Vector3 _target_velocity;
193 public d.Mass pMass;
194
195 public int m_eventsubscription;
196 private CollisionEventUpdate CollisionEventsThisFrame;
197
198 private IntPtr m_linkJoint = IntPtr.Zero;
199
200 public volatile bool childPrim;
201
202 internal int m_material = (int)Material.Wood;
203
204 private IntPtr m_body = IntPtr.Zero;
205
206 // Vehicle properties ============================================================================================
207 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
208 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
209 private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings:
210 // HOVER_TERRAIN_ONLY
211 // HOVER_GLOBAL_HEIGHT
212 // NO_DEFLECTION_UP
213 // HOVER_WATER_ONLY
214 // HOVER_UP_ONLY
215 // LIMIT_MOTOR_UP
216 // LIMIT_ROLL_ONLY
217
218 // Linear properties
219 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
220 //requested by LSL
221 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
222 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
223 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
224
225 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
226 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
227 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
228
229 //Angular properties
230 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
231
232 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
233 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
234 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
235
236 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
237 // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
238 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
239
240 //Deflection properties
241 // private float m_angularDeflectionEfficiency = 0;
242 // private float m_angularDeflectionTimescale = 0;
243 // private float m_linearDeflectionEfficiency = 0;
244 // private float m_linearDeflectionTimescale = 0;
245
246 //Banking properties
247 // private float m_bankingEfficiency = 0;
248 // private float m_bankingMix = 0;
249 // private float m_bankingTimescale = 0;
250
251 //Hover and Buoyancy properties
252 private float m_VhoverHeight = 0f;
253 // private float m_VhoverEfficiency = 0f;
254 private float m_VhoverTimescale = 0f;
255 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
256 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
257 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
258 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
259 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
260
261 //Attractor properties
262 private float m_verticalAttractionEfficiency = 1.0f; // damped
263 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
264
265// SerialControl m_taintserial = null;
266 object m_taintvehicledata = null;
267
268 public void DoSetVehicle()
269 {
270 VehicleData vd = (VehicleData)m_taintvehicledata;
271
272 m_type = vd.m_type;
273 m_flags = vd.m_flags;
274
275 // Linear properties
276 m_linearMotorDirection = vd.m_linearMotorDirection;
277 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
278 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
279 m_linearMotorTimescale = vd.m_linearMotorTimescale;
280// m_linearMotorOffset = vd.m_linearMotorOffset;
281
282 //Angular properties
283 m_angularMotorDirection = vd.m_angularMotorDirection;
284 m_angularMotorTimescale = vd.m_angularMotorTimescale;
285 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
286 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
287
288 //Deflection properties
289// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
290// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
291// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
292// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
293
294 //Banking properties
295// m_bankingEfficiency = vd.m_bankingEfficiency;
296// m_bankingMix = vd.m_bankingMix;
297// m_bankingTimescale = vd.m_bankingTimescale;
298
299 //Hover and Buoyancy properties
300 m_VhoverHeight = vd.m_VhoverHeight;
301// m_VhoverEfficiency = vd.m_VhoverEfficiency;
302 m_VhoverTimescale = vd.m_VhoverTimescale;
303 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
304
305 //Attractor properties
306 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
307 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
308
309 // Axis
310// m_referenceFrame = vd.m_referenceFrame;
311
312
313 m_taintvehicledata = null;
314 }
315
316 public override void SetVehicle(object vdata)
317 {
318 m_taintvehicledata = vdata;
319 _parent_scene.AddPhysicsActorTaint(this);
320 }
321
322 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
323 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical,
324 bool pisPhantom,byte shapetype, CollisionLocker dode, uint localid)
325 {
326 m_localID = localid;
327 ode = dode;
328 if (!pos.IsFinite())
329 {
330 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
331 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
332 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
333 }
334
335 _position = pos;
336 m_taintposition = pos;
337 PID_D = parent_scene.bodyPIDD;
338 PID_G = parent_scene.bodyPIDG;
339 m_density = parent_scene.geomDefaultDensity;
340 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
341 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
342
343 prim_geom = IntPtr.Zero;
344 // prev_geom = IntPtr.Zero;
345
346 if (!pos.IsFinite())
347 {
348 size = new Vector3(0.5f, 0.5f, 0.5f);
349 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
350 }
351
352 if (size.X <= 0) size.X = 0.01f;
353 if (size.Y <= 0) size.Y = 0.01f;
354 if (size.Z <= 0) size.Z = 0.01f;
355
356 _size = size;
357 m_taintsize = _size;
358
359 if (!QuaternionIsFinite(rotation))
360 {
361 rotation = Quaternion.Identity;
362 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
363 }
364
365 _orientation = rotation;
366 m_taintrot = _orientation;
367 _mesh = mesh;
368 _pbs = pbs;
369 m_shapetype = shapetype;
370 m_taintshapetype = shapetype;
371
372 _parent_scene = parent_scene;
373 m_targetSpace = (IntPtr)0;
374
375 // if (pos.Z < 0)
376 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
377 m_isphysical = false;
378 else
379 {
380 m_isphysical = pisPhysical;
381 // If we're physical, we need to be in the master space for now.
382 // linksets *should* be in a space together.. but are not currently
383 if (m_isphysical)
384 m_targetSpace = _parent_scene.space;
385 }
386
387 m_isphantom = pisPhantom;
388 m_taintphantom = pisPhantom;
389
390 _triMeshData = IntPtr.Zero;
391 m_NoColide = false;
392
393// m_taintserial = null;
394 m_primName = primName;
395 m_taintadd = true;
396 _parent_scene.AddPhysicsActorTaint(this);
397 // don't do .add() here; old geoms get recycled with the same hash
398 }
399
400 public override int PhysicsActorType
401 {
402 get { return (int)ActorTypes.Prim; }
403 set { return; }
404 }
405
406 public override bool SetAlwaysRun
407 {
408 get { return false; }
409 set { return; }
410 }
411
412 public override uint LocalID
413 {
414 set
415 {
416 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
417 m_localID = value;
418 }
419 }
420
421 public override bool Grabbed
422 {
423 set { return; }
424 }
425
426 public override bool Selected
427 {
428 set
429 {
430 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
431 // This only makes the object not collidable if the object
432 // is physical or the object is modified somehow *IN THE FUTURE*
433 // without this, if an avatar selects prim, they can walk right
434 // through it while it's selected
435 m_collisionscore = 0;
436 if ((m_isphysical && !_zeroFlag) || !value)
437 {
438 m_taintselected = value;
439 _parent_scene.AddPhysicsActorTaint(this);
440 }
441 else
442 {
443 m_taintselected = value;
444 m_isSelected = value;
445 }
446 if (m_isSelected) disableBodySoft();
447 }
448 }
449
450 public override bool IsPhysical
451 {
452 get { return m_isphysical; }
453 set
454 {
455 m_isphysical = value;
456 if (!m_isphysical)
457 { // Zero the remembered last velocity
458 m_lastVelocity = Vector3.Zero;
459 if (m_type != Vehicle.TYPE_NONE) Halt();
460 }
461 }
462 }
463
464 public override bool IsVolumeDtc
465 {
466 set { return; }
467 get { return m_isVolumeDetect; }
468
469 }
470
471 public override bool Phantom
472 {
473 get { return m_isphantom; }
474 set
475 {
476 m_isphantom = value;
477 }
478 }
479
480 public void setPrimForRemoval()
481 {
482 m_taintremove = true;
483 }
484
485 public override bool Flying
486 {
487 // no flying prims for you
488 get { return false; }
489 set { }
490 }
491
492 public override bool IsColliding
493 {
494 get { return iscolliding; }
495 set { iscolliding = value; }
496 }
497
498 public override bool CollidingGround
499 {
500 get { return false; }
501 set { return; }
502 }
503
504 public override bool CollidingObj
505 {
506 get { return false; }
507 set { return; }
508 }
509
510 public override bool ThrottleUpdates
511 {
512 get { return m_throttleUpdates; }
513 set { m_throttleUpdates = value; }
514 }
515
516 public override bool Stopped
517 {
518 get { return _zeroFlag; }
519 }
520
521 public override Vector3 Position
522 {
523 get { return _position; }
524
525 set
526 {
527 _position = value;
528 //m_log.Info("[PHYSICS]: " + _position.ToString());
529 }
530 }
531
532 public override Vector3 Size
533 {
534 get { return _size; }
535 set
536 {
537 if (value.IsFinite())
538 {
539 _size = value;
540 }
541 else
542 {
543 m_log.Warn("[PHYSICS]: Got NaN Size on object");
544 }
545 }
546 }
547
548 public override float Mass
549 {
550 get
551 {
552 CalculateMass();
553 return m_primMass;
554 }
555 }
556
557 public override Vector3 Force
558 {
559 //get { return Vector3.Zero; }
560 get { return m_force; }
561 set
562 {
563 if (value.IsFinite())
564 {
565 m_force = value;
566 }
567 else
568 {
569 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
570 }
571 }
572 }
573
574 public override int VehicleType
575 {
576 get { return (int)m_type; }
577 set { ProcessTypeChange((Vehicle)value); }
578 }
579
580 public override void VehicleFloatParam(int param, float value)
581 {
582 ProcessFloatVehicleParam((Vehicle)param, value);
583 }
584
585 public override void VehicleVectorParam(int param, Vector3 value)
586 {
587 ProcessVectorVehicleParam((Vehicle)param, value);
588 }
589
590 public override void VehicleRotationParam(int param, Quaternion rotation)
591 {
592 ProcessRotationVehicleParam((Vehicle)param, rotation);
593 }
594
595 public override void VehicleFlags(int param, bool remove)
596 {
597 ProcessVehicleFlags(param, remove);
598 }
599
600 public override void SetVolumeDetect(int param)
601 {
602 lock (_parent_scene.OdeLock)
603 {
604 m_isVolumeDetect = (param != 0);
605 }
606 }
607
608
609 public override Vector3 CenterOfMass
610 {
611 get { return Vector3.Zero; }
612 }
613
614 public override Vector3 GeometricCenter
615 {
616 get { return Vector3.Zero; }
617 }
618
619 public override PrimitiveBaseShape Shape
620 {
621 set
622 {
623 _pbs = value;
624 m_taintshape = true;
625 }
626 }
627
628 public override byte PhysicsShapeType
629 {
630 get
631 {
632 return m_shapetype;
633 }
634 set
635 {
636 m_taintshapetype = value;
637 _parent_scene.AddPhysicsActorTaint(this);
638 }
639 }
640
641 public override Vector3 Velocity
642 {
643 get
644 {
645 // Averate previous velocity with the new one so
646 // client object interpolation works a 'little' better
647 if (_zeroFlag)
648 return Vector3.Zero;
649
650 Vector3 returnVelocity = Vector3.Zero;
651 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
652 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
653 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
654 return returnVelocity;
655 }
656 set
657 {
658 if (value.IsFinite())
659 {
660 _velocity = value;
661 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
662 _acceleration = Vector3.Zero;
663
664 m_taintVelocity = value;
665 _parent_scene.AddPhysicsActorTaint(this);
666 }
667 else
668 {
669 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
670 }
671
672 }
673 }
674
675 public override Vector3 Torque
676 {
677 get
678 {
679 if (!m_isphysical || Body == IntPtr.Zero)
680 return Vector3.Zero;
681
682 return _torque;
683 }
684
685 set
686 {
687 if (value.IsFinite())
688 {
689 m_taintTorque = value;
690 _parent_scene.AddPhysicsActorTaint(this);
691 }
692 else
693 {
694 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
695 }
696 }
697 }
698
699 public override float CollisionScore
700 {
701 get { return m_collisionscore; }
702 set { m_collisionscore = value; }
703 }
704
705 public override bool Kinematic
706 {
707 get { return false; }
708 set { }
709 }
710
711 public override Quaternion Orientation
712 {
713 get { return _orientation; }
714 set
715 {
716 if (QuaternionIsFinite(value))
717 {
718 _orientation = value;
719 }
720 else
721 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
722
723 }
724 }
725
726 public override bool FloatOnWater
727 {
728 set
729 {
730 m_taintCollidesWater = value;
731 _parent_scene.AddPhysicsActorTaint(this);
732 }
733 }
734
735 public override void SetMomentum(Vector3 momentum)
736 {
737 }
738
739 public override Vector3 PIDTarget
740 {
741 set
742 {
743 if (value.IsFinite())
744 {
745 m_PIDTarget = value;
746 }
747 else
748 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
749 }
750 }
751 public override bool PIDActive { set { m_usePID = value; } }
752 public override float PIDTau { set { m_PIDTau = value; } }
753
754 // For RotLookAt
755 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
756 public override bool APIDActive { set { m_useAPID = value; } }
757 public override float APIDStrength { set { m_APIDStrength = value; } }
758 public override float APIDDamping { set { m_APIDDamping = value; } }
759
760 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
761 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
762 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
763 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
764
765 internal static bool QuaternionIsFinite(Quaternion q)
766 {
767 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
768 return false;
769 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
770 return false;
771 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
772 return false;
773 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
774 return false;
775 return true;
776 }
777
778 public override Vector3 Acceleration // client updates read data via here
779 {
780 get
781 {
782 if (_zeroFlag)
783 {
784 return Vector3.Zero;
785 }
786 return _acceleration;
787 }
788 set { _acceleration = value; }
789 }
790
791
792 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
793 {
794 _acceleration = accel;
795 }
796
797 public override void AddForce(Vector3 force, bool pushforce)
798 {
799 if (force.IsFinite())
800 {
801 lock (m_forcelist)
802 m_forcelist.Add(force);
803
804 m_taintforce = true;
805 }
806 else
807 {
808 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
809 }
810 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
811 }
812
813 public override void AddAngularForce(Vector3 force, bool pushforce)
814 {
815 if (force.IsFinite())
816 {
817 m_angularforcelist.Add(force);
818 m_taintaddangularforce = true;
819 }
820 else
821 {
822 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
823 }
824 }
825
826 public override Vector3 RotationalVelocity
827 {
828 get
829 {
830 return m_rotationalVelocity;
831 }
832 set
833 {
834 if (value.IsFinite())
835 {
836 m_rotationalVelocity = value;
837 }
838 else
839 {
840 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
841 }
842 }
843 }
844
845 public override void CrossingFailure()
846 {
847 if (m_outofBounds)
848 {
849 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
850 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
851 _position.Z = Util.Clip(_position.Z, -100f, 50000f);
852 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
853
854 m_lastposition = _position;
855
856 _velocity = Vector3.Zero;
857 m_lastVelocity = _velocity;
858
859
860 if (m_type != Vehicle.TYPE_NONE)
861 Halt();
862
863 d.BodySetLinearVel(Body, 0, 0, 0);
864 base.RequestPhysicsterseUpdate();
865 m_outofBounds = false;
866 }
867 /*
868 int tmp = Interlocked.Increment(ref m_crossingfailures);
869 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
870 {
871 base.RaiseOutOfBounds(_position);
872 return;
873 }
874 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
875 {
876 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
877 }
878 */
879 }
880
881 public override float Buoyancy
882 {
883 get { return m_buoyancy; }
884 set { m_buoyancy = value; }
885 }
886
887 public override void link(PhysicsActor obj)
888 {
889 m_taintparent = obj;
890 }
891
892 public override void delink()
893 {
894 m_taintparent = null;
895 }
896
897 public override void LockAngularMotion(Vector3 axis)
898 {
899 // This is actually ROTATION ENABLE, not a lock.
900 // default is <1,1,1> which is all enabled.
901 // The lock value is updated inside Move(), no point in using the taint system.
902 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
903 if (axis.IsFinite())
904 {
905 axis.X = (axis.X > 0) ? 1f : 0f;
906 axis.Y = (axis.Y > 0) ? 1f : 0f;
907 axis.Z = (axis.Z > 0) ? 1f : 0f;
908 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
909 m_rotateEnableRequest = axis;
910 m_rotateEnableUpdate = true;
911 }
912 else
913 {
914 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
915 }
916 }
917
918 public void SetGeom(IntPtr geom)
919 {
920 if (prim_geom != IntPtr.Zero)
921 {
922 // Remove any old entries
923 //string tPA;
924 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
925 //Console.WriteLine("**** Remove {0}", tPA);
926 if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
927 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
928 d.GeomDestroy(prim_geom);
929 }
930
931 prim_geom = geom;
932 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
933 if (prim_geom != IntPtr.Zero)
934 {
935 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
936 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
937 //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
938 if (m_NoColide)
939 {
940 d.GeomSetCategoryBits(prim_geom, 0);
941 if (m_isphysical && !m_isVolumeDetect)
942 {
943 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
944 }
945 else
946 {
947 d.GeomSetCollideBits(prim_geom, 0);
948 d.GeomDisable(prim_geom);
949 }
950 }
951 else
952 {
953 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
954 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
955 }
956 }
957
958 if (childPrim)
959 {
960 if (_parent != null && _parent is OdePrim)
961 {
962 OdePrim parent = (OdePrim)_parent;
963 //Console.WriteLine("SetGeom calls ChildSetGeom");
964 parent.ChildSetGeom(this);
965 }
966 }
967 //m_log.Warn("Setting Geom to: " + prim_geom);
968 }
969
970 public void enableBodySoft()
971 {
972 if (!childPrim)
973 {
974 if (m_isphysical && Body != IntPtr.Zero)
975 {
976 d.BodyEnable(Body);
977 if (m_type != Vehicle.TYPE_NONE)
978 Enable(Body, _parent_scene);
979 }
980
981 m_disabled = false;
982 }
983 }
984
985 public void disableBodySoft()
986 {
987 m_disabled = true;
988
989 if (m_isphysical && Body != IntPtr.Zero)
990 {
991 d.BodyDisable(Body);
992 Halt();
993 }
994 }
995
996 public void enableBody()
997 {
998 // Don't enable this body if we're a child prim
999 // this should be taken care of in the parent function not here
1000 if (!childPrim)
1001 {
1002 // Sets the geom to a body
1003 Body = d.BodyCreate(_parent_scene.world);
1004
1005 setMass();
1006 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1007 d.Quaternion myrot = new d.Quaternion();
1008 myrot.X = _orientation.X;
1009 myrot.Y = _orientation.Y;
1010 myrot.Z = _orientation.Z;
1011 myrot.W = _orientation.W;
1012 d.BodySetQuaternion(Body, ref myrot);
1013 d.GeomSetBody(prim_geom, Body);
1014
1015 m_collisionCategories |= CollisionCategories.Body;
1016 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1017
1018 if (m_NoColide)
1019 {
1020 d.GeomSetCategoryBits(prim_geom, 0);
1021 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1022 }
1023 else
1024 {
1025 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1026 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1027 }
1028
1029 d.BodySetAutoDisableFlag(Body, true);
1030 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1031
1032 // disconnect from world gravity so we can apply buoyancy
1033 d.BodySetGravityMode(Body, false);
1034
1035 m_interpenetrationcount = 0;
1036 m_collisionscore = 0;
1037 m_disabled = false;
1038
1039 if (m_type != Vehicle.TYPE_NONE)
1040 {
1041 Enable(Body, _parent_scene);
1042 }
1043
1044 _parent_scene.addActivePrim(this);
1045 }
1046 }
1047
1048 #region Mass Calculation
1049
1050 private float CalculateMass()
1051 {
1052 float volume = _size.X * _size.Y * _size.Z; // default
1053 float tmp;
1054
1055 float returnMass = 0;
1056 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1057 float hollowVolume = hollowAmount * hollowAmount;
1058
1059 switch (_pbs.ProfileShape)
1060 {
1061 case ProfileShape.Square:
1062 // default box
1063
1064 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1065 {
1066 if (hollowAmount > 0.0)
1067 {
1068 switch (_pbs.HollowShape)
1069 {
1070 case HollowShape.Square:
1071 case HollowShape.Same:
1072 break;
1073
1074 case HollowShape.Circle:
1075
1076 hollowVolume *= 0.78539816339f;
1077 break;
1078
1079 case HollowShape.Triangle:
1080
1081 hollowVolume *= (0.5f * .5f);
1082 break;
1083
1084 default:
1085 hollowVolume = 0;
1086 break;
1087 }
1088 volume *= (1.0f - hollowVolume);
1089 }
1090 }
1091
1092 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1093 {
1094 //a tube
1095
1096 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1097 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1098 volume -= volume * tmp * tmp;
1099
1100 if (hollowAmount > 0.0)
1101 {
1102 hollowVolume *= hollowAmount;
1103
1104 switch (_pbs.HollowShape)
1105 {
1106 case HollowShape.Square:
1107 case HollowShape.Same:
1108 break;
1109
1110 case HollowShape.Circle:
1111 hollowVolume *= 0.78539816339f; ;
1112 break;
1113
1114 case HollowShape.Triangle:
1115 hollowVolume *= 0.5f * 0.5f;
1116 break;
1117 default:
1118 hollowVolume = 0;
1119 break;
1120 }
1121 volume *= (1.0f - hollowVolume);
1122 }
1123 }
1124
1125 break;
1126
1127 case ProfileShape.Circle:
1128
1129 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1130 {
1131 volume *= 0.78539816339f; // elipse base
1132
1133 if (hollowAmount > 0.0)
1134 {
1135 switch (_pbs.HollowShape)
1136 {
1137 case HollowShape.Same:
1138 case HollowShape.Circle:
1139 break;
1140
1141 case HollowShape.Square:
1142 hollowVolume *= 0.5f * 2.5984480504799f;
1143 break;
1144
1145 case HollowShape.Triangle:
1146 hollowVolume *= .5f * 1.27323954473516f;
1147 break;
1148
1149 default:
1150 hollowVolume = 0;
1151 break;
1152 }
1153 volume *= (1.0f - hollowVolume);
1154 }
1155 }
1156
1157 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1158 {
1159 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1160 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1161 volume *= (1.0f - tmp * tmp);
1162
1163 if (hollowAmount > 0.0)
1164 {
1165
1166 // calculate the hollow volume by it's shape compared to the prim shape
1167 hollowVolume *= hollowAmount;
1168
1169 switch (_pbs.HollowShape)
1170 {
1171 case HollowShape.Same:
1172 case HollowShape.Circle:
1173 break;
1174
1175 case HollowShape.Square:
1176 hollowVolume *= 0.5f * 2.5984480504799f;
1177 break;
1178
1179 case HollowShape.Triangle:
1180 hollowVolume *= .5f * 1.27323954473516f;
1181 break;
1182
1183 default:
1184 hollowVolume = 0;
1185 break;
1186 }
1187 volume *= (1.0f - hollowVolume);
1188 }
1189 }
1190 break;
1191
1192 case ProfileShape.HalfCircle:
1193 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1194 {
1195 volume *= 0.52359877559829887307710723054658f;
1196 }
1197 break;
1198
1199 case ProfileShape.EquilateralTriangle:
1200
1201 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1202 {
1203 volume *= 0.32475953f;
1204
1205 if (hollowAmount > 0.0)
1206 {
1207
1208 // calculate the hollow volume by it's shape compared to the prim shape
1209 switch (_pbs.HollowShape)
1210 {
1211 case HollowShape.Same:
1212 case HollowShape.Triangle:
1213 hollowVolume *= .25f;
1214 break;
1215
1216 case HollowShape.Square:
1217 hollowVolume *= 0.499849f * 3.07920140172638f;
1218 break;
1219
1220 case HollowShape.Circle:
1221 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1222 // Cyllinder hollow volume calculation
1223
1224 hollowVolume *= 0.1963495f * 3.07920140172638f;
1225 break;
1226
1227 default:
1228 hollowVolume = 0;
1229 break;
1230 }
1231 volume *= (1.0f - hollowVolume);
1232 }
1233 }
1234 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1235 {
1236 volume *= 0.32475953f;
1237 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1238 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1239 volume *= (1.0f - tmp * tmp);
1240
1241 if (hollowAmount > 0.0)
1242 {
1243
1244 hollowVolume *= hollowAmount;
1245
1246 switch (_pbs.HollowShape)
1247 {
1248 case HollowShape.Same:
1249 case HollowShape.Triangle:
1250 hollowVolume *= .25f;
1251 break;
1252
1253 case HollowShape.Square:
1254 hollowVolume *= 0.499849f * 3.07920140172638f;
1255 break;
1256
1257 case HollowShape.Circle:
1258
1259 hollowVolume *= 0.1963495f * 3.07920140172638f;
1260 break;
1261
1262 default:
1263 hollowVolume = 0;
1264 break;
1265 }
1266 volume *= (1.0f - hollowVolume);
1267 }
1268 }
1269 break;
1270
1271 default:
1272 break;
1273 }
1274
1275
1276
1277 float taperX1;
1278 float taperY1;
1279 float taperX;
1280 float taperY;
1281 float pathBegin;
1282 float pathEnd;
1283 float profileBegin;
1284 float profileEnd;
1285
1286 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1287 {
1288 taperX1 = _pbs.PathScaleX * 0.01f;
1289 if (taperX1 > 1.0f)
1290 taperX1 = 2.0f - taperX1;
1291 taperX = 1.0f - taperX1;
1292
1293 taperY1 = _pbs.PathScaleY * 0.01f;
1294 if (taperY1 > 1.0f)
1295 taperY1 = 2.0f - taperY1;
1296 taperY = 1.0f - taperY1;
1297 }
1298 else
1299 {
1300 taperX = _pbs.PathTaperX * 0.01f;
1301 if (taperX < 0.0f)
1302 taperX = -taperX;
1303 taperX1 = 1.0f - taperX;
1304
1305 taperY = _pbs.PathTaperY * 0.01f;
1306 if (taperY < 0.0f)
1307 taperY = -taperY;
1308 taperY1 = 1.0f - taperY;
1309
1310 }
1311
1312
1313 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1314
1315 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1316 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1317 volume *= (pathEnd - pathBegin);
1318
1319 // this is crude aproximation
1320 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1321 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1322 volume *= (profileEnd - profileBegin);
1323
1324 returnMass = m_density * volume;
1325
1326 if (returnMass <= 0)
1327 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1328 // else if (returnMass > _parent_scene.maximumMassObject)
1329 // returnMass = _parent_scene.maximumMassObject;
1330
1331
1332
1333 m_primMass = returnMass;
1334 if (m_primMass > _parent_scene.maximumMassObject)
1335 m_primMass = _parent_scene.maximumMassObject;
1336
1337 // Recursively calculate mass
1338 bool HasChildPrim = false;
1339 lock (childrenPrim)
1340 {
1341 if (childrenPrim.Count > 0)
1342 {
1343 HasChildPrim = true;
1344 }
1345
1346 }
1347 if (HasChildPrim)
1348 {
1349 OdePrim[] childPrimArr = new OdePrim[0];
1350
1351 lock (childrenPrim)
1352 childPrimArr = childrenPrim.ToArray();
1353
1354 for (int i = 0; i < childPrimArr.Length; i++)
1355 {
1356 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1357 returnMass += childPrimArr[i].CalculateMass();
1358 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1359 if (i > 256)
1360 break;
1361 }
1362 }
1363 if (returnMass > _parent_scene.maximumMassObject)
1364 returnMass = _parent_scene.maximumMassObject;
1365 return returnMass;
1366 }// end CalculateMass
1367
1368 #endregion
1369
1370 public void setMass()
1371 {
1372 if (Body != (IntPtr)0)
1373 {
1374 float newmass = CalculateMass();
1375
1376 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1377
1378 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1379 d.BodySetMass(Body, ref pMass);
1380 }
1381 }
1382
1383
1384 private void UpdateDataFromGeom()
1385 {
1386 if (prim_geom != IntPtr.Zero)
1387 {
1388 d.Quaternion qtmp;
1389 d.GeomCopyQuaternion(prim_geom, out qtmp);
1390 _orientation.W = qtmp.W;
1391 _orientation.X = qtmp.X;
1392 _orientation.Y = qtmp.Y;
1393 _orientation.Z = qtmp.Z;
1394
1395 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
1396 _position.X = lpos.X;
1397 _position.Y = lpos.Y;
1398 _position.Z = lpos.Z;
1399 }
1400 }
1401
1402 public void disableBody()
1403 {
1404 //this kills the body so things like 'mesh' can re-create it.
1405 lock (this)
1406 {
1407 if (!childPrim)
1408 {
1409 if (Body != IntPtr.Zero)
1410 {
1411 _parent_scene.remActivePrim(this);
1412 m_collisionCategories &= ~CollisionCategories.Body;
1413 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1414
1415 if (prim_geom != IntPtr.Zero)
1416 {
1417 if (m_NoColide)
1418 {
1419 d.GeomSetCategoryBits(prim_geom, 0);
1420 d.GeomSetCollideBits(prim_geom, 0);
1421 d.GeomDisable(prim_geom);
1422 }
1423 else
1424 {
1425 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1426 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1427 }
1428 }
1429
1430 UpdateDataFromGeom();
1431
1432 lock (childrenPrim)
1433 {
1434 if (childrenPrim.Count > 0)
1435 {
1436 foreach (OdePrim prm in childrenPrim)
1437 {
1438 if (prm.prim_geom != IntPtr.Zero)
1439 {
1440 if (prm.m_NoColide)
1441 {
1442 d.GeomSetCategoryBits(prm.prim_geom, 0);
1443 d.GeomSetCollideBits(prm.prim_geom, 0);
1444 d.GeomDisable(prm.prim_geom);
1445
1446 }
1447 prm.UpdateDataFromGeom();
1448 }
1449 _parent_scene.remActivePrim(prm);
1450 prm.Body = IntPtr.Zero;
1451 }
1452 }
1453 }
1454 d.BodyDestroy(Body);
1455 Body = IntPtr.Zero;
1456 }
1457 }
1458 else
1459 {
1460 _parent_scene.remActivePrim(this);
1461
1462 m_collisionCategories &= ~CollisionCategories.Body;
1463 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1464
1465 if (prim_geom != IntPtr.Zero)
1466 {
1467 if (m_NoColide)
1468 {
1469 d.GeomSetCategoryBits(prim_geom, 0);
1470 d.GeomSetCollideBits(prim_geom, 0);
1471 d.GeomDisable(prim_geom);
1472 }
1473 else
1474 {
1475 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1476 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1477 }
1478 }
1479
1480 Body = IntPtr.Zero;
1481 }
1482 }
1483 m_disabled = true;
1484 m_collisionscore = 0;
1485 }
1486
1487// private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1488
1489 public bool setMesh(OdeScene parent_scene, IMesh mesh)
1490 {
1491 //Kill Body so that mesh can re-make the geom
1492 if (IsPhysical && Body != IntPtr.Zero)
1493 {
1494 if (childPrim)
1495 {
1496 if (_parent != null)
1497 {
1498 OdePrim parent = (OdePrim)_parent;
1499 parent.ChildDelink(this);
1500 }
1501 }
1502 else
1503 {
1504 disableBody();
1505 }
1506 }
1507
1508 IntPtr vertices, indices;
1509 int vertexCount, indexCount;
1510 int vertexStride, triStride;
1511 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1512 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1513
1514 // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
1515 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1516
1517 if (vertexCount == 0 || indexCount == 0)
1518 {
1519 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1520 return false;
1521 }
1522
1523 IntPtr geo = IntPtr.Zero;
1524 try
1525 {
1526 _triMeshData = d.GeomTriMeshDataCreate();
1527 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1528 d.GeomTriMeshDataPreprocess(_triMeshData);
1529
1530 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1531
1532 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1533 }
1534 catch (Exception e)
1535 {
1536 m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);
1537
1538 if (_triMeshData != IntPtr.Zero)
1539 {
1540 d.GeomTriMeshDataDestroy(_triMeshData);
1541 _triMeshData = IntPtr.Zero;
1542 }
1543 return false;
1544 }
1545
1546 SetGeom(geo);
1547
1548 return true;
1549 }
1550
1551 public void ProcessTaints(float timestep) //=============================================================================
1552 {
1553 if (m_taintadd)
1554 {
1555 changeadd(timestep);
1556 }
1557
1558 if (m_taintremove)
1559 return;
1560
1561 if (prim_geom != IntPtr.Zero)
1562 {
1563 if (!_position.ApproxEquals(m_taintposition, 0f))
1564 {
1565 changemove(timestep);
1566 }
1567 if (m_taintrot != _orientation)
1568 {
1569 if (childPrim && IsPhysical) // For physical child prim...
1570 {
1571 rotate(timestep);
1572 // KF: ODE will also rotate the parent prim!
1573 // so rotate the root back to where it was
1574 OdePrim parent = (OdePrim)_parent;
1575 parent.rotate(timestep);
1576 }
1577 else
1578 {
1579 //Just rotate the prim
1580 rotate(timestep);
1581 }
1582 }
1583 //
1584 if (m_taintphantom != m_isphantom )
1585 {
1586 changePhantomStatus();
1587 }//
1588
1589 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1590 {
1591 changePhysicsStatus(timestep);
1592 }//
1593
1594
1595 if (!_size.ApproxEquals(m_taintsize, 0f))
1596 changesize(timestep);
1597 //
1598
1599 if(m_taintshapetype != m_shapetype)
1600 {
1601 m_shapetype = m_taintshapetype;
1602 changeshape(timestep);
1603 }
1604
1605 if (m_taintshape)
1606 changeshape(timestep);
1607 //
1608
1609 if (m_taintforce)
1610 changeAddForce(timestep);
1611
1612 if (m_taintaddangularforce)
1613 changeAddAngularForce(timestep);
1614
1615 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1616 changeSetTorque(timestep);
1617
1618 if (m_taintdisable)
1619 changedisable(timestep);
1620
1621 if (m_taintselected != m_isSelected)
1622 changeSelectedStatus();
1623
1624 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1625 changevelocity(timestep);
1626
1627 if (m_taintparent != _parent)
1628 changelink(timestep);
1629
1630 if (m_taintCollidesWater != m_collidesWater)
1631 changefloatonwater(timestep);
1632
1633 if (m_taintvehicledata != null)
1634 DoSetVehicle();
1635
1636 /* obsolete
1637 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1638 changeAngularLock(timestep);
1639 */
1640 }
1641
1642 else
1643 {
1644 m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom");
1645
1646 // not sure this will not flame...
1647 m_taintremove = true;
1648 _parent_scene.AddPhysicsActorTaint(this);
1649 }
1650
1651 }
1652
1653 private void changelink(float timestep)
1654 {
1655 // If the newly set parent is not null
1656 // create link
1657 if (_parent == null && m_taintparent != null)
1658 {
1659 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1660 {
1661 OdePrim obj = (OdePrim)m_taintparent;
1662 obj.ParentPrim(this);
1663 }
1664 }
1665 // If the newly set parent is null
1666 // destroy link
1667 else if (_parent != null && m_taintparent == null)
1668 {
1669 if (_parent is OdePrim)
1670 {
1671 OdePrim obj = (OdePrim)_parent;
1672 obj.ChildDelink(this);
1673 childPrim = false;
1674 }
1675 }
1676
1677 _parent = m_taintparent;
1678 m_taintPhysics = m_isphysical;
1679 }
1680
1681 // I'm the parent
1682 // prim is the child
1683 public void ParentPrim(OdePrim prim)
1684 {
1685 if (this.m_localID != prim.m_localID)
1686 {
1687 if (Body == IntPtr.Zero)
1688 {
1689 Body = d.BodyCreate(_parent_scene.world);
1690 // disconnect from world gravity so we can apply buoyancy
1691 d.BodySetGravityMode(Body, false);
1692
1693 setMass();
1694 }
1695 if (Body != IntPtr.Zero)
1696 {
1697 lock (childrenPrim)
1698 {
1699 if (!childrenPrim.Contains(prim))
1700 {
1701 childrenPrim.Add(prim);
1702
1703 foreach (OdePrim prm in childrenPrim)
1704 {
1705 d.Mass m2;
1706 d.MassSetZero(out m2);
1707 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1708
1709
1710 d.Quaternion quat = new d.Quaternion();
1711 quat.W = prm._orientation.W;
1712 quat.X = prm._orientation.X;
1713 quat.Y = prm._orientation.Y;
1714 quat.Z = prm._orientation.Z;
1715
1716 d.Matrix3 mat = new d.Matrix3();
1717 d.RfromQ(out mat, ref quat);
1718 d.MassRotate(ref m2, ref mat);
1719 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1720 d.MassAdd(ref pMass, ref m2);
1721 }
1722 foreach (OdePrim prm in childrenPrim)
1723 {
1724 if (m_isphantom && !prm.m_isVolumeDetect)
1725 {
1726 prm.m_collisionCategories = 0;
1727 prm.m_collisionFlags = CollisionCategories.Land;
1728 }
1729 else
1730 {
1731 prm.m_collisionCategories |= CollisionCategories.Body;
1732 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1733 }
1734 if (prm.prim_geom == IntPtr.Zero)
1735 {
1736 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1737 continue;
1738 }
1739
1740 if (prm.m_NoColide)
1741 {
1742 d.GeomSetCategoryBits(prm.prim_geom, 0);
1743 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1744 }
1745 else
1746 {
1747 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1748 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1749 }
1750
1751 d.Quaternion quat = new d.Quaternion();
1752 quat.W = prm._orientation.W;
1753 quat.X = prm._orientation.X;
1754 quat.Y = prm._orientation.Y;
1755 quat.Z = prm._orientation.Z;
1756
1757 d.Matrix3 mat = new d.Matrix3();
1758 d.RfromQ(out mat, ref quat);
1759 if (Body != IntPtr.Zero)
1760 {
1761 d.GeomSetBody(prm.prim_geom, Body);
1762 prm.childPrim = true;
1763 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1764 //d.GeomSetOffsetPosition(prim.prim_geom,
1765 // (Position.X - prm.Position.X) - pMass.c.X,
1766 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1767 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1768 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1769 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1770 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1771 d.BodySetMass(Body, ref pMass);
1772 }
1773 else
1774 {
1775 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1776 }
1777
1778 prm.m_interpenetrationcount = 0;
1779 prm.m_collisionscore = 0;
1780 prm.m_disabled = false;
1781
1782 prm.Body = Body;
1783
1784 _parent_scene.addActivePrim(prm);
1785 }
1786
1787 if (m_isphantom && !m_isVolumeDetect)
1788 {
1789 m_collisionCategories = 0;
1790 m_collisionFlags = CollisionCategories.Land;
1791 }
1792 else
1793 {
1794 m_collisionCategories |= CollisionCategories.Body;
1795 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1796 }
1797
1798 if (m_NoColide)
1799 {
1800 d.GeomSetCategoryBits(prim_geom, 0);
1801 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1802 }
1803 else
1804 {
1805 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1806 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1807 }
1808
1809 d.Quaternion quat2 = new d.Quaternion();
1810 quat2.W = _orientation.W;
1811 quat2.X = _orientation.X;
1812 quat2.Y = _orientation.Y;
1813 quat2.Z = _orientation.Z;
1814
1815 d.Matrix3 mat2 = new d.Matrix3();
1816 d.RfromQ(out mat2, ref quat2);
1817 d.GeomSetBody(prim_geom, Body);
1818 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1819 //d.GeomSetOffsetPosition(prim.prim_geom,
1820 // (Position.X - prm.Position.X) - pMass.c.X,
1821 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1822 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1823 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1824 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1825 d.BodySetMass(Body, ref pMass);
1826
1827 d.BodySetAutoDisableFlag(Body, true);
1828 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1829
1830 m_interpenetrationcount = 0;
1831 m_collisionscore = 0;
1832 m_disabled = false;
1833
1834 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1835 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1836
1837 _parent_scene.addActivePrim(this);
1838 }
1839 }
1840 }
1841 }
1842 }
1843
1844 private void ChildSetGeom(OdePrim odePrim)
1845 {
1846 lock (childrenPrim)
1847 {
1848 foreach (OdePrim prm in childrenPrim)
1849 {
1850 prm.disableBody();
1851 }
1852 }
1853 disableBody();
1854
1855 if (Body != IntPtr.Zero)
1856 {
1857 _parent_scene.remActivePrim(this);
1858 }
1859
1860 lock (childrenPrim)
1861 {
1862 foreach (OdePrim prm in childrenPrim)
1863 {
1864 ParentPrim(prm);
1865 }
1866 }
1867 }
1868
1869 private void ChildDelink(OdePrim odePrim)
1870 {
1871 // Okay, we have a delinked child.. need to rebuild the body.
1872 lock (childrenPrim)
1873 {
1874 foreach (OdePrim prm in childrenPrim)
1875 {
1876 prm.childPrim = true;
1877 prm.disableBody();
1878 }
1879 }
1880 disableBody();
1881
1882 lock (childrenPrim)
1883 {
1884 childrenPrim.Remove(odePrim);
1885 }
1886
1887 if (Body != IntPtr.Zero)
1888 {
1889 _parent_scene.remActivePrim(this);
1890 }
1891
1892 lock (childrenPrim)
1893 {
1894 foreach (OdePrim prm in childrenPrim)
1895 {
1896 ParentPrim(prm);
1897 }
1898 }
1899 }
1900
1901 private void changePhantomStatus()
1902 {
1903 m_taintphantom = m_isphantom;
1904 changeSelectedStatus();
1905 }
1906
1907/* not in use
1908 private void SetCollider()
1909 {
1910 SetCollider(m_isSelected, m_isphysical, m_isphantom, m_isSelected);
1911 }
1912
1913 private void SetCollider(bool sel, bool phys, bool phan, bool vdtc)
1914 {
1915 if (sel)
1916 {
1917 m_collisionCategories = CollisionCategories.Selected;
1918 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1919 }
1920 else
1921 {
1922 if (phan && !vdtc)
1923 {
1924 m_collisionCategories = 0;
1925 if (phys)
1926 m_collisionFlags = CollisionCategories.Land;
1927 else
1928 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
1929 }
1930 else
1931 {
1932 m_collisionCategories = CollisionCategories.Geom;
1933 if (phys)
1934 m_collisionCategories |= CollisionCategories.Body;
1935
1936 m_collisionFlags = m_default_collisionFlags;
1937
1938 if (m_collidesLand)
1939 m_collisionFlags |= CollisionCategories.Land;
1940 if (m_collidesWater)
1941 m_collisionFlags |= CollisionCategories.Water;
1942 }
1943 }
1944
1945 if (prim_geom != IntPtr.Zero)
1946 {
1947 if (m_NoColide)
1948 {
1949 d.GeomSetCategoryBits(prim_geom, 0);
1950 if (phys)
1951 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1952 else
1953 {
1954 d.GeomSetCollideBits(prim_geom, 0);
1955 d.GeomDisable(prim_geom);
1956 }
1957 }
1958 else
1959 {
1960 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1961 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1962 }
1963 }
1964 }
1965*/
1966
1967 private void changeSelectedStatus()
1968 {
1969 if (m_taintselected)
1970 {
1971 m_collisionCategories = CollisionCategories.Selected;
1972 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1973
1974 // We do the body disable soft twice because 'in theory' a collision could have happened
1975 // in between the disabling and the collision properties setting
1976 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1977 // through the ground.
1978
1979 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1980 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1981 // so that causes the selected part to wake up and continue moving.
1982
1983 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1984 // assembly will stop simulating during the selection, because of the lack of atomicity
1985 // of select operations (their processing could be interrupted by a thread switch, causing
1986 // simulation to continue before all of the selected object notifications trickle down to
1987 // the physics engine).
1988
1989 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1990 // selected and disabled. then, due to a thread switch, the selection processing is
1991 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1992 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1993 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1994 // up, start simulating again, which in turn wakes up the last 50.
1995
1996 if (m_isphysical)
1997 {
1998 disableBodySoft();
1999 }
2000
2001 if (prim_geom != IntPtr.Zero)
2002 {
2003 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2004 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2005 if (m_NoColide)
2006 d.GeomDisable(prim_geom);
2007 }
2008
2009 if (m_isphysical)
2010 {
2011 disableBodySoft();
2012 }
2013 if (Body != IntPtr.Zero)
2014 {
2015 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2016 d.BodySetForce(Body, 0f, 0f, 0f);
2017 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
2018 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
2019 }
2020 }
2021 else
2022 {
2023 if (m_isphantom && !m_isVolumeDetect)
2024 {
2025 m_collisionCategories = 0;
2026 if (m_isphysical)
2027 m_collisionFlags = CollisionCategories.Land;
2028 else
2029 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
2030 }
2031 else
2032 {
2033 m_collisionCategories = CollisionCategories.Geom;
2034 if (m_isphysical)
2035 m_collisionCategories |= CollisionCategories.Body;
2036
2037 m_collisionFlags = m_default_collisionFlags;
2038
2039 if (m_collidesLand)
2040 m_collisionFlags |= CollisionCategories.Land;
2041 if (m_collidesWater)
2042 m_collisionFlags |= CollisionCategories.Water;
2043 }
2044
2045 if (prim_geom != IntPtr.Zero)
2046 {
2047 if (m_NoColide)
2048 {
2049 d.GeomSetCategoryBits(prim_geom, 0);
2050 if (m_isphysical)
2051 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2052 else
2053 {
2054 d.GeomSetCollideBits(prim_geom, 0);
2055 d.GeomDisable(prim_geom);
2056 }
2057 }
2058 else
2059 {
2060 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2061 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2062 }
2063 }
2064 if (Body != IntPtr.Zero)
2065 {
2066 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2067 d.BodySetForce(Body, 0f, 0f, 0f);
2068 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
2069 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
2070 }
2071
2072 if (m_isphysical)
2073 {
2074 if (Body != IntPtr.Zero)
2075 {
2076 enableBodySoft();
2077 }
2078 }
2079 }
2080
2081 resetCollisionAccounting();
2082 m_isSelected = m_taintselected;
2083 }//end changeSelectedStatus
2084
2085 public void ResetTaints()
2086 {
2087 m_taintposition = _position;
2088 m_taintrot = _orientation;
2089 m_taintPhysics = m_isphysical;
2090 m_taintselected = m_isSelected;
2091 m_taintsize = _size;
2092 m_taintshape = false;
2093 m_taintforce = false;
2094 m_taintdisable = false;
2095 m_taintVelocity = Vector3.Zero;
2096 }
2097
2098 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
2099 {
2100 bool gottrimesh = false;
2101
2102 m_NoColide = false; // assume all will go well
2103
2104 if (_triMeshData != IntPtr.Zero)
2105 {
2106 d.GeomTriMeshDataDestroy(_triMeshData);
2107 _triMeshData = IntPtr.Zero;
2108 }
2109
2110 if (_mesh != null)
2111 {
2112 gottrimesh = setMesh(_parent_scene, _mesh);
2113 if (!gottrimesh)
2114 {
2115 // getting a mesh failed,
2116 // lets go on having a basic box or sphere, with prim size but not coliding
2117 // physical colides with land, non with nothing
2118
2119 m_NoColide = true;
2120 }
2121 }
2122
2123 if (!gottrimesh)
2124 { // we will have a basic box or sphere
2125 IntPtr geo = IntPtr.Zero;
2126
2127 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
2128 && _size.X == _size.Y && _size.X == _size.Z)
2129 {
2130 // its a sphere
2131 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2132 try
2133 {
2134 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
2135 }
2136 catch (Exception e)
2137 {
2138 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2139 geo = IntPtr.Zero;
2140 ode.dunlock(_parent_scene.world);
2141 }
2142 }
2143 else // make it a box
2144 {
2145 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2146 try
2147 {
2148 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
2149 }
2150 catch (Exception e)
2151 {
2152 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2153 geo = IntPtr.Zero;
2154 ode.dunlock(_parent_scene.world);
2155 }
2156 }
2157
2158 if (geo == IntPtr.Zero) // if this happens it must be fixed
2159 {
2160 // if it does lets stop what we can
2161 // not sure this will not flame...
2162
2163 m_taintremove = true;
2164 _parent_scene.AddPhysicsActorTaint(this);
2165 return;
2166 }
2167
2168 SetGeom(geo); // this processes the m_NoColide
2169 }
2170 }
2171
2172 public void changeadd(float timestep)
2173 {
2174 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2175 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
2176
2177 if (targetspace == IntPtr.Zero)
2178 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2179
2180 m_targetSpace = targetspace;
2181
2182 if (_mesh == null) // && m_meshfailed == false)
2183 {
2184 if (_parent_scene.needsMeshing(_pbs))
2185 {
2186 bool convex;
2187 if (m_shapetype == 2)
2188 convex = true;
2189 else
2190 convex = false;
2191 try
2192 {
2193 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,false,convex,false);
2194 }
2195 catch
2196 {
2197 //Don't continuously try to mesh prims when meshing has failed
2198 m_meshfailed = true;
2199 _mesh = null;
2200 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2201 }
2202 }
2203 }
2204
2205 lock (_parent_scene.OdeLock)
2206 {
2207 CreateGeom(m_targetSpace, _mesh);
2208
2209 if (prim_geom != IntPtr.Zero)
2210 {
2211 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2212 d.Quaternion myrot = new d.Quaternion();
2213 myrot.X = _orientation.X;
2214 myrot.Y = _orientation.Y;
2215 myrot.Z = _orientation.Z;
2216 myrot.W = _orientation.W;
2217 d.GeomSetQuaternion(prim_geom, ref myrot);
2218 }
2219
2220 if (m_isphysical && Body == IntPtr.Zero)
2221 {
2222 enableBody();
2223 }
2224 }
2225
2226 changeSelectedStatus();
2227
2228 m_taintadd = false;
2229 }
2230
2231 public void changemove(float timestep)
2232 {
2233 if (m_isphysical)
2234 {
2235 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
2236 if (!m_taintremove && !childPrim)
2237 {
2238 if (Body == IntPtr.Zero)
2239 enableBody();
2240 //Prim auto disable after 20 frames,
2241 //if you move it, re-enable the prim manually.
2242 if (_parent != null)
2243 {
2244 if (m_linkJoint != IntPtr.Zero)
2245 {
2246 d.JointDestroy(m_linkJoint);
2247 m_linkJoint = IntPtr.Zero;
2248 }
2249 }
2250 if (Body != IntPtr.Zero)
2251 {
2252 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
2253
2254 if (_parent != null)
2255 {
2256 OdePrim odParent = (OdePrim)_parent;
2257 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
2258 {
2259 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
2260 Console.WriteLine("ODEPrim JointCreateFixed !!!");
2261 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
2262 d.JointAttach(m_linkJoint, Body, odParent.Body);
2263 d.JointSetFixed(m_linkJoint);
2264 }
2265 }
2266 d.BodyEnable(Body);
2267 if (m_type != Vehicle.TYPE_NONE)
2268 {
2269 Enable(Body, _parent_scene);
2270 }
2271 }
2272 else
2273 {
2274 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
2275 }
2276 }
2277 //else
2278 // {
2279 //m_log.Debug("[BUG]: race!");
2280 //}
2281 }
2282 else
2283 {
2284 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2285 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2286 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2287
2288 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
2289 m_targetSpace = tempspace;
2290
2291 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2292 if (prim_geom != IntPtr.Zero)
2293 {
2294 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2295
2296 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2297 d.SpaceAdd(m_targetSpace, prim_geom);
2298 }
2299 }
2300
2301 changeSelectedStatus();
2302
2303 resetCollisionAccounting();
2304 m_taintposition = _position;
2305 }
2306
2307 public void rotate(float timestep)
2308 {
2309 d.Quaternion myrot = new d.Quaternion();
2310 myrot.X = _orientation.X;
2311 myrot.Y = _orientation.Y;
2312 myrot.Z = _orientation.Z;
2313 myrot.W = _orientation.W;
2314 if (Body != IntPtr.Zero)
2315 {
2316 // KF: If this is a root prim do BodySet
2317 d.BodySetQuaternion(Body, ref myrot);
2318 }
2319 else
2320 {
2321 // daughter prim, do Geom set
2322 d.GeomSetQuaternion(prim_geom, ref myrot);
2323 }
2324
2325 resetCollisionAccounting();
2326 m_taintrot = _orientation;
2327 }
2328
2329 private void resetCollisionAccounting()
2330 {
2331 m_collisionscore = 0;
2332 m_interpenetrationcount = 0;
2333 m_disabled = false;
2334 }
2335
2336 public void changedisable(float timestep)
2337 {
2338 m_disabled = true;
2339 if (Body != IntPtr.Zero)
2340 {
2341 d.BodyDisable(Body);
2342 Body = IntPtr.Zero;
2343 }
2344
2345 m_taintdisable = false;
2346 }
2347
2348 public void changePhysicsStatus(float timestep)
2349 {
2350 if (m_isphysical == true)
2351 {
2352 if (Body == IntPtr.Zero)
2353 {
2354 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2355 {
2356 changeshape(2f);
2357 }
2358 else
2359 {
2360 enableBody();
2361 }
2362 }
2363 }
2364 else
2365 {
2366 if (Body != IntPtr.Zero)
2367 {
2368 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2369 {
2370 _mesh = null;
2371 changeadd(2f);
2372 }
2373 if (childPrim)
2374 {
2375 if (_parent != null)
2376 {
2377 OdePrim parent = (OdePrim)_parent;
2378 parent.ChildDelink(this);
2379 }
2380 }
2381 else
2382 {
2383 disableBody();
2384 }
2385 }
2386 }
2387
2388 changeSelectedStatus();
2389
2390 resetCollisionAccounting();
2391 m_taintPhysics = m_isphysical;
2392 }
2393
2394 public void changesize(float timestamp)
2395 {
2396
2397 string oldname = _parent_scene.geom_name_map[prim_geom];
2398
2399 if (_size.X <= 0) _size.X = 0.01f;
2400 if (_size.Y <= 0) _size.Y = 0.01f;
2401 if (_size.Z <= 0) _size.Z = 0.01f;
2402
2403 // Cleanup of old prim geometry
2404 if (_mesh != null)
2405 {
2406 // Cleanup meshing here
2407 }
2408 //kill body to rebuild
2409 if (IsPhysical && Body != IntPtr.Zero)
2410 {
2411 if (childPrim)
2412 {
2413 if (_parent != null)
2414 {
2415 OdePrim parent = (OdePrim)_parent;
2416 parent.ChildDelink(this);
2417 }
2418 }
2419 else
2420 {
2421 disableBody();
2422 }
2423 }
2424 if (d.SpaceQuery(m_targetSpace, prim_geom))
2425 {
2426 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2427 d.SpaceRemove(m_targetSpace, prim_geom);
2428 }
2429 // we don't need to do space calculation because the client sends a position update also.
2430
2431 // Construction of new prim
2432 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2433 {
2434 float meshlod = _parent_scene.meshSculptLOD;
2435
2436 if (IsPhysical)
2437 meshlod = _parent_scene.MeshSculptphysicalLOD;
2438 // Don't need to re-enable body.. it's done in SetMesh
2439
2440 IMesh mesh = null;
2441
2442 try
2443 {
2444 if (_parent_scene.needsMeshing(_pbs))
2445 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
2446 }
2447 catch
2448 {
2449 m_meshfailed = true;
2450 mesh = null;
2451 m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2452 }
2453
2454 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2455 CreateGeom(m_targetSpace, mesh);
2456 }
2457 else
2458 {
2459 _mesh = null;
2460 CreateGeom(m_targetSpace, _mesh);
2461 }
2462
2463 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2464 d.Quaternion myrot = new d.Quaternion();
2465 myrot.X = _orientation.X;
2466 myrot.Y = _orientation.Y;
2467 myrot.Z = _orientation.Z;
2468 myrot.W = _orientation.W;
2469 d.GeomSetQuaternion(prim_geom, ref myrot);
2470
2471 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2472 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2473 {
2474 // Re creates body on size.
2475 // EnableBody also does setMass()
2476 enableBody();
2477 d.BodyEnable(Body);
2478 }
2479
2480 _parent_scene.geom_name_map[prim_geom] = oldname;
2481
2482 changeSelectedStatus();
2483 if (childPrim)
2484 {
2485 if (_parent is OdePrim)
2486 {
2487 OdePrim parent = (OdePrim)_parent;
2488 parent.ChildSetGeom(this);
2489 }
2490 }
2491 resetCollisionAccounting();
2492 m_taintsize = _size;
2493 }
2494
2495
2496
2497 public void changefloatonwater(float timestep)
2498 {
2499 m_collidesWater = m_taintCollidesWater;
2500
2501 if (prim_geom != IntPtr.Zero)
2502 {
2503 if (m_collidesWater)
2504 {
2505 m_collisionFlags |= CollisionCategories.Water;
2506 }
2507 else
2508 {
2509 m_collisionFlags &= ~CollisionCategories.Water;
2510 }
2511 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2512 }
2513 }
2514
2515 public void changeshape(float timestamp)
2516 {
2517 string oldname = _parent_scene.geom_name_map[prim_geom];
2518
2519 // Cleanup of old prim geometry and Bodies
2520 if (IsPhysical && Body != IntPtr.Zero)
2521 {
2522 if (childPrim)
2523 {
2524 if (_parent != null)
2525 {
2526 OdePrim parent = (OdePrim)_parent;
2527 parent.ChildDelink(this);
2528 }
2529 }
2530 else
2531 {
2532 disableBody();
2533 }
2534 }
2535
2536
2537 // we don't need to do space calculation because the client sends a position update also.
2538 if (_size.X <= 0) _size.X = 0.01f;
2539 if (_size.Y <= 0) _size.Y = 0.01f;
2540 if (_size.Z <= 0) _size.Z = 0.01f;
2541 // Construction of new prim
2542
2543 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2544 {
2545 // Don't need to re-enable body.. it's done in SetMesh
2546 float meshlod = _parent_scene.meshSculptLOD;
2547 IMesh mesh;
2548
2549 if (IsPhysical)
2550 meshlod = _parent_scene.MeshSculptphysicalLOD;
2551
2552 bool convex;
2553 if (m_shapetype == 2)
2554 convex = true;
2555 else
2556 convex = false;
2557
2558 try
2559 {
2560 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, false,convex,false);
2561 }
2562 catch
2563 {
2564 mesh = null;
2565 m_meshfailed = true;
2566 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2567 }
2568
2569 CreateGeom(m_targetSpace, mesh);
2570
2571 // createmesh returns null when it doesn't mesh.
2572 }
2573 else
2574 {
2575 _mesh = null;
2576 CreateGeom(m_targetSpace, null);
2577 }
2578
2579 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2580 d.Quaternion myrot = new d.Quaternion();
2581 //myrot.W = _orientation.w;
2582 myrot.W = _orientation.W;
2583 myrot.X = _orientation.X;
2584 myrot.Y = _orientation.Y;
2585 myrot.Z = _orientation.Z;
2586 d.GeomSetQuaternion(prim_geom, ref myrot);
2587
2588 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2589 if (IsPhysical && Body == IntPtr.Zero)
2590 {
2591 // Re creates body on size.
2592 // EnableBody also does setMass()
2593 enableBody();
2594 if (Body != IntPtr.Zero)
2595 {
2596 d.BodyEnable(Body);
2597 }
2598 }
2599 _parent_scene.geom_name_map[prim_geom] = oldname;
2600
2601 changeSelectedStatus();
2602 if (childPrim)
2603 {
2604 if (_parent is OdePrim)
2605 {
2606 OdePrim parent = (OdePrim)_parent;
2607 parent.ChildSetGeom(this);
2608 }
2609 }
2610 resetCollisionAccounting();
2611 m_taintshape = false;
2612 }
2613
2614 public void changeAddForce(float timestamp)
2615 {
2616 if (!m_isSelected)
2617 {
2618 lock (m_forcelist)
2619 {
2620 //m_log.Info("[PHYSICS]: dequeing forcelist");
2621 if (IsPhysical)
2622 {
2623 Vector3 iforce = Vector3.Zero;
2624 int i = 0;
2625 try
2626 {
2627 for (i = 0; i < m_forcelist.Count; i++)
2628 {
2629
2630 iforce = iforce + (m_forcelist[i] * 100);
2631 }
2632 }
2633 catch (IndexOutOfRangeException)
2634 {
2635 m_forcelist = new List<Vector3>();
2636 m_collisionscore = 0;
2637 m_interpenetrationcount = 0;
2638 m_taintforce = false;
2639 return;
2640 }
2641 catch (ArgumentOutOfRangeException)
2642 {
2643 m_forcelist = new List<Vector3>();
2644 m_collisionscore = 0;
2645 m_interpenetrationcount = 0;
2646 m_taintforce = false;
2647 return;
2648 }
2649 d.BodyEnable(Body);
2650
2651 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2652 }
2653 m_forcelist.Clear();
2654 }
2655
2656 m_collisionscore = 0;
2657 m_interpenetrationcount = 0;
2658 }
2659
2660 m_taintforce = false;
2661
2662 }
2663
2664
2665
2666 public void changeSetTorque(float timestamp)
2667 {
2668 if (!m_isSelected)
2669 {
2670 if (IsPhysical && Body != IntPtr.Zero)
2671 {
2672 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2673 }
2674 }
2675
2676 m_taintTorque = Vector3.Zero;
2677 }
2678
2679 public void changeAddAngularForce(float timestamp)
2680 {
2681 if (!m_isSelected)
2682 {
2683 lock (m_angularforcelist)
2684 {
2685 //m_log.Info("[PHYSICS]: dequeing forcelist");
2686 if (IsPhysical)
2687 {
2688 Vector3 iforce = Vector3.Zero;
2689 for (int i = 0; i < m_angularforcelist.Count; i++)
2690 {
2691 iforce = iforce + (m_angularforcelist[i] * 100);
2692 }
2693 d.BodyEnable(Body);
2694 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2695
2696 }
2697 m_angularforcelist.Clear();
2698 }
2699
2700 m_collisionscore = 0;
2701 m_interpenetrationcount = 0;
2702 }
2703
2704 m_taintaddangularforce = false;
2705 }
2706
2707 private void changevelocity(float timestep)
2708 {
2709 if (!m_isSelected)
2710 {
2711 Thread.Sleep(20);
2712 if (IsPhysical)
2713 {
2714 if (Body != IntPtr.Zero)
2715 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2716 }
2717
2718 //resetCollisionAccounting();
2719 }
2720 m_taintVelocity = Vector3.Zero;
2721 }
2722
2723 public void UpdatePositionAndVelocity()
2724 {
2725 return; // moved to the Move () method
2726 }
2727
2728 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2729 {
2730 obj.I.M00 = pMat[0, 0];
2731 obj.I.M01 = pMat[0, 1];
2732 obj.I.M02 = pMat[0, 2];
2733 obj.I.M10 = pMat[1, 0];
2734 obj.I.M11 = pMat[1, 1];
2735 obj.I.M12 = pMat[1, 2];
2736 obj.I.M20 = pMat[2, 0];
2737 obj.I.M21 = pMat[2, 1];
2738 obj.I.M22 = pMat[2, 2];
2739 return obj;
2740 }
2741
2742 public override void SubscribeEvents(int ms)
2743 {
2744 m_eventsubscription = ms;
2745 _parent_scene.addCollisionEventReporting(this);
2746 }
2747
2748 public override void UnSubscribeEvents()
2749 {
2750 _parent_scene.remCollisionEventReporting(this);
2751 m_eventsubscription = 0;
2752 }
2753
2754 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2755 {
2756 if (CollisionEventsThisFrame == null)
2757 CollisionEventsThisFrame = new CollisionEventUpdate();
2758 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
2759 }
2760
2761 public void SendCollisions()
2762 {
2763 if (CollisionEventsThisFrame == null)
2764 return;
2765
2766 base.SendCollisionUpdate(CollisionEventsThisFrame);
2767
2768 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
2769 CollisionEventsThisFrame = null;
2770 else
2771 CollisionEventsThisFrame = new CollisionEventUpdate();
2772 }
2773
2774 public override bool SubscribedEvents()
2775 {
2776 if (m_eventsubscription > 0)
2777 return true;
2778 return false;
2779 }
2780
2781 public static Matrix4 Inverse(Matrix4 pMat)
2782 {
2783 if (determinant3x3(pMat) == 0)
2784 {
2785 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2786 }
2787
2788
2789
2790 return (Adjoint(pMat) / determinant3x3(pMat));
2791 }
2792
2793 public static Matrix4 Adjoint(Matrix4 pMat)
2794 {
2795 Matrix4 adjointMatrix = new Matrix4();
2796 for (int i = 0; i < 4; i++)
2797 {
2798 for (int j = 0; j < 4; j++)
2799 {
2800 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2801 }
2802 }
2803
2804 adjointMatrix = Transpose(adjointMatrix);
2805 return adjointMatrix;
2806 }
2807
2808 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2809 {
2810 Matrix4 minor = new Matrix4();
2811 int m = 0, n = 0;
2812 for (int i = 0; i < 4; i++)
2813 {
2814 if (i == iRow)
2815 continue;
2816 n = 0;
2817 for (int j = 0; j < 4; j++)
2818 {
2819 if (j == iCol)
2820 continue;
2821 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2822 n++;
2823 }
2824 m++;
2825 }
2826 return minor;
2827 }
2828
2829 public static Matrix4 Transpose(Matrix4 pMat)
2830 {
2831 Matrix4 transposeMatrix = new Matrix4();
2832 for (int i = 0; i < 4; i++)
2833 for (int j = 0; j < 4; j++)
2834 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
2835 return transposeMatrix;
2836 }
2837
2838 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2839 {
2840 switch (r)
2841 {
2842 case 0:
2843 switch (c)
2844 {
2845 case 0:
2846 pMat.M11 = val;
2847 break;
2848 case 1:
2849 pMat.M12 = val;
2850 break;
2851 case 2:
2852 pMat.M13 = val;
2853 break;
2854 case 3:
2855 pMat.M14 = val;
2856 break;
2857 }
2858
2859 break;
2860 case 1:
2861 switch (c)
2862 {
2863 case 0:
2864 pMat.M21 = val;
2865 break;
2866 case 1:
2867 pMat.M22 = val;
2868 break;
2869 case 2:
2870 pMat.M23 = val;
2871 break;
2872 case 3:
2873 pMat.M24 = val;
2874 break;
2875 }
2876
2877 break;
2878 case 2:
2879 switch (c)
2880 {
2881 case 0:
2882 pMat.M31 = val;
2883 break;
2884 case 1:
2885 pMat.M32 = val;
2886 break;
2887 case 2:
2888 pMat.M33 = val;
2889 break;
2890 case 3:
2891 pMat.M34 = val;
2892 break;
2893 }
2894
2895 break;
2896 case 3:
2897 switch (c)
2898 {
2899 case 0:
2900 pMat.M41 = val;
2901 break;
2902 case 1:
2903 pMat.M42 = val;
2904 break;
2905 case 2:
2906 pMat.M43 = val;
2907 break;
2908 case 3:
2909 pMat.M44 = val;
2910 break;
2911 }
2912
2913 break;
2914 }
2915 }
2916 private static float determinant3x3(Matrix4 pMat)
2917 {
2918 float det = 0;
2919 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2920 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2921 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2922 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2923 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2924 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2925
2926 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2927 return det;
2928
2929 }
2930
2931 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2932 {
2933 dst.c.W = src.c.W;
2934 dst.c.X = src.c.X;
2935 dst.c.Y = src.c.Y;
2936 dst.c.Z = src.c.Z;
2937 dst.mass = src.mass;
2938 dst.I.M00 = src.I.M00;
2939 dst.I.M01 = src.I.M01;
2940 dst.I.M02 = src.I.M02;
2941 dst.I.M10 = src.I.M10;
2942 dst.I.M11 = src.I.M11;
2943 dst.I.M12 = src.I.M12;
2944 dst.I.M20 = src.I.M20;
2945 dst.I.M21 = src.I.M21;
2946 dst.I.M22 = src.I.M22;
2947 }
2948
2949 public override void SetMaterial(int pMaterial)
2950 {
2951 m_material = pMaterial;
2952 }
2953
2954 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2955 {
2956 switch (pParam)
2957 {
2958 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2959 if (pValue < 0.01f) pValue = 0.01f;
2960 // m_angularDeflectionEfficiency = pValue;
2961 break;
2962 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2963 if (pValue < 0.1f) pValue = 0.1f;
2964 // m_angularDeflectionTimescale = pValue;
2965 break;
2966 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2967 if (pValue < 0.3f) pValue = 0.3f;
2968 m_angularMotorDecayTimescale = pValue;
2969 break;
2970 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2971 if (pValue < 0.3f) pValue = 0.3f;
2972 m_angularMotorTimescale = pValue;
2973 break;
2974 case Vehicle.BANKING_EFFICIENCY:
2975 if (pValue < 0.01f) pValue = 0.01f;
2976 // m_bankingEfficiency = pValue;
2977 break;
2978 case Vehicle.BANKING_MIX:
2979 if (pValue < 0.01f) pValue = 0.01f;
2980 // m_bankingMix = pValue;
2981 break;
2982 case Vehicle.BANKING_TIMESCALE:
2983 if (pValue < 0.01f) pValue = 0.01f;
2984 // m_bankingTimescale = pValue;
2985 break;
2986 case Vehicle.BUOYANCY:
2987 if (pValue < -1f) pValue = -1f;
2988 if (pValue > 1f) pValue = 1f;
2989 m_VehicleBuoyancy = pValue;
2990 break;
2991 // case Vehicle.HOVER_EFFICIENCY:
2992 // if (pValue < 0f) pValue = 0f;
2993 // if (pValue > 1f) pValue = 1f;
2994 // m_VhoverEfficiency = pValue;
2995 // break;
2996 case Vehicle.HOVER_HEIGHT:
2997 m_VhoverHeight = pValue;
2998 break;
2999 case Vehicle.HOVER_TIMESCALE:
3000 if (pValue < 0.1f) pValue = 0.1f;
3001 m_VhoverTimescale = pValue;
3002 break;
3003 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
3004 if (pValue < 0.01f) pValue = 0.01f;
3005 // m_linearDeflectionEfficiency = pValue;
3006 break;
3007 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
3008 if (pValue < 0.01f) pValue = 0.01f;
3009 // m_linearDeflectionTimescale = pValue;
3010 break;
3011 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
3012 if (pValue < 0.3f) pValue = 0.3f;
3013 m_linearMotorDecayTimescale = pValue;
3014 break;
3015 case Vehicle.LINEAR_MOTOR_TIMESCALE:
3016 if (pValue < 0.1f) pValue = 0.1f;
3017 m_linearMotorTimescale = pValue;
3018 break;
3019 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
3020 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
3021 if (pValue > 1.0f) pValue = 1.0f;
3022 m_verticalAttractionEfficiency = pValue;
3023 break;
3024 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
3025 if (pValue < 0.1f) pValue = 0.1f;
3026 m_verticalAttractionTimescale = pValue;
3027 break;
3028
3029 // These are vector properties but the engine lets you use a single float value to
3030 // set all of the components to the same value
3031 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
3032 if (pValue > 30f) pValue = 30f;
3033 if (pValue < 0.1f) pValue = 0.1f;
3034 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
3035 break;
3036 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3037 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
3038 UpdateAngDecay();
3039 break;
3040 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3041 if (pValue < 0.1f) pValue = 0.1f;
3042 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
3043 break;
3044 case Vehicle.LINEAR_MOTOR_DIRECTION:
3045 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
3046 UpdateLinDecay();
3047 break;
3048 case Vehicle.LINEAR_MOTOR_OFFSET:
3049 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
3050 break;
3051
3052 }
3053
3054 }//end ProcessFloatVehicleParam
3055
3056 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
3057 {
3058 switch (pParam)
3059 {
3060 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
3061 if (pValue.X > 30f) pValue.X = 30f;
3062 if (pValue.X < 0.1f) pValue.X = 0.1f;
3063 if (pValue.Y > 30f) pValue.Y = 30f;
3064 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3065 if (pValue.Z > 30f) pValue.Z = 30f;
3066 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3067 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3068 break;
3069 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3070 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
3071 // Limit requested angular speed to 2 rps= 4 pi rads/sec
3072 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
3073 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
3074 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
3075 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
3076 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
3077 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
3078 UpdateAngDecay();
3079 break;
3080 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3081 if (pValue.X < 0.1f) pValue.X = 0.1f;
3082 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3083 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3084 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3085 break;
3086 case Vehicle.LINEAR_MOTOR_DIRECTION:
3087 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
3088 UpdateLinDecay();
3089 break;
3090 case Vehicle.LINEAR_MOTOR_OFFSET:
3091 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
3092 break;
3093 }
3094
3095 }//end ProcessVectorVehicleParam
3096
3097 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
3098 {
3099 switch (pParam)
3100 {
3101 case Vehicle.REFERENCE_FRAME:
3102 // m_referenceFrame = pValue;
3103 break;
3104 }
3105
3106 }//end ProcessRotationVehicleParam
3107
3108 internal void ProcessVehicleFlags(int pParam, bool remove)
3109 {
3110 if (remove)
3111 {
3112 m_flags &= ~((VehicleFlag)pParam);
3113 }
3114 else
3115 {
3116 m_flags |= (VehicleFlag)pParam;
3117 }
3118 }
3119
3120 internal void ProcessTypeChange(Vehicle pType)
3121 {
3122 // Set Defaults For Type
3123 m_type = pType;
3124 switch (pType)
3125 {
3126 case Vehicle.TYPE_SLED:
3127 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
3128 m_angularFrictionTimescale = new Vector3(30, 30, 30);
3129 // m_lLinMotorVel = Vector3.Zero;
3130 m_linearMotorTimescale = 1000;
3131 m_linearMotorDecayTimescale = 120;
3132 m_angularMotorDirection = Vector3.Zero;
3133 m_angularMotorDVel = Vector3.Zero;
3134 m_angularMotorTimescale = 1000;
3135 m_angularMotorDecayTimescale = 120;
3136 m_VhoverHeight = 0;
3137 // m_VhoverEfficiency = 1;
3138 m_VhoverTimescale = 10;
3139 m_VehicleBuoyancy = 0;
3140 // m_linearDeflectionEfficiency = 1;
3141 // m_linearDeflectionTimescale = 1;
3142 // m_angularDeflectionEfficiency = 1;
3143 // m_angularDeflectionTimescale = 1000;
3144 // m_bankingEfficiency = 0;
3145 // m_bankingMix = 1;
3146 // m_bankingTimescale = 10;
3147 // m_referenceFrame = Quaternion.Identity;
3148 m_flags &=
3149 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3150 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3151 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3152 break;
3153 case Vehicle.TYPE_CAR:
3154 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
3155 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
3156 // m_lLinMotorVel = Vector3.Zero;
3157 m_linearMotorTimescale = 1;
3158 m_linearMotorDecayTimescale = 60;
3159 m_angularMotorDirection = Vector3.Zero;
3160 m_angularMotorDVel = Vector3.Zero;
3161 m_angularMotorTimescale = 1;
3162 m_angularMotorDecayTimescale = 0.8f;
3163 m_VhoverHeight = 0;
3164 // m_VhoverEfficiency = 0;
3165 m_VhoverTimescale = 1000;
3166 m_VehicleBuoyancy = 0;
3167 // // m_linearDeflectionEfficiency = 1;
3168 // // m_linearDeflectionTimescale = 2;
3169 // // m_angularDeflectionEfficiency = 0;
3170 // m_angularDeflectionTimescale = 10;
3171 m_verticalAttractionEfficiency = 1f;
3172 m_verticalAttractionTimescale = 10f;
3173 // m_bankingEfficiency = -0.2f;
3174 // m_bankingMix = 1;
3175 // m_bankingTimescale = 1;
3176 // m_referenceFrame = Quaternion.Identity;
3177 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3178 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
3179 VehicleFlag.LIMIT_MOTOR_UP);
3180 break;
3181 case Vehicle.TYPE_BOAT:
3182 m_linearFrictionTimescale = new Vector3(10, 3, 2);
3183 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3184 // m_lLinMotorVel = Vector3.Zero;
3185 m_linearMotorTimescale = 5;
3186 m_linearMotorDecayTimescale = 60;
3187 m_angularMotorDirection = Vector3.Zero;
3188 m_angularMotorDVel = Vector3.Zero;
3189 m_angularMotorTimescale = 4;
3190 m_angularMotorDecayTimescale = 4;
3191 m_VhoverHeight = 0;
3192 // m_VhoverEfficiency = 0.5f;
3193 m_VhoverTimescale = 2;
3194 m_VehicleBuoyancy = 1;
3195 // m_linearDeflectionEfficiency = 0.5f;
3196 // m_linearDeflectionTimescale = 3;
3197 // m_angularDeflectionEfficiency = 0.5f;
3198 // m_angularDeflectionTimescale = 5;
3199 m_verticalAttractionEfficiency = 0.5f;
3200 m_verticalAttractionTimescale = 5f;
3201 // m_bankingEfficiency = -0.3f;
3202 // m_bankingMix = 0.8f;
3203 // m_bankingTimescale = 1;
3204 // m_referenceFrame = Quaternion.Identity;
3205 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
3206 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3207 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
3208 VehicleFlag.LIMIT_MOTOR_UP);
3209 break;
3210 case Vehicle.TYPE_AIRPLANE:
3211 m_linearFrictionTimescale = new Vector3(200, 10, 5);
3212 m_angularFrictionTimescale = new Vector3(20, 20, 20);
3213 // m_lLinMotorVel = Vector3.Zero;
3214 m_linearMotorTimescale = 2;
3215 m_linearMotorDecayTimescale = 60;
3216 m_angularMotorDirection = Vector3.Zero;
3217 m_angularMotorDVel = Vector3.Zero;
3218 m_angularMotorTimescale = 4;
3219 m_angularMotorDecayTimescale = 4;
3220 m_VhoverHeight = 0;
3221 // m_VhoverEfficiency = 0.5f;
3222 m_VhoverTimescale = 1000;
3223 m_VehicleBuoyancy = 0;
3224 // m_linearDeflectionEfficiency = 0.5f;
3225 // m_linearDeflectionTimescale = 3;
3226 // m_angularDeflectionEfficiency = 1;
3227 // m_angularDeflectionTimescale = 2;
3228 m_verticalAttractionEfficiency = 0.9f;
3229 m_verticalAttractionTimescale = 2f;
3230 // m_bankingEfficiency = 1;
3231 // m_bankingMix = 0.7f;
3232 // m_bankingTimescale = 2;
3233 // m_referenceFrame = Quaternion.Identity;
3234 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3235 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3236 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
3237 break;
3238 case Vehicle.TYPE_BALLOON:
3239 m_linearFrictionTimescale = new Vector3(5, 5, 5);
3240 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3241 m_linearMotorTimescale = 5;
3242 m_linearMotorDecayTimescale = 60;
3243 m_angularMotorDirection = Vector3.Zero;
3244 m_angularMotorDVel = Vector3.Zero;
3245 m_angularMotorTimescale = 6;
3246 m_angularMotorDecayTimescale = 10;
3247 m_VhoverHeight = 5;
3248 // m_VhoverEfficiency = 0.8f;
3249 m_VhoverTimescale = 10;
3250 m_VehicleBuoyancy = 1;
3251 // m_linearDeflectionEfficiency = 0;
3252 // m_linearDeflectionTimescale = 5;
3253 // m_angularDeflectionEfficiency = 0;
3254 // m_angularDeflectionTimescale = 5;
3255 m_verticalAttractionEfficiency = 1f;
3256 m_verticalAttractionTimescale = 100f;
3257 // m_bankingEfficiency = 0;
3258 // m_bankingMix = 0.7f;
3259 // m_bankingTimescale = 5;
3260 // m_referenceFrame = Quaternion.Identity;
3261 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3262 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3263 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3264 break;
3265
3266 }
3267 }//end SetDefaultsForType
3268
3269 internal void Enable(IntPtr pBody, OdeScene pParentScene)
3270 {
3271 if (m_type == Vehicle.TYPE_NONE)
3272 return;
3273
3274 m_body = pBody;
3275 }
3276
3277
3278 internal void Halt()
3279 { // Kill all motions, when non-physical
3280 // m_linearMotorDirection = Vector3.Zero;
3281 m_lLinMotorDVel = Vector3.Zero;
3282 m_lLinObjectVel = Vector3.Zero;
3283 m_wLinObjectVel = Vector3.Zero;
3284 m_angularMotorDirection = Vector3.Zero;
3285 m_lastAngularVelocity = Vector3.Zero;
3286 m_angularMotorDVel = Vector3.Zero;
3287 _acceleration = Vector3.Zero;
3288 }
3289
3290 private void UpdateLinDecay()
3291 {
3292 m_lLinMotorDVel.X = m_linearMotorDirection.X;
3293 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3294 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3295 } // else let the motor decay on its own
3296
3297 private void UpdateAngDecay()
3298 {
3299 m_angularMotorDVel.X = m_angularMotorDirection.X;
3300 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3301 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3302 } // else let the motor decay on its own
3303
3304 public void Move(float timestep)
3305 {
3306 float fx = 0;
3307 float fy = 0;
3308 float fz = 0;
3309 Vector3 linvel; // velocity applied, including any reversal
3310
3311 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3312 // This is a temp patch until proper region crossing is developed.
3313
3314
3315 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3316 {
3317 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3318 bool lastZeroFlag = _zeroFlag; // was it stopped
3319
3320 d.Vector3 vec = d.BodyGetPosition(Body);
3321 Vector3 l_position = Vector3.Zero;
3322 l_position.X = vec.X;
3323 l_position.Y = vec.Y;
3324 l_position.Z = vec.Z;
3325 m_lastposition = _position;
3326 _position = l_position;
3327
3328 d.Quaternion ori = d.BodyGetQuaternion(Body);
3329 // Quaternion l_orientation = Quaternion.Identity;
3330 _orientation.X = ori.X;
3331 _orientation.Y = ori.Y;
3332 _orientation.Z = ori.Z;
3333 _orientation.W = ori.W;
3334 m_lastorientation = _orientation;
3335
3336 d.Vector3 vel = d.BodyGetLinearVel(Body);
3337 m_lastVelocity = _velocity;
3338 _velocity.X = vel.X;
3339 _velocity.Y = vel.Y;
3340 _velocity.Z = vel.Z;
3341 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3342
3343 d.Vector3 torque = d.BodyGetTorque(Body);
3344 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3345
3346
3347 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3348 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3349 )
3350 {
3351 // we are outside current region
3352 // clip position to a stop just outside region and stop it only internally
3353 // do it only once using m_crossingfailures as control
3354 _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f);
3355 _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f);
3356 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3357 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3358 d.BodySetLinearVel(Body, 0, 0, 0);
3359 m_outofBounds = true;
3360 base.RequestPhysicsterseUpdate();
3361 return;
3362 }
3363
3364 base.RequestPhysicsterseUpdate();
3365
3366 if (l_position.Z < 0)
3367 {
3368 // This is so prim that get lost underground don't fall forever and suck up
3369 //
3370 // Sim resources and memory.
3371 // Disables the prim's movement physics....
3372 // It's a hack and will generate a console message if it fails.
3373
3374 //IsPhysical = false;
3375 if (_parent == null) base.RaiseOutOfBounds(_position);
3376
3377
3378 _acceleration.X = 0; // This stuff may stop client display but it has no
3379 _acceleration.Y = 0; // effect on the object in phys engine!
3380 _acceleration.Z = 0;
3381
3382 _velocity.X = 0;
3383 _velocity.Y = 0;
3384 _velocity.Z = 0;
3385 m_lastVelocity = Vector3.Zero;
3386 m_rotationalVelocity.X = 0;
3387 m_rotationalVelocity.Y = 0;
3388 m_rotationalVelocity.Z = 0;
3389
3390 if (_parent == null) base.RequestPhysicsterseUpdate();
3391
3392 m_throttleUpdates = false;
3393 throttleCounter = 0;
3394 _zeroFlag = true;
3395 //outofBounds = true;
3396 } // end neg Z check
3397
3398 // Is it moving?
3399 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3400 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3401 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3402 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3403 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3404 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3405 {
3406 _zeroFlag = true;
3407 m_throttleUpdates = false;
3408 }
3409 else
3410 {
3411 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3412 _zeroFlag = false;
3413 m_lastUpdateSent = false;
3414 //m_throttleUpdates = false;
3415 }
3416
3417 if (_zeroFlag)
3418 { // Its stopped
3419 _velocity.X = 0.0f;
3420 _velocity.Y = 0.0f;
3421 // _velocity.Z = 0.0f;
3422
3423 _acceleration.X = 0;
3424 _acceleration.Y = 0;
3425 // _acceleration.Z = 0;
3426
3427 m_rotationalVelocity.X = 0;
3428 m_rotationalVelocity.Y = 0;
3429 m_rotationalVelocity.Z = 0;
3430 // Stop it in the phys engine
3431 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3432 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3433 d.BodySetForce(Body, 0f, 0f, 0f);
3434
3435 if (!m_lastUpdateSent)
3436 {
3437 m_throttleUpdates = false;
3438 throttleCounter = 0;
3439 if (_parent == null)
3440 {
3441 base.RequestPhysicsterseUpdate();
3442 }
3443
3444 m_lastUpdateSent = true;
3445 }
3446 }
3447 else
3448 { // Its moving
3449 if (lastZeroFlag != _zeroFlag)
3450 {
3451 if (_parent == null)
3452 {
3453 base.RequestPhysicsterseUpdate();
3454 }
3455 }
3456 m_lastUpdateSent = false;
3457 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3458 {
3459 if (_parent == null)
3460 {
3461 base.RequestPhysicsterseUpdate();
3462 }
3463 }
3464 else
3465 {
3466 throttleCounter++;
3467 }
3468 }
3469 m_lastposition = l_position;
3470
3471 /// End UpdatePositionAndVelocity insert
3472
3473
3474 // Rotation lock =====================================
3475 if (m_rotateEnableUpdate)
3476 {
3477 // Snapshot current angles, set up Amotor(s)
3478 m_rotateEnableUpdate = false;
3479 m_rotateEnable = m_rotateEnableRequest;
3480 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3481
3482 if (Amotor != IntPtr.Zero)
3483 {
3484 d.JointDestroy(Amotor);
3485 Amotor = IntPtr.Zero;
3486 //Console.WriteLine("Old Amotor Destroyed");
3487 }
3488
3489 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3490 { // not all are enabled
3491 d.Quaternion r = d.BodyGetQuaternion(Body);
3492 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3493 // extract the axes vectors
3494 Vector3 vX = new Vector3(1f, 0f, 0f);
3495 Vector3 vY = new Vector3(0f, 1f, 0f);
3496 Vector3 vZ = new Vector3(0f, 0f, 1f);
3497 vX = vX * locrot;
3498 vY = vY * locrot;
3499 vZ = vZ * locrot;
3500 // snapshot the current angle vectors
3501 m_lockX = vX;
3502 m_lockY = vY;
3503 m_lockZ = vZ;
3504 // m_lockRot = locrot;
3505 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3506 d.JointAttach(Amotor, Body, IntPtr.Zero);
3507 d.JointSetAMotorMode(Amotor, 0); // User mode??
3508 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3509
3510 float axisnum = 3; // how many to lock
3511 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3512 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3513 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3514
3515 int i = 0;
3516
3517 if (m_rotateEnable.X == 0)
3518 {
3519 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3520 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3521 i++;
3522 }
3523
3524 if (m_rotateEnable.Y == 0)
3525 {
3526 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3527 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3528 i++;
3529 }
3530
3531 if (m_rotateEnable.Z == 0)
3532 {
3533 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3534 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3535 i++;
3536 }
3537
3538 // These lowstops and high stops are effectively (no wiggle room)
3539 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3540 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3541 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3542 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3543 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3544 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3545 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3546 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3547 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3548 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3549 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3550 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3551 } // else none are locked
3552 } // end Rotation Update
3553
3554
3555 // VEHICLE processing ==========================================
3556 if (m_type != Vehicle.TYPE_NONE)
3557 {
3558 // get body attitude
3559 d.Quaternion rot = d.BodyGetQuaternion(Body);
3560 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3561 Quaternion irotq = Quaternion.Inverse(rotq);
3562
3563 // VEHICLE Linear Motion
3564 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3565 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3566 m_lLinObjectVel = vel_now * irotq;
3567 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3568 {
3569 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3570 {
3571 float decayfactor = m_linearMotorDecayTimescale / timestep;
3572 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3573 m_lLinMotorDVel -= decayAmount;
3574 }
3575 else
3576 {
3577 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3578 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3579 m_lLinMotorDVel -= decel;
3580 }
3581 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3582 {
3583 m_lLinMotorDVel = Vector3.Zero;
3584 }
3585
3586 /* else
3587 {
3588 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3589 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3590 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3591 } */
3592 } // end linear motor decay
3593
3594 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3595 {
3596 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3597 if (m_linearMotorTimescale < 300.0f)
3598 {
3599 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3600 float linfactor = m_linearMotorTimescale / timestep;
3601 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3602 m_lLinObjectVel += attackAmount;
3603 }
3604 if (m_linearFrictionTimescale.X < 300.0f)
3605 {
3606 float fricfactor = m_linearFrictionTimescale.X / timestep;
3607 float fricX = m_lLinObjectVel.X / fricfactor;
3608 m_lLinObjectVel.X -= fricX;
3609 }
3610 if (m_linearFrictionTimescale.Y < 300.0f)
3611 {
3612 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3613 float fricY = m_lLinObjectVel.Y / fricfactor;
3614 m_lLinObjectVel.Y -= fricY;
3615 }
3616 if (m_linearFrictionTimescale.Z < 300.0f)
3617 {
3618 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3619 float fricZ = m_lLinObjectVel.Z / fricfactor;
3620 m_lLinObjectVel.Z -= fricZ;
3621 }
3622 }
3623 m_wLinObjectVel = m_lLinObjectVel * rotq;
3624
3625 // Gravity and Buoyancy
3626 Vector3 grav = Vector3.Zero;
3627 if (m_VehicleBuoyancy < 1.0f)
3628 {
3629 // There is some gravity, make a gravity force vector
3630 // that is applied after object velocity.
3631 d.Mass objMass;
3632 d.BodyGetMass(Body, out objMass);
3633 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3634 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3635 } // else its 1.0, no gravity.
3636
3637 // Hovering
3638 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3639 {
3640 // We should hover, get the target height
3641 d.Vector3 pos = d.BodyGetPosition(Body);
3642 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3643 {
3644 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3645 }
3646 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3647 {
3648 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3649 }
3650 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3651 {
3652 m_VhoverTargetHeight = m_VhoverHeight;
3653 }
3654
3655 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3656 {
3657 // If body is aready heigher, use its height as target height
3658 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3659 }
3660
3661 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3662 // m_VhoverTimescale = 0f; // time to acheive height
3663 // timestep is time since last frame,in secs
3664 float herr0 = pos.Z - m_VhoverTargetHeight;
3665 // Replace Vertical speed with correction figure if significant
3666 if (Math.Abs(herr0) > 0.01f)
3667 {
3668 //? d.Mass objMass;
3669 //? d.BodyGetMass(Body, out objMass);
3670 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3671 //KF: m_VhoverEfficiency is not yet implemented
3672 }
3673 else
3674 {
3675 m_wLinObjectVel.Z = 0f;
3676 }
3677 }
3678 else
3679 { // not hovering
3680 if (m_wLinObjectVel.Z == 0f)
3681 { // Gravity rules
3682 m_wLinObjectVel.Z = vel_now.Z;
3683 } // else the motor has it
3684 }
3685 linvel = m_wLinObjectVel;
3686
3687 // Vehicle Linear Motion done =======================================
3688 // Apply velocity
3689 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3690 // apply gravity force
3691 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3692 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3693 // end MoveLinear()
3694
3695
3696 // MoveAngular
3697 /*
3698 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3699
3700 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3701 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3702 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3703
3704 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3705 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3706 */
3707 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3708
3709 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3710 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3711 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3712
3713 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3714
3715 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3716 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3717 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3718 // Decay Angular Motor 2.
3719 if (m_angularMotorDecayTimescale < 300.0f)
3720 {
3721 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3722 {
3723 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3724 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3725 m_angularMotorDVel -= decayAmount;
3726 }
3727 else
3728 {
3729 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3730 m_angularMotorDVel -= decel;
3731 }
3732
3733 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3734 {
3735 m_angularMotorDVel = Vector3.Zero;
3736 }
3737 else
3738 {
3739 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3740 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3741 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3742 }
3743 } // end decay angular motor
3744 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3745
3746 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3747
3748 if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3749 { // if motor or object have motion
3750 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3751
3752 if (m_angularMotorTimescale < 300.0f)
3753 {
3754 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3755 float angfactor = m_angularMotorTimescale / timestep;
3756 Vector3 attackAmount = (attack_error / angfactor);
3757 angObjectVel += attackAmount;
3758 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3759 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3760 }
3761
3762 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3763 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3764 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3765 } // else no signif. motion
3766
3767 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3768 // Bank section tba
3769 // Deflection section tba
3770 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3771
3772
3773 /* // Rotation Axis Disables:
3774 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3775 {
3776 if (m_angularEnable.X == 0)
3777 angObjectVel.X = 0f;
3778 if (m_angularEnable.Y == 0)
3779 angObjectVel.Y = 0f;
3780 if (m_angularEnable.Z == 0)
3781 angObjectVel.Z = 0f;
3782 }
3783 */
3784 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3785
3786 // Vertical attractor section
3787 Vector3 vertattr = Vector3.Zero;
3788
3789 if (m_verticalAttractionTimescale < 300)
3790 {
3791 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3792 // make a vector pointing up
3793 Vector3 verterr = Vector3.Zero;
3794 verterr.Z = 1.0f;
3795 // rotate it to Body Angle
3796 verterr = verterr * rotq;
3797 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
3798 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
3799 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3800
3801 if (verterr.Z < 0.0f)
3802 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3803 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3804 //Console.WriteLine("InvertFlip");
3805 verterr.X = 2.0f - verterr.X;
3806 verterr.Y = 2.0f - verterr.Y;
3807 }
3808 verterr *= 0.5f;
3809 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3810 Vector3 xyav = angObjectVel;
3811 xyav.Z = 0.0f;
3812 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3813 {
3814 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3815 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3816 vertattr.X = verterr.Y;
3817 vertattr.Y = -verterr.X;
3818 vertattr.Z = 0f;
3819 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3820
3821 // scaling appears better usingsquare-law
3822 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3823 float bounce = 1.0f - damped;
3824 // 0 = crit damp, 1 = bouncy
3825 float oavz = angObjectVel.Z; // retain z velocity
3826 // time-scaled correction, which sums, therefore is bouncy:
3827 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3828 // damped, good @ < 90:
3829 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3830 angObjectVel.Z = oavz;
3831 //if(frcount == 0) Console.WriteLine("VA+");
3832 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3833 }
3834 else
3835 {
3836 // else error is very small
3837 angObjectVel.X = 0f;
3838 angObjectVel.Y = 0f;
3839 //if(frcount == 0) Console.WriteLine("VA0");
3840 }
3841 } // else vertical attractor is off
3842 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3843
3844
3845 m_lastAngularVelocity = angObjectVel;
3846 // apply Angular Velocity to body
3847 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3848 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3849
3850 } // end VEHICLES
3851 else
3852 {
3853 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3854
3855 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3856
3857 /// Dynamics Buoyancy
3858 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3859 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3860 // NB Prims in ODE are no subject to global gravity
3861 // This should only affect gravity operations
3862
3863 float m_mass = CalculateMass();
3864 // calculate z-force due togravity on object.
3865 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3866 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3867 {
3868 fz = 0; // llMoveToTarget ignores gravity.
3869 // it also ignores mass of object, and any physical resting on it.
3870 // Vector3 m_PIDTarget is where we are going
3871 // float m_PIDTau is time to get there
3872 fx = 0;
3873 fy = 0;
3874 d.Vector3 pos = d.BodyGetPosition(Body);
3875 Vector3 error = new Vector3(
3876 (m_PIDTarget.X - pos.X),
3877 (m_PIDTarget.Y - pos.Y),
3878 (m_PIDTarget.Z - pos.Z));
3879 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3880 { // Very close, Jump there and quit move
3881
3882 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3883 _target_velocity = Vector3.Zero;
3884 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3885 d.BodySetForce(Body, 0f, 0f, 0f);
3886 }
3887 else
3888 {
3889 float scale = 50.0f * timestep / m_PIDTau;
3890 if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
3891 {
3892 // Nearby, quit update of velocity
3893 }
3894 else
3895 { // Far, calc damped velocity
3896 _target_velocity = error * scale;
3897 }
3898 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3899 }
3900 } // end PID MoveToTarget
3901
3902
3903 /// Dynamics Hover ===================================================================================
3904 // Hover PID Controller can only run if the PIDcontroller is not in use.
3905 if (m_useHoverPID && !m_usePID)
3906 {
3907 //Console.WriteLine("Hover " + m_primName);
3908
3909 // If we're using the PID controller, then we have no gravity
3910 fz = (-1 * _parent_scene.gravityz) * m_mass;
3911
3912 // no lock; for now it's only called from within Simulate()
3913
3914 // If the PID Controller isn't active then we set our force
3915 // calculating base velocity to the current position
3916
3917 if ((m_PIDTau < 1))
3918 {
3919 PID_G = PID_G / m_PIDTau;
3920 }
3921
3922 if ((PID_G - m_PIDTau) <= 0)
3923 {
3924 PID_G = m_PIDTau + 1;
3925 }
3926
3927
3928 // Where are we, and where are we headed?
3929 d.Vector3 pos = d.BodyGetPosition(Body);
3930 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3931
3932
3933 // Non-Vehicles have a limited set of Hover options.
3934 // determine what our target height really is based on HoverType
3935 switch (m_PIDHoverType)
3936 {
3937 case PIDHoverType.Ground:
3938 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3939 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3940 break;
3941 case PIDHoverType.GroundAndWater:
3942 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3943 m_waterHeight = _parent_scene.GetWaterLevel();
3944 if (m_groundHeight > m_waterHeight)
3945 {
3946 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3947 }
3948 else
3949 {
3950 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3951 }
3952 break;
3953
3954 } // end switch (m_PIDHoverType)
3955
3956
3957 _target_velocity =
3958 new Vector3(0.0f, 0.0f,
3959 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3960 );
3961
3962 // if velocity is zero, use position control; otherwise, velocity control
3963
3964 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3965 {
3966 // keep track of where we stopped. No more slippin' & slidin'
3967
3968 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3969 // react to the physics scene by moving it's position.
3970 // Avatar to Avatar collisions
3971 // Prim to avatar collisions
3972 d.Vector3 dlinvel = vel;
3973 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3974 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3975 d.BodyAddForce(Body, 0, 0, fz);
3976 //KF this prevents furthur motions return;
3977 }
3978 else
3979 {
3980 _zeroFlag = false;
3981
3982 // We're flying and colliding with something
3983 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3984 }
3985 } // end m_useHoverPID && !m_usePID
3986
3987
3988 /// Dynamics Apply Forces ===================================================================================
3989 fx *= m_mass;
3990 fy *= m_mass;
3991 //fz *= m_mass;
3992 fx += m_force.X;
3993 fy += m_force.Y;
3994 fz += m_force.Z;
3995
3996 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3997 if (fx != 0 || fy != 0 || fz != 0)
3998 {
3999 //m_taintdisable = true;
4000 //base.RaiseOutOfBounds(Position);
4001 //d.BodySetLinearVel(Body, fx, fy, 0f);
4002 if (!d.BodyIsEnabled(Body))
4003 {
4004 // A physical body at rest on a surface will auto-disable after a while,
4005 // this appears to re-enable it incase the surface it is upon vanishes,
4006 // and the body should fall again.
4007 d.BodySetLinearVel(Body, 0f, 0f, 0f);
4008 d.BodySetForce(Body, 0f, 0f, 0f);
4009 enableBodySoft();
4010 }
4011
4012 // 35x10 = 350n times the mass per second applied maximum.
4013 float nmax = 35f * m_mass;
4014 float nmin = -35f * m_mass;
4015
4016
4017 if (fx > nmax)
4018 fx = nmax;
4019 if (fx < nmin)
4020 fx = nmin;
4021 if (fy > nmax)
4022 fy = nmax;
4023 if (fy < nmin)
4024 fy = nmin;
4025 d.BodyAddForce(Body, fx, fy, fz);
4026 } // end apply forces
4027 } // end Vehicle/Dynamics
4028
4029 /// RotLookAt / LookAt =================================================================================
4030 if (m_useAPID)
4031 {
4032 // RotLookAt, apparently overrides all other rotation sources. Inputs:
4033 // Quaternion m_APIDTarget
4034 // float m_APIDStrength // From SL experiments, this is the time to get there
4035 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
4036 // Also in SL the mass of the object has no effect on time to get there.
4037 // Factors:
4038 // get present body rotation
4039 float limit = 1.0f;
4040 float rscaler = 50f; // adjusts rotation damping time
4041 float lscaler = 10f; // adjusts linear damping time in llLookAt
4042 float RLAservo = 0f;
4043 Vector3 diff_axis;
4044 float diff_angle;
4045 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
4046 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
4047 Quaternion rtarget = new Quaternion();
4048
4049 if (m_APIDTarget.W == -99.9f)
4050 {
4051 // this is really a llLookAt(), x,y,z is the target vector
4052 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
4053 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
4054 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
4055 float twopi = 2.0f * (float)Math.PI;
4056 Vector3 dir = target - _position;
4057 dir.Normalize();
4058 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
4059 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
4060 float terot = (float)Math.Atan2(dir.Z, txy);
4061 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
4062 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
4063 float oerot = (float)Math.Atan2(ospin.Z, oxy);
4064 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
4065 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
4066 float roll = (float)Math.Atan2(ra, rb);
4067 float errorz = tzrot - ozrot;
4068 if (errorz > (float)Math.PI) errorz -= twopi;
4069 else if (errorz < -(float)Math.PI) errorz += twopi;
4070 float errory = oerot - terot;
4071 if (errory > (float)Math.PI) errory -= twopi;
4072 else if (errory < -(float)Math.PI) errory += twopi;
4073 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
4074 if (diff_angle > 0.01f * m_APIDdamper)
4075 {
4076 m_APIDdamper = 1.0f;
4077 RLAservo = timestep / m_APIDStrength * rscaler;
4078 errorz *= RLAservo;
4079 errory *= RLAservo;
4080 error.X = -roll * 8.0f;
4081 error.Y = errory;
4082 error.Z = errorz;
4083 error *= rotq;
4084 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
4085 }
4086 else
4087 {
4088 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4089 m_APIDdamper = 2.0f;
4090 }
4091 }
4092 else
4093 {
4094 // this is a llRotLookAt()
4095 rtarget = m_APIDTarget;
4096
4097 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
4098 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
4099 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
4100
4101 // diff_axis.Normalize(); it already is!
4102 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
4103 {
4104 m_APIDdamper = 1.0f;
4105 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
4106 rotforce = rotforce * rotq;
4107 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
4108 RLAservo = timestep / m_APIDStrength * lscaler;
4109 rotforce = rotforce * RLAservo * diff_angle;
4110 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
4111 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
4112 }
4113 else
4114 { // close enough
4115 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4116 m_APIDdamper = 2.0f;
4117 }
4118 } // end llLookAt/llRotLookAt
4119 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
4120 } // end m_useAPID
4121 } // end root prims
4122 } // end Move()
4123 } // end class
4124}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..712029e
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,384 @@
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 OpenMetaverse;
34using OpenSim.Region.Physics.Manager;
35using Ode.NET;
36using log4net;
37
38namespace OpenSim.Region.Physics.OdePlugin
39{
40 /// <summary>
41 /// Processes raycast requests as ODE is in a state to be able to do them.
42 /// This ensures that it's thread safe and there will be no conflicts.
43 /// Requests get returned by a different thread then they were requested by.
44 /// </summary>
45 public class ODERayCastRequestManager
46 {
47 /// <summary>
48 /// Pending Raycast Requests
49 /// </summary>
50 protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
51
52 /// <summary>
53 /// Scene that created this object.
54 /// </summary>
55 private OdeScene m_scene;
56
57 /// <summary>
58 /// ODE contact array to be filled by the collision testing
59 /// </summary>
60 d.ContactGeom[] contacts = new d.ContactGeom[5];
61
62 /// <summary>
63 /// ODE near callback delegate
64 /// </summary>
65 private d.NearCallback nearCallback;
66 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
67 private List<ContactResult> m_contactResults = new List<ContactResult>();
68
69
70 public ODERayCastRequestManager(OdeScene pScene)
71 {
72 m_scene = pScene;
73 nearCallback = near;
74
75 }
76
77 /// <summary>
78 /// Queues a raycast
79 /// </summary>
80 /// <param name="position">Origin of Ray</param>
81 /// <param name="direction">Ray normal</param>
82 /// <param name="length">Ray length</param>
83 /// <param name="retMethod">Return method to send the results</param>
84 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
85 {
86 lock (m_PendingRequests)
87 {
88 ODERayCastRequest req = new ODERayCastRequest();
89 req.callbackMethod = retMethod;
90 req.length = length;
91 req.Normal = direction;
92 req.Origin = position;
93
94 m_PendingRequests.Add(req);
95 }
96 }
97
98 /// <summary>
99 /// Process all queued raycast requests
100 /// </summary>
101 /// <returns>Time in MS the raycasts took to process.</returns>
102 public int ProcessQueuedRequests()
103 {
104 int time = System.Environment.TickCount;
105 lock (m_PendingRequests)
106 {
107 if (m_PendingRequests.Count > 0)
108 {
109 ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
110 for (int i = 0; i < reqs.Length; i++)
111 {
112 try
113 {
114 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
115 RayCast(reqs[i]); // if there isn't anyone to send results
116 }
117 catch
118 {
119 //Fail silently
120 //This can genuinely happen because raycast requests are queued, and the actor may have
121 //been removed from the scene since it was queued
122 }
123 }
124 /*
125 foreach (ODERayCastRequest req in m_PendingRequests)
126 {
127 if (req.callbackMethod != null) // quick optimization here, don't raycast
128 RayCast(req); // if there isn't anyone to send results to
129
130 }
131 */
132 m_PendingRequests.Clear();
133 }
134 }
135
136 lock (m_contactResults)
137 m_contactResults.Clear();
138
139 return System.Environment.TickCount - time;
140 }
141
142 /// <summary>
143 /// Method that actually initiates the raycast
144 /// </summary>
145 /// <param name="req"></param>
146 private void RayCast(ODERayCastRequest req)
147 {
148 // Create the ray
149 IntPtr ray = d.CreateRay(m_scene.space, req.length);
150 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
151
152 // Collide test
153 d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
154
155 // Remove Ray
156 d.GeomDestroy(ray);
157
158
159 // Define default results
160 bool hitYN = false;
161 uint hitConsumerID = 0;
162 float distance = 999999999999f;
163 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
164 Vector3 snormal = Vector3.Zero;
165
166 // Find closest contact and object.
167 lock (m_contactResults)
168 {
169 foreach (ContactResult cResult in m_contactResults)
170 {
171 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
172 {
173 closestcontact = cResult.Pos;
174 hitConsumerID = cResult.ConsumerID;
175 distance = cResult.Depth;
176 hitYN = true;
177 snormal = cResult.Normal;
178 }
179 }
180
181 m_contactResults.Clear();
182 }
183
184 // Return results
185 if (req.callbackMethod != null)
186 req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
187 }
188
189 // This is the standard Near. Uses space AABBs to speed up detection.
190 private void near(IntPtr space, IntPtr g1, IntPtr g2)
191 {
192
193 //Don't test against heightfield Geom, or you'll be sorry!
194
195 /*
196 terminate called after throwing an instance of 'std::bad_alloc'
197 what(): std::bad_alloc
198 Stacktrace:
199
200 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
201 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
202 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
203 at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
204 fffff>
205 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
206 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
207 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
208 0x00114>
209 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
210 at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
211 at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
212 at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
213 at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
214 at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
215
216 Native stacktrace:
217
218 mono [0x80d2a42]
219 [0xb7f5840c]
220 /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
221 /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
222 /usr/lib/libstdc++.so.6 [0xb45fa865]
223 /usr/lib/libstdc++.so.6 [0xb45fa8a2]
224 /usr/lib/libstdc++.so.6 [0xb45fa9da]
225 /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
226 /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
227 libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
228 libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
229 libode.so(dCollide+0x102) [0xb46571b2]
230 [0x95cfdec9]
231 [0x8ea07fe1]
232 [0xab260146]
233 libode.so [0xb465a5c4]
234 libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
235 libode.so(dSpaceCollide2+0x177) [0xb465ac67]
236 [0x95cf978e]
237 [0x8ea07945]
238 [0x95cf2bbc]
239 [0xab2787e7]
240 [0xab419fb3]
241 [0xab416657]
242 [0xab415bda]
243 [0xb609b08e]
244 mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
245 mono [0x81a2f0f]
246 mono [0x81d28b6]
247 mono [0x81ea2c6]
248 /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
249 /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
250 */
251
252 // Exclude heightfield geom
253
254 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
255 return;
256 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
257 return;
258
259 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
260 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
261 {
262 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
263 return;
264
265 // Separating static prim geometry spaces.
266 // We'll be calling near recursivly if one
267 // of them is a space to find all of the
268 // contact points in the space
269 try
270 {
271 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
272 }
273 catch (AccessViolationException)
274 {
275 m_log.Warn("[PHYSICS]: Unable to collide test a space");
276 return;
277 }
278 //Colliding a space or a geom with a space or a geom. so drill down
279
280 //Collide all geoms in each space..
281 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
282 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
283 return;
284 }
285
286 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
287 return;
288
289 int count = 0;
290 try
291 {
292
293 if (g1 == g2)
294 return; // Can't collide with yourself
295
296 lock (contacts)
297 {
298 count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
299 }
300 }
301 catch (SEHException)
302 {
303 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.");
304 }
305 catch (Exception e)
306 {
307 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
308 return;
309 }
310
311 PhysicsActor p1 = null;
312 PhysicsActor p2 = null;
313
314 if (g1 != IntPtr.Zero)
315 m_scene.actor_name_map.TryGetValue(g1, out p1);
316
317 if (g2 != IntPtr.Zero)
318 m_scene.actor_name_map.TryGetValue(g1, out p2);
319
320 // Loop over contacts, build results.
321 for (int i = 0; i < count; i++)
322 {
323 if (p1 != null) {
324 if (p1 is OdePrim)
325 {
326 ContactResult collisionresult = new ContactResult();
327
328 collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
329 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
330 collisionresult.Depth = contacts[i].depth;
331 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
332 contacts[i].normal.Z);
333 lock (m_contactResults)
334 m_contactResults.Add(collisionresult);
335 }
336 }
337
338 if (p2 != null)
339 {
340 if (p2 is OdePrim)
341 {
342 ContactResult collisionresult = new ContactResult();
343
344 collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
345 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
346 collisionresult.Depth = contacts[i].depth;
347 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
348 contacts[i].normal.Z);
349
350 lock (m_contactResults)
351 m_contactResults.Add(collisionresult);
352 }
353 }
354
355
356 }
357
358 }
359
360 /// <summary>
361 /// Dereference the creator scene so that it can be garbage collected if needed.
362 /// </summary>
363 internal void Dispose()
364 {
365 m_scene = null;
366 }
367 }
368
369 public struct ODERayCastRequest
370 {
371 public Vector3 Origin;
372 public Vector3 Normal;
373 public float length;
374 public RaycastCallback callbackMethod;
375 }
376
377 public struct ContactResult
378 {
379 public Vector3 Pos;
380 public float Depth;
381 public uint ConsumerID;
382 public Vector3 Normal;
383 }
384}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
new file mode 100644
index 0000000..b4a3c48
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePhysicsJoint.cs
@@ -0,0 +1,48 @@
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 OpenMetaverse;
30using Ode.NET;
31using OpenSim.Framework;
32using OpenSim.Region.Physics.Manager;
33using OpenSim.Region.Physics.OdePlugin;
34
35namespace OpenSim.Region.Physics.OdePlugin
36{
37 class OdePhysicsJoint : PhysicsJoint
38 {
39 public override bool IsInPhysicsEngine
40 {
41 get
42 {
43 return (jointID != IntPtr.Zero);
44 }
45 }
46 public IntPtr jointID;
47 }
48}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..00f5122
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3887 @@
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 USE_DRAWSTUFF
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 Ode.NET;
40#if USE_DRAWSTUFF
41using Drawstuff.NET;
42#endif
43using OpenSim.Framework;
44using OpenSim.Region.Physics.Manager;
45using OpenMetaverse;
46
47//using OpenSim.Region.Physics.OdePlugin.Meshing;
48
49namespace OpenSim.Region.Physics.OdePlugin
50{
51 /// <summary>
52 /// ODE plugin
53 /// </summary>
54 public class OdePlugin : IPhysicsPlugin
55 {
56 //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
57
58 private CollisionLocker ode;
59 private OdeScene _mScene;
60
61 public OdePlugin()
62 {
63 ode = new CollisionLocker();
64 }
65
66 public bool Init()
67 {
68 return true;
69 }
70
71 public PhysicsScene GetScene(String sceneIdentifier)
72 {
73 if (_mScene == null)
74 {
75 if (Util.IsWindows())
76 Util.LoadArchSpecificWindowsDll("ode.dll");
77
78 // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
79 // http://opensimulator.org/mantis/view.php?id=2750).
80 d.InitODE();
81
82 _mScene = new OdeScene(ode, sceneIdentifier);
83 }
84 return (_mScene);
85 }
86
87 public string GetName()
88 {
89 return ("ChODE");
90 }
91
92 public void Dispose()
93 {
94 }
95 }
96
97 public enum StatusIndicators : int
98 {
99 Generic = 0,
100 Start = 1,
101 End = 2
102 }
103
104 public struct sCollisionData
105 {
106 public uint ColliderLocalId;
107 public uint CollidedWithLocalId;
108 public int NumberOfCollisions;
109 public int CollisionType;
110 public int StatusIndicator;
111 public int lastframe;
112 }
113
114 [Flags]
115 public enum CollisionCategories : int
116 {
117 Disabled = 0,
118 Geom = 0x00000001,
119 Body = 0x00000002,
120 Space = 0x00000004,
121 Character = 0x00000008,
122 Land = 0x00000010,
123 Water = 0x00000020,
124 Wind = 0x00000040,
125 Sensor = 0x00000080,
126 Selected = 0x00000100
127 }
128
129 /// <summary>
130 /// Material type for a primitive
131 /// </summary>
132 public enum Material : int
133 {
134 /// <summary></summary>
135 Stone = 0,
136 /// <summary></summary>
137 Metal = 1,
138 /// <summary></summary>
139 Glass = 2,
140 /// <summary></summary>
141 Wood = 3,
142 /// <summary></summary>
143 Flesh = 4,
144 /// <summary></summary>
145 Plastic = 5,
146 /// <summary></summary>
147 Rubber = 6
148
149 }
150
151 public sealed class OdeScene : PhysicsScene
152 {
153 private readonly ILog m_log;
154 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
155
156 CollisionLocker ode;
157
158 private Random fluidRandomizer = new Random(Environment.TickCount);
159
160 private const uint m_regionWidth = Constants.RegionSize;
161 private const uint m_regionHeight = Constants.RegionSize;
162
163 private float ODE_STEPSIZE = 0.020f;
164 private float metersInSpace = 29.9f;
165 private float m_timeDilation = 1.0f;
166
167 public float gravityx = 0f;
168 public float gravityy = 0f;
169 public float gravityz = -9.8f;
170
171 private float contactsurfacelayer = 0.001f;
172
173 private int worldHashspaceLow = -4;
174 private int worldHashspaceHigh = 128;
175
176 private int smallHashspaceLow = -4;
177 private int smallHashspaceHigh = 66;
178
179 private float waterlevel = 0f;
180 private int framecount = 0;
181 //private int m_returncollisions = 10;
182
183 private readonly IntPtr contactgroup;
184
185 internal IntPtr LandGeom;
186 internal IntPtr WaterGeom;
187
188 private float nmTerrainContactFriction = 255.0f;
189 private float nmTerrainContactBounce = 0.1f;
190 private float nmTerrainContactERP = 0.1025f;
191
192 private float mTerrainContactFriction = 75f;
193 private float mTerrainContactBounce = 0.1f;
194 private float mTerrainContactERP = 0.05025f;
195
196 private float nmAvatarObjectContactFriction = 250f;
197 private float nmAvatarObjectContactBounce = 0.1f;
198
199 private float mAvatarObjectContactFriction = 75f;
200 private float mAvatarObjectContactBounce = 0.1f;
201
202 private float avPIDD = 3200f;
203 private float avPIDP = 1400f;
204 private float avCapRadius = 0.37f;
205 private float avStandupTensor = 2000000f;
206 private bool avCapsuleTilted = true; // true = old compatibility mode with leaning capsule; false = new corrected mode
207 public bool IsAvCapsuleTilted { get { return avCapsuleTilted; } set { avCapsuleTilted = value; } }
208 private float avDensity = 80f;
209 private float avHeightFudgeFactor = 0.52f;
210 private float avMovementDivisorWalk = 1.3f;
211 private float avMovementDivisorRun = 0.8f;
212 private float minimumGroundFlightOffset = 3f;
213 public float maximumMassObject = 10000.01f;
214
215 public bool meshSculptedPrim = true;
216 public bool forceSimplePrimMeshing = false;
217
218 public float meshSculptLOD = 32;
219 public float MeshSculptphysicalLOD = 16;
220
221 public float geomDefaultDensity = 10.000006836f;
222
223 public int geomContactPointsStartthrottle = 3;
224 public int geomUpdatesPerThrottledUpdate = 15;
225
226 public float bodyPIDD = 35f;
227 public float bodyPIDG = 25;
228
229 public int geomCrossingFailuresBeforeOutofbounds = 5;
230 public float geomRegionFence = 0.0f;
231
232 public float bodyMotorJointMaxforceTensor = 2;
233
234 public int bodyFramesAutoDisable = 20;
235
236 private DateTime m_lastframe = DateTime.UtcNow;
237
238 private float[] _watermap;
239 private bool m_filterCollisions = true;
240
241 private d.NearCallback nearCallback;
242 public d.TriCallback triCallback;
243 public d.TriArrayCallback triArrayCallback;
244 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
245 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
246 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
247 private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
248 private readonly Object _taintedPrimLock = new Object();
249 private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
250 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
251 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
252 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
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 private bool m_NINJA_physics_joints_enabled = false;
257 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
258 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
259 private d.ContactGeom[] contacts;
260 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
261 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
262 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
263 private readonly List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
264 private Object externalJointRequestsLock = new Object();
265 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
266 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
267 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
268 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
269
270 private d.Contact ContactCopy; // local copy that can be modified
271 private d.Contact TerrainContact;
272 private d.Contact AvatarStaticprimContact; // was 'contact'
273 private d.Contact AvatarMovementprimContact;
274 private d.Contact AvatarMovementTerrainContact;
275 private d.Contact WaterContact;
276 private d.Contact[,] m_materialContacts;
277
278//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
279//Ckrinke private int m_randomizeWater = 200;
280 private int m_physicsiterations = 10;
281 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
282 private readonly PhysicsActor PANull = new NullPhysicsActor();
283 private float step_time = 0.0f;
284//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
285//Ckrinke private int ms = 0;
286 public IntPtr world;
287 //private bool returncollisions = false;
288 // private uint obj1LocalID = 0;
289 private uint obj2LocalID = 0;
290 //private int ctype = 0;
291 private OdeCharacter cc1;
292 private OdePrim cp1;
293 private OdeCharacter cc2;
294 private OdePrim cp2;
295 //private int cStartStop = 0;
296 //private string cDictKey = "";
297
298 public IntPtr space;
299
300 //private IntPtr tmpSpace;
301 // split static geometry collision handling into spaces of 30 meters
302 public IntPtr[,] staticPrimspace;
303
304 public Object OdeLock;
305
306 public IMesher mesher;
307
308 private IConfigSource m_config;
309
310 public bool physics_logging = false;
311 public int physics_logging_interval = 0;
312 public bool physics_logging_append_existing_logfile = false;
313
314 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
315 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
316
317 // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
318 // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
319 // TODO: unused: private uint heightmapWidthSamples;
320 // TODO: unused: private uint heightmapHeightSamples;
321
322 private volatile int m_global_contactcount = 0;
323
324 private Vector3 m_worldOffset = Vector3.Zero;
325 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
326 private PhysicsScene m_parentScene = null;
327
328 private ODERayCastRequestManager m_rayCastManager;
329
330 /// <summary>
331 /// Initiailizes the scene
332 /// Sets many properties that ODE requires to be stable
333 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
334 /// </summary>
335 public OdeScene(CollisionLocker dode, string sceneIdentifier)
336 {
337 m_log
338 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
339
340 OdeLock = new Object();
341 ode = dode;
342 nearCallback = near;
343 triCallback = TriCallback;
344 triArrayCallback = TriArrayCallback;
345 m_rayCastManager = new ODERayCastRequestManager(this);
346 lock (OdeLock)
347 {
348 // Create the world and the first space
349 world = d.WorldCreate();
350 space = d.HashSpaceCreate(IntPtr.Zero);
351
352
353 contactgroup = d.JointGroupCreate(0);
354 //contactgroup
355
356 d.WorldSetAutoDisableFlag(world, false);
357 #if USE_DRAWSTUFF
358
359 Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
360 viewthread.Start();
361 #endif
362 }
363
364
365 _watermap = new float[258 * 258];
366
367 // Zero out the prim spaces array (we split our space into smaller spaces so
368 // we can hit test less.
369 }
370
371#if USE_DRAWSTUFF
372 public void startvisualization(object o)
373 {
374 ds.Functions fn;
375 fn.version = ds.VERSION;
376 fn.start = new ds.CallbackFunction(start);
377 fn.step = new ds.CallbackFunction(step);
378 fn.command = new ds.CallbackFunction(command);
379 fn.stop = null;
380 fn.path_to_textures = "./textures";
381 string[] args = new string[0];
382 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
383 }
384#endif
385
386 // Initialize the mesh plugin
387 public override void Initialise(IMesher meshmerizer, IConfigSource config)
388 {
389 mesher = meshmerizer;
390 m_config = config;
391 // Defaults
392
393 if (Environment.OSVersion.Platform == PlatformID.Unix)
394 {
395 avPIDD = 3200.0f;
396 avPIDP = 1400.0f;
397 avStandupTensor = 2000000f;
398 }
399 else
400 {
401 avPIDD = 2200.0f;
402 avPIDP = 900.0f;
403 avStandupTensor = 550000f;
404 }
405
406 int contactsPerCollision = 80;
407
408 if (m_config != null)
409 {
410 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
411 if (physicsconfig != null)
412 {
413 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
414 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
415 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
416
417 worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
418 worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
419
420 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
421 smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
422 smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
423
424 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
425
426 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
427 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
428 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
429
430 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
431 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
432 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
433
434 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
435 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
436
437 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
438 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
439
440 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
441 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
442
443 avDensity = physicsconfig.GetFloat("av_density", 80f);
444 avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
445 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
446 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
447 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
448 avCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
449
450 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
451
452 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
453 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
454 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
455 geomRegionFence = physicsconfig.GetFloat("region_border_fence", 0.0f);
456
457 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
458 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
459
460 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
461 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
462
463 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
464 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
465 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
466 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
467 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
468
469 if (Environment.OSVersion.Platform == PlatformID.Unix)
470 {
471 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
472 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
473 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
474 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
475 }
476 else
477 {
478 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
479 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
480 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
481 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
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 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
489 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
490 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
491 }
492 }
493
494 contacts = new d.ContactGeom[contactsPerCollision];
495
496 staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
497
498 // Avatar static on a Prim parameters
499 AvatarStaticprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
500 AvatarStaticprimContact.surface.mu = 255.0f;
501 AvatarStaticprimContact.surface.bounce = 0.0f;
502 AvatarStaticprimContact.surface.soft_cfm = 0.0f;
503 AvatarStaticprimContact.surface.soft_erp = 0.30f; // If this is too small static Av will fall through a sloping prim. 1.0 prevents fall-thru
504
505 // Avatar moving on a Prim parameters
506 AvatarMovementprimContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
507 AvatarMovementprimContact.surface.mu = 255.0f;
508 AvatarMovementprimContact.surface.bounce = 0.0f;
509 AvatarMovementprimContact.surface.soft_cfm = 0.0f; // if this is 0.01 then prims become phantom to Avs!
510 AvatarMovementprimContact.surface.soft_erp = 0.3f;
511
512 // Static Avatar on Terrain parameters
513 // Keeps Avatar in place better
514 TerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
515 TerrainContact.surface.mu = 255.0f;
516 TerrainContact.surface.bounce = 0.0f;
517 TerrainContact.surface.soft_cfm = 0.0f;
518 TerrainContact.surface.soft_erp = 0.05f;
519
520 // Moving Avatar on Terrain parameters
521 AvatarMovementTerrainContact.surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
522 AvatarMovementTerrainContact.surface.mu = 75f;
523 AvatarMovementTerrainContact.surface.bounce = 0.0f;
524 AvatarMovementTerrainContact.surface.soft_cfm = 0.0f;
525 AvatarMovementTerrainContact.surface.soft_erp = 0.05f;
526
527 // Avatar or prim the the water, this may not be used, possibly water is same as air?
528 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
529 WaterContact.surface.mu = 0.0f; // No friction
530 WaterContact.surface.bounce = 0.0f; // No bounce
531 WaterContact.surface.soft_cfm = 0.010f;
532 WaterContact.surface.soft_erp = 0.010f;
533
534
535 // Prim static or moving on a prim, depends on material type
536 m_materialContacts = new d.Contact[7,2];
537 // V 1 = Sliding; 0 = static or fell onto
538 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
539 m_materialContacts[(int)Material.Stone, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
540 m_materialContacts[(int)Material.Stone, 0].surface.mu = 60f; // friction, 1 = slippery, 255 = no slip
541 m_materialContacts[(int)Material.Stone, 0].surface.bounce = 0.0f;
542 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.0f;
543 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.50f; // erp also changes friction, more erp=less friction
544
545 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
546 m_materialContacts[(int)Material.Stone, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
547 m_materialContacts[(int)Material.Stone, 1].surface.mu = 40f;
548 m_materialContacts[(int)Material.Stone, 1].surface.bounce = 0.0f;
549 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.0f;
550 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.50f;
551
552 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
553 m_materialContacts[(int)Material.Metal, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
554 m_materialContacts[(int)Material.Metal, 0].surface.mu = 15f;
555 m_materialContacts[(int)Material.Metal, 0].surface.bounce = 0.2f;
556 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.0f;
557 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.50f;
558
559 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
560 m_materialContacts[(int)Material.Metal, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
561 m_materialContacts[(int)Material.Metal, 1].surface.mu = 10f;
562 m_materialContacts[(int)Material.Metal, 1].surface.bounce = 0.2f;
563 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.0f;
564 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.50f;
565
566 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
567 m_materialContacts[(int)Material.Glass, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
568 m_materialContacts[(int)Material.Glass, 0].surface.mu = 7.5f;
569 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.0f;
570 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.0f;
571 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.50f;
572
573 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
574 m_materialContacts[(int)Material.Glass, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
575 m_materialContacts[(int)Material.Glass, 1].surface.mu = 5f;
576 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.0f;
577 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.0f;
578 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.50f;
579
580 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
581 m_materialContacts[(int)Material.Wood, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
582 m_materialContacts[(int)Material.Wood, 0].surface.mu = 45f;
583 m_materialContacts[(int)Material.Wood, 0].surface.bounce = 0.1f;
584 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.0f;
585 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.50f;
586
587 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
588 m_materialContacts[(int)Material.Wood, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
589 m_materialContacts[(int)Material.Wood, 1].surface.mu = 30f;
590 m_materialContacts[(int)Material.Wood, 1].surface.bounce = 0.1f;
591 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.0f;
592 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.50f;
593
594 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
595 m_materialContacts[(int)Material.Flesh, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
596 m_materialContacts[(int)Material.Flesh, 0].surface.mu = 150f;
597 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = 0.0f;
598 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.0f;
599 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.50f;
600
601 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
602 m_materialContacts[(int)Material.Flesh, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
603 m_materialContacts[(int)Material.Flesh, 1].surface.mu = 100f;
604 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = 0.0f;
605 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.0f;
606 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.50f;
607
608 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
609 m_materialContacts[(int)Material.Plastic, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
610 m_materialContacts[(int)Material.Plastic, 0].surface.mu = 30f;
611 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = 0.2f;
612 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.0f;
613 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.50f;
614
615 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
616 m_materialContacts[(int)Material.Plastic, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
617 m_materialContacts[(int)Material.Plastic, 1].surface.mu = 20f;
618 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = 0.2f;
619 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.0f;
620 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.50f;
621
622 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
623 m_materialContacts[(int)Material.Rubber, 0].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
624 m_materialContacts[(int)Material.Rubber, 0].surface.mu = 150f;
625 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = 0.7f;
626 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.0f;
627 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.50f;
628
629 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
630 m_materialContacts[(int)Material.Rubber, 1].surface.mode = d.ContactFlags.SoftCFM | d.ContactFlags.SoftERP | d.ContactFlags.Bounce;
631 m_materialContacts[(int)Material.Rubber, 1].surface.mu = 100f;
632 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = 0.7f;
633 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.0f;
634 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.50f;
635
636 d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
637
638 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
639
640 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
641 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
642
643
644 d.WorldSetLinearDampingThreshold(world, 256f);
645 d.WorldSetLinearDamping(world, 256f);
646// d.WorldSetLinearDampingThreshold(world, 0.01f);
647// d.WorldSetLinearDamping(world, 0.1f);
648 d.WorldSetAngularDampingThreshold(world, 256f);
649 d.WorldSetAngularDamping(world, 256f);
650 d.WorldSetMaxAngularSpeed(world, 256f);
651
652 // Set how many steps we go without running collision testing
653 // This is in addition to the step size.
654 // Essentially Steps * m_physicsiterations
655 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
656 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
657
658
659
660 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
661 {
662 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
663 {
664 staticPrimspace[i, j] = IntPtr.Zero;
665 }
666 }
667 }
668
669 internal void waitForSpaceUnlock(IntPtr space)
670 {
671 //if (space != IntPtr.Zero)
672 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
673 }
674
675 /// <summary>
676 /// Debug space message for printing the space that a prim/avatar is in.
677 /// </summary>
678 /// <param name="pos"></param>
679 /// <returns>Returns which split up space the given position is in.</returns>
680 public string whichspaceamIin(Vector3 pos)
681 {
682 return calculateSpaceForGeom(pos).ToString();
683 }
684
685 #region Collision Detection
686
687 /// <summary>
688 /// This is our near callback. A geometry is near a body
689 /// </summary>
690 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
691 /// <param name="g1">a geometry or space</param>
692 /// <param name="g2">another geometry or space</param>
693 private void near(IntPtr space, IntPtr g1, IntPtr g2)
694 {
695 // no lock here! It's invoked from within Simulate(), which is thread-locked
696
697 // Test if we're colliding a geom with a space.
698 // If so we have to drill down into the space recursively
699//Console.WriteLine("near -----------"); //##
700 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
701 {
702 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
703 return;
704
705 // Separating static prim geometry spaces.
706 // We'll be calling near recursivly if one
707 // of them is a space to find all of the
708 // contact points in the space
709 try
710 {
711 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
712 }
713 catch (AccessViolationException)
714 {
715 m_log.Warn("[PHYSICS]: Unable to collide test a space");
716 return;
717 }
718 //Colliding a space or a geom with a space or a geom. so drill down
719
720 //Collide all geoms in each space..
721 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
722 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
723 return;
724 }
725
726 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
727 return;
728
729 IntPtr b1 = d.GeomGetBody(g1);
730 IntPtr b2 = d.GeomGetBody(g2);
731
732 // d.GeomClassID id = d.GeomGetClass(g1);
733
734 String name1 = null;
735 String name2 = null;
736
737 if (!geom_name_map.TryGetValue(g1, out name1))
738 {
739 name1 = "null";
740 }
741 if (!geom_name_map.TryGetValue(g2, out name2))
742 {
743 name2 = "null";
744 }
745
746 //if (id == d.GeomClassId.TriMeshClass)
747 //{
748 // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
749 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
750 //}
751
752 // Figure out how many contact points we have
753 int count = 0;
754 try
755 {
756 // Colliding Geom To Geom
757 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
758
759 if (g1 == g2)
760 return; // Can't collide with yourself
761
762 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
763 return;
764
765 lock (contacts)
766 {
767 count = d.Collide(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
768 if (count > contacts.Length)
769 m_log.Error("[PHYSICS]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
770 }
771 }
772 catch (SEHException)
773 {
774 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.");
775 ode.drelease(world);
776 base.TriggerPhysicsBasedRestart();
777 }
778 catch (Exception e)
779 {
780 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
781 return;
782 }
783
784 PhysicsActor p1;
785 PhysicsActor p2;
786
787 if (!actor_name_map.TryGetValue(g1, out p1))
788 {
789 p1 = PANull;
790 }
791
792 if (!actor_name_map.TryGetValue(g2, out p2))
793 {
794 p2 = PANull;
795 }
796
797 ContactPoint maxDepthContact = new ContactPoint();
798 if (p1.CollisionScore + count >= float.MaxValue)
799 p1.CollisionScore = 0;
800 p1.CollisionScore += count;
801
802 if (p2.CollisionScore + count >= float.MaxValue)
803 p2.CollisionScore = 0;
804 p2.CollisionScore += count;
805 for (int i = 0; i < count; i++)
806 {
807 d.ContactGeom curContact = contacts[i];
808
809 if (curContact.depth > maxDepthContact.PenetrationDepth)
810 {
811 maxDepthContact = new ContactPoint(
812 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
813 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
814 curContact.depth
815 );
816 }
817
818 //m_log.Warn("[CCOUNT]: " + count);
819 IntPtr joint;
820 // If we're colliding with terrain, use 'TerrainContact' instead of AvatarStaticprimContact.
821 // allows us to have different settings
822
823 // We only need to test p2 for 'jump crouch purposes'
824 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
825 {
826 // Testing if the collision is at the feet of the avatar
827
828 //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
829//#@ if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
830//#@ p2.IsColliding = true;
831 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)){ //##
832 p2.IsColliding = true; //##
833 }else{
834
835 } //##
836 }
837 else
838 {
839 p2.IsColliding = true;
840 }
841
842 //if ((framecount % m_returncollisions) == 0)
843
844 switch (p1.PhysicsActorType)
845 {
846 case (int)ActorTypes.Agent:
847 p2.CollidingObj = true;
848 break;
849 case (int)ActorTypes.Prim:
850 if (p2.Velocity.LengthSquared() > 0.0f)
851 p2.CollidingObj = true;
852 break;
853 case (int)ActorTypes.Unknown:
854 p2.CollidingGround = true;
855 break;
856 default:
857 p2.CollidingGround = true;
858 break;
859 }
860
861 // we don't want prim or avatar to explode
862
863 #region InterPenetration Handling - Unintended physics explosions
864# region disabled code1
865
866 if (curContact.depth >= 0.08f)
867 {
868 //This is disabled at the moment only because it needs more tweaking
869 //It will eventually be uncommented
870 /*
871 if (AvatarStaticprimContact.depth >= 1.00f)
872 {
873 //m_log.Debug("[PHYSICS]: " + AvatarStaticprimContact.depth.ToString());
874 }
875
876 //If you interpenetrate a prim with an agent
877 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
878 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
879 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
880 p2.PhysicsActorType == (int) ActorTypes.Prim))
881 {
882
883 //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth * 4.15f;
884 /*
885 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
886 {
887 p2.CollidingObj = true;
888 AvatarStaticprimContact.depth = 0.003f;
889 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
890 OdeCharacter character = (OdeCharacter) p2;
891 character.SetPidStatus(true);
892 AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p1.Size.X / 2), AvatarStaticprimContact.pos.Y + (p1.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p1.Size.Z / 2));
893
894 }
895 else
896 {
897
898 //AvatarStaticprimContact.depth = 0.0000000f;
899 }
900 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
901 {
902
903 p1.CollidingObj = true;
904 AvatarStaticprimContact.depth = 0.003f;
905 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
906 AvatarStaticprimContact.pos = new d.Vector3(AvatarStaticprimContact.pos.X + (p2.Size.X / 2), AvatarStaticprimContact.pos.Y + (p2.Size.Y / 2), AvatarStaticprimContact.pos.Z + (p2.Size.Z / 2));
907 OdeCharacter character = (OdeCharacter)p1;
908 character.SetPidStatus(true);
909 }
910 else
911 {
912
913 //AvatarStaticprimContact.depth = 0.0000000f;
914 }
915
916
917
918 }
919*/
920 // If you interpenetrate a prim with another prim
921 /*
922 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
923 {
924 #region disabledcode2
925 //OdePrim op1 = (OdePrim)p1;
926 //OdePrim op2 = (OdePrim)p2;
927 //op1.m_collisionscore++;
928 //op2.m_collisionscore++;
929
930 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
931 //{
932 //op1.m_taintdisable = true;
933 //AddPhysicsActorTaint(p1);
934 //op2.m_taintdisable = true;
935 //AddPhysicsActorTaint(p2);
936 //}
937
938 //if (AvatarStaticprimContact.depth >= 0.25f)
939 //{
940 // Don't collide, one or both prim will expld.
941
942 //op1.m_interpenetrationcount++;
943 //op2.m_interpenetrationcount++;
944 //interpenetrations_before_disable = 200;
945 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
946 //{
947 //op1.m_taintdisable = true;
948 //AddPhysicsActorTaint(p1);
949 //}
950 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
951 //{
952 // op2.m_taintdisable = true;
953 //AddPhysicsActorTaint(p2);
954 //}
955
956 //AvatarStaticprimContact.depth = AvatarStaticprimContact.depth / 8f;
957 //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1);
958 //}
959 //if (op1.m_disabled || op2.m_disabled)
960 //{
961 //Manually disabled objects stay disabled
962 //AvatarStaticprimContact.depth = 0f;
963 //}
964 #endregion
965 }
966 */
967#endregion
968 if (curContact.depth >= 1.00f)
969 {
970 //m_log.Info("[P]: " + AvatarStaticprimContact.depth.ToString());
971 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
972 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
973 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
974 p2.PhysicsActorType == (int) ActorTypes.Unknown))
975 {
976 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
977 {
978 if (p2 is OdeCharacter)
979 {
980 OdeCharacter character = (OdeCharacter) p2;
981
982 //p2.CollidingObj = true;
983 curContact.depth = 0.00000003f;
984 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
985 curContact.pos =
986 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
987 curContact.pos.Y + (p1.Size.Y/2),
988 curContact.pos.Z + (p1.Size.Z/2));
989 character.SetPidStatus(true);
990 }
991 }
992
993
994 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
995 {
996 if (p1 is OdeCharacter)
997 {
998 OdeCharacter character = (OdeCharacter) p1;
999
1000 //p2.CollidingObj = true;
1001 curContact.depth = 0.00000003f;
1002 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1003 curContact.pos =
1004 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1005 curContact.pos.Y + (p1.Size.Y/2),
1006 curContact.pos.Z + (p1.Size.Z/2));
1007 character.SetPidStatus(true);
1008 }
1009 }
1010 }
1011 }
1012 }
1013
1014 #endregion
1015
1016 // Logic for collision handling
1017 // Note, that if *all* contacts are skipped (VolumeDetect)
1018 // The prim still detects (and forwards) collision events but
1019 // appears to be phantom for the world
1020 Boolean skipThisContact = false;
1021
1022 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1023 skipThisContact = true; // No collision on volume detect prims
1024
1025 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1026 skipThisContact = true; // No collision on volume detect prims
1027
1028 if (!skipThisContact && curContact.depth < 0f)
1029 skipThisContact = true;
1030
1031 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1032 skipThisContact = true;
1033
1034 const int maxContactsbeforedeath = 4000;
1035 joint = IntPtr.Zero;
1036
1037 if (!skipThisContact)
1038 {
1039 // Add contact joints with materials params----------------------------------
1040 // p1 is what is being hit, p2 is the physical object doing the hitting
1041 int material = (int) Material.Wood;
1042 int movintYN = 0; // 1 = Sliding; 0 = static or fell onto
1043 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) movintYN = 1;
1044
1045 // If we're colliding against terrain
1046 if (name1 == "Terrain" || name2 == "Terrain")
1047 {
1048 // If we're moving
1049 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && (movintYN == 1))
1050 {
1051 //$ Av walk/run on terrain (not falling) Use the Avatar movement terrain contact
1052 AvatarMovementTerrainContact.geom = curContact;
1053 _perloopContact.Add(curContact);
1054 if (m_global_contactcount < maxContactsbeforedeath)
1055 {
1056 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1057 m_global_contactcount++;
1058 }
1059 }
1060 else
1061 {
1062 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1063 {
1064 //$ Av standing on terrain, Use the non moving Avata terrain contact
1065 TerrainContact.geom = curContact;
1066 _perloopContact.Add(curContact);
1067 if (m_global_contactcount < maxContactsbeforedeath)
1068 {
1069 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1070 m_global_contactcount++;
1071 }
1072 }
1073 else
1074 {
1075 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1076 {
1077 //& THIS NEVER HAPPENS? prim prim contact In terrain contact?
1078 // int pj294950 = 0;
1079 // prim terrain contact
1080
1081 if (p2 is OdePrim)
1082 material = ((OdePrim)p2).m_material;
1083 //m_log.DebugFormat("Material: {0}", material);
1084 m_materialContacts[material, movintYN].geom = curContact;
1085 _perloopContact.Add(curContact);
1086
1087 if (m_global_contactcount < maxContactsbeforedeath)
1088 {
1089 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1090 m_global_contactcount++;
1091
1092 }
1093
1094 }
1095 else
1096 {
1097 //$ prim on terrain contact
1098 if (p2 is OdePrim)
1099 material = ((OdePrim)p2).m_material;
1100 //m_log.DebugFormat("Material: {0}", material);
1101 m_materialContacts[material, movintYN].geom = curContact;
1102 _perloopContact.Add(curContact);
1103
1104 ContactCopy = m_materialContacts[material, movintYN];
1105 if(movintYN == 1)
1106 {
1107 // prevent excessive slide on terrain
1108 ContactCopy.surface.mu = m_materialContacts[material, movintYN].surface.mu * 30.0f;
1109 }
1110
1111 if (m_global_contactcount < maxContactsbeforedeath)
1112 {
1113 joint = d.JointCreateContact(world, contactgroup, ref ContactCopy);
1114 m_global_contactcount++;
1115 }
1116 }
1117 }
1118 }
1119 }
1120 else if (name1 == "Water" || name2 == "Water")
1121 {
1122 //$ This never happens! Perhaps water is treated like air?
1123 /*
1124 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1125 {
1126 }
1127 else
1128 {
1129 }
1130 */
1131 //WaterContact.surface.soft_cfm = 0.0000f;
1132 //WaterContact.surface.soft_erp = 0.00000f;
1133 if (curContact.depth > 0.1f)
1134 {
1135 curContact.depth *= 52;
1136 //AvatarStaticprimContact.normal = new d.Vector3(0, 0, 1);
1137 //AvatarStaticprimContact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1138 }
1139 WaterContact.geom = curContact;
1140 _perloopContact.Add(curContact);
1141 if (m_global_contactcount < maxContactsbeforedeath)
1142 {
1143 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1144 m_global_contactcount++;
1145 }
1146 //m_log.Info("[PHYSICS]: Prim Water Contact" + AvatarStaticprimContact.depth);
1147 }
1148 else
1149 {
1150
1151 // no terrain and no water, we're colliding with prim or avatar
1152 // check if we're moving
1153 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1154 {
1155 //$ Avatar on Prim or other Avatar
1156 if (movintYN == 1)
1157 {
1158 // Use the AV Movement / prim contact
1159 AvatarMovementprimContact.geom = curContact;
1160 _perloopContact.Add(curContact);
1161 if (m_global_contactcount < maxContactsbeforedeath)
1162 {
1163 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1164 m_global_contactcount++;
1165 }
1166 }
1167 else
1168 {
1169 // Use the Av non movement / prim contact
1170 AvatarStaticprimContact.geom = curContact;
1171 _perloopContact.Add(curContact);
1172 ContactCopy = AvatarStaticprimContact; // local copy so we can change locally
1173
1174 if (m_global_contactcount < maxContactsbeforedeath)
1175 {
1176 if (curContact.depth > 0.2)
1177 { // embedded, eject slowly
1178 ContactCopy.surface.soft_erp = 0.1f;
1179 ContactCopy.surface.soft_cfm = 0.1f;
1180 }
1181 else
1182 { // keep on the surface
1183 ContactCopy.surface.soft_erp = 0.3f;
1184 ContactCopy.surface.soft_cfm = 0.0f;
1185 }
1186 joint = d.JointCreateContact(world, contactgroup, ref ContactCopy);
1187 m_global_contactcount++;
1188 }
1189 }
1190 }
1191 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1192 {
1193 //$ Prim on Prim
1194 //p1.PhysicsActorType
1195
1196 if (p2 is OdePrim) material = ((OdePrim)p2).m_material;
1197 //m_log.DebugFormat("Material: {0}", material);
1198
1199 m_materialContacts[material, movintYN].geom = curContact;
1200 _perloopContact.Add(curContact);
1201
1202 if (m_global_contactcount < maxContactsbeforedeath)
1203 {
1204 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1205 m_global_contactcount++;
1206 }
1207 }
1208 }
1209
1210 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1211 {
1212 d.JointAttach(joint, b1, b2);
1213 m_global_contactcount++;
1214 }
1215
1216 }
1217 collision_accounting_events(p1, p2, maxDepthContact);
1218 if (count > geomContactPointsStartthrottle)
1219 {
1220 // If there are more then 3 contact points, it's likely
1221 // that we've got a pile of objects, so ...
1222 // We don't want to send out hundreds of terse updates over and over again
1223 // so lets throttle them and send them again after it's somewhat sorted out.
1224 p2.ThrottleUpdates = true;
1225 }
1226 //m_log.Debug(count.ToString());
1227 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1228 } // end for i.. loop
1229 } // end near
1230
1231 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1232 {
1233 bool result = false;
1234 //return result;
1235 if (!m_filterCollisions)
1236 return false;
1237
1238 ActorTypes at = (ActorTypes)atype;
1239 lock (_perloopContact)
1240 {
1241 foreach (d.ContactGeom contact in _perloopContact)
1242 {
1243 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1244 //{
1245 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1246 if (at == ActorTypes.Agent)
1247 {
1248 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
1249 {
1250
1251 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1252 {
1253 //contactGeom.depth *= .00005f;
1254 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1255 // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1256 result = true;
1257 break;
1258 }
1259 else
1260 {
1261 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1262 }
1263 }
1264 else
1265 {
1266 //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1267 //int i = 0;
1268 }
1269 }
1270 else if (at == ActorTypes.Prim)
1271 {
1272 //d.AABB aabb1 = new d.AABB();
1273 //d.AABB aabb2 = new d.AABB();
1274
1275 //d.GeomGetAABB(contactGeom.g2, out aabb2);
1276 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1277 //aabb1.
1278 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
1279 {
1280 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1281 {
1282 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1283 {
1284 result = true;
1285 break;
1286 }
1287 }
1288 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1289 //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1290 }
1291
1292 }
1293
1294 //}
1295
1296 }
1297 }
1298 return result;
1299 }
1300
1301 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1302 {
1303 // obj1LocalID = 0;
1304 //returncollisions = false;
1305 obj2LocalID = 0;
1306 //ctype = 0;
1307 //cStartStop = 0;
1308 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1309 return;
1310
1311 switch ((ActorTypes)p2.PhysicsActorType)
1312 {
1313 case ActorTypes.Agent:
1314 cc2 = (OdeCharacter)p2;
1315
1316 // obj1LocalID = cc2.m_localID;
1317 switch ((ActorTypes)p1.PhysicsActorType)
1318 {
1319 case ActorTypes.Agent:
1320 cc1 = (OdeCharacter)p1;
1321 obj2LocalID = cc1.m_localID;
1322 cc1.AddCollisionEvent(cc2.m_localID, contact);
1323 //ctype = (int)CollisionCategories.Character;
1324
1325 //if (cc1.CollidingObj)
1326 //cStartStop = (int)StatusIndicators.Generic;
1327 //else
1328 //cStartStop = (int)StatusIndicators.Start;
1329
1330 //returncollisions = true;
1331 break;
1332 case ActorTypes.Prim:
1333 if (p1 is OdePrim)
1334 {
1335 cp1 = (OdePrim) p1;
1336 obj2LocalID = cp1.m_localID;
1337 cp1.AddCollisionEvent(cc2.m_localID, contact);
1338 }
1339 //ctype = (int)CollisionCategories.Geom;
1340
1341 //if (cp1.CollidingObj)
1342 //cStartStop = (int)StatusIndicators.Generic;
1343 //else
1344 //cStartStop = (int)StatusIndicators.Start;
1345
1346 //returncollisions = true;
1347 break;
1348
1349 case ActorTypes.Ground:
1350 case ActorTypes.Unknown:
1351 obj2LocalID = 0;
1352 //ctype = (int)CollisionCategories.Land;
1353 //returncollisions = true;
1354 break;
1355 }
1356
1357 cc2.AddCollisionEvent(obj2LocalID, contact);
1358 break;
1359 case ActorTypes.Prim:
1360
1361 if (p2 is OdePrim)
1362 {
1363 cp2 = (OdePrim) p2;
1364
1365 // obj1LocalID = cp2.m_localID;
1366 switch ((ActorTypes) p1.PhysicsActorType)
1367 {
1368 case ActorTypes.Agent:
1369 if (p1 is OdeCharacter)
1370 {
1371 cc1 = (OdeCharacter) p1;
1372 obj2LocalID = cc1.m_localID;
1373 cc1.AddCollisionEvent(cp2.m_localID, contact);
1374 //ctype = (int)CollisionCategories.Character;
1375
1376 //if (cc1.CollidingObj)
1377 //cStartStop = (int)StatusIndicators.Generic;
1378 //else
1379 //cStartStop = (int)StatusIndicators.Start;
1380 //returncollisions = true;
1381 }
1382 break;
1383 case ActorTypes.Prim:
1384
1385 if (p1 is OdePrim)
1386 {
1387 cp1 = (OdePrim) p1;
1388 obj2LocalID = cp1.m_localID;
1389 cp1.AddCollisionEvent(cp2.m_localID, contact);
1390 //ctype = (int)CollisionCategories.Geom;
1391
1392 //if (cp1.CollidingObj)
1393 //cStartStop = (int)StatusIndicators.Generic;
1394 //else
1395 //cStartStop = (int)StatusIndicators.Start;
1396
1397 //returncollisions = true;
1398 }
1399 break;
1400
1401 case ActorTypes.Ground:
1402 case ActorTypes.Unknown:
1403 obj2LocalID = 0;
1404 //ctype = (int)CollisionCategories.Land;
1405
1406 //returncollisions = true;
1407 break;
1408 }
1409
1410 cp2.AddCollisionEvent(obj2LocalID, contact);
1411 }
1412 break;
1413 }
1414 //if (returncollisions)
1415 //{
1416
1417 //lock (m_storedCollisions)
1418 //{
1419 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1420 //if (m_storedCollisions.ContainsKey(cDictKey))
1421 //{
1422 //sCollisionData objd = m_storedCollisions[cDictKey];
1423 //objd.NumberOfCollisions += 1;
1424 //objd.lastframe = framecount;
1425 //m_storedCollisions[cDictKey] = objd;
1426 //}
1427 //else
1428 //{
1429 //sCollisionData objd = new sCollisionData();
1430 //objd.ColliderLocalId = obj1LocalID;
1431 //objd.CollidedWithLocalId = obj2LocalID;
1432 //objd.CollisionType = ctype;
1433 //objd.NumberOfCollisions = 1;
1434 //objd.lastframe = framecount;
1435 //objd.StatusIndicator = cStartStop;
1436 //m_storedCollisions.Add(cDictKey, objd);
1437 //}
1438 //}
1439 // }
1440 }
1441
1442 public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1443 {
1444 /* String name1 = null;
1445 String name2 = null;
1446
1447 if (!geom_name_map.TryGetValue(trimesh, out name1))
1448 {
1449 name1 = "null";
1450 }
1451 if (!geom_name_map.TryGetValue(refObject, out name2))
1452 {
1453 name2 = "null";
1454 }
1455
1456 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
1457 */
1458 return 1;
1459 }
1460
1461 public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1462 {
1463 String name1 = null;
1464 String name2 = null;
1465
1466 if (!geom_name_map.TryGetValue(trimesh, out name1))
1467 {
1468 name1 = "null";
1469 }
1470
1471 if (!geom_name_map.TryGetValue(refObject, out name2))
1472 {
1473 name2 = "null";
1474 }
1475
1476 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1477
1478 d.Vector3 v0 = new d.Vector3();
1479 d.Vector3 v1 = new d.Vector3();
1480 d.Vector3 v2 = new d.Vector3();
1481
1482 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1483 // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
1484
1485 return 1;
1486 }
1487
1488 /// <summary>
1489 /// This is our collision testing routine in ODE
1490 /// </summary>
1491 /// <param name="timeStep"></param>
1492 private void collision_optimized(float timeStep)
1493 {
1494 _perloopContact.Clear();
1495
1496 lock (_characters)
1497 {
1498 foreach (OdeCharacter chr in _characters)
1499 {
1500 // Reset the collision values to false
1501 // since we don't know if we're colliding yet
1502
1503 // For some reason this can happen. Don't ask...
1504 //
1505 if (chr == null)
1506 continue;
1507
1508 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1509 continue;
1510
1511 chr.IsColliding = false;
1512 chr.CollidingGround = false;
1513 chr.CollidingObj = false;
1514
1515 // test the avatar's geometry for collision with the space
1516 // This will return near and the space that they are the closest to
1517 // And we'll run this again against the avatar and the space segment
1518 // This will return with a bunch of possible objects in the space segment
1519 // and we'll run it again on all of them.
1520 try
1521 {
1522 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
1523 }
1524 catch (AccessViolationException)
1525 {
1526 m_log.Warn("[PHYSICS]: Unable to space collide");
1527 }
1528 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1529 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1530 //{
1531 //chr.Position.Z = terrainheight + 10.0f;
1532 //forcedZ = true;
1533 //}
1534 }
1535 }
1536
1537 lock (_activeprims)
1538 {
1539 List<OdePrim> removeprims = null;
1540 foreach (OdePrim chr in _activeprims)
1541 {
1542 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled) && !chr.m_outofBounds)
1543 {
1544 try
1545 {
1546 lock (chr)
1547 {
1548 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1549 {
1550 d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
1551 }
1552 else
1553 {
1554 if (removeprims == null)
1555 {
1556 removeprims = new List<OdePrim>();
1557 }
1558 removeprims.Add(chr);
1559 /// Commented this because it triggers on every bullet
1560 //m_log.Debug("[PHYSICS]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!");
1561 }
1562 }
1563 }
1564 catch (AccessViolationException)
1565 {
1566 m_log.Warn("[PHYSICS]: Unable to space collide");
1567 }
1568 }
1569 }
1570 if (removeprims != null)
1571 {
1572 foreach (OdePrim chr in removeprims)
1573 {
1574 _activeprims.Remove(chr);
1575 }
1576 }
1577 }
1578
1579 _perloopContact.Clear();
1580 }
1581
1582 #endregion
1583
1584 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1585 {
1586 m_worldOffset = offset;
1587 WorldExtents = new Vector2(extents.X, extents.Y);
1588 m_parentScene = pScene;
1589
1590 }
1591
1592 // Recovered for use by fly height. Kitto Flora
1593 public float GetTerrainHeightAtXY(float x, float y)
1594 {
1595
1596 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1597 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1598
1599 IntPtr heightFieldGeom = IntPtr.Zero;
1600
1601 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1602 {
1603 if (heightFieldGeom != IntPtr.Zero)
1604 {
1605 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1606 {
1607
1608 int index;
1609
1610
1611 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1612 (int)x < 0.001f || (int)y < 0.001f)
1613 return 0;
1614
1615 x = x - offsetX;
1616 y = y - offsetY;
1617
1618 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1619
1620 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1621 {
1622 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1623 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1624 }
1625
1626 else
1627 return 0f;
1628 }
1629 else
1630 {
1631 return 0f;
1632 }
1633
1634 }
1635 else
1636 {
1637 return 0f;
1638 }
1639
1640 }
1641 else
1642 {
1643 return 0f;
1644 }
1645
1646
1647 }
1648// End recovered. Kitto Flora
1649
1650 public void addCollisionEventReporting(PhysicsActor obj)
1651 {
1652 lock (_collisionEventPrim)
1653 {
1654 if (!_collisionEventPrim.Contains(obj))
1655 _collisionEventPrim.Add(obj);
1656 }
1657 }
1658
1659 public void remCollisionEventReporting(PhysicsActor obj)
1660 {
1661 lock (_collisionEventPrim)
1662 {
1663 if (!_collisionEventPrim.Contains(obj))
1664 _collisionEventPrim.Remove(obj);
1665 }
1666 }
1667
1668 #region Add/Remove Entities
1669
1670 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1671 {
1672 Vector3 pos;
1673 pos.X = position.X;
1674 pos.Y = position.Y;
1675 pos.Z = position.Z;
1676 OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
1677 newAv.Flying = isFlying;
1678 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1679
1680 return newAv;
1681 }
1682
1683 public void AddCharacter(OdeCharacter chr)
1684 {
1685 lock (_characters)
1686 {
1687 if (!_characters.Contains(chr))
1688 {
1689 _characters.Add(chr);
1690 if (chr.bad)
1691 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1692 }
1693 }
1694 }
1695
1696 public void RemoveCharacter(OdeCharacter chr)
1697 {
1698 lock (_characters)
1699 {
1700 if (_characters.Contains(chr))
1701 {
1702 _characters.Remove(chr);
1703 }
1704 }
1705 }
1706 public void BadCharacter(OdeCharacter chr)
1707 {
1708 lock (_badCharacter)
1709 {
1710 if (!_badCharacter.Contains(chr))
1711 _badCharacter.Add(chr);
1712 }
1713 }
1714
1715 public override void RemoveAvatar(PhysicsActor actor)
1716 {
1717 //m_log.Debug("[PHYSICS]:ODELOCK");
1718 ((OdeCharacter) actor).Destroy();
1719
1720 }
1721
1722 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1723 IMesh mesh, PrimitiveBaseShape pbs, bool isphysical, bool isphantom, byte shapetype, uint localid)
1724 {
1725
1726 Vector3 pos = position;
1727 Vector3 siz = size;
1728 Quaternion rot = rotation;
1729
1730 OdePrim newPrim;
1731 lock (OdeLock)
1732 {
1733 newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, isphantom, shapetype, ode, localid);
1734
1735 lock (_prims)
1736 _prims.Add(newPrim);
1737 }
1738
1739 return newPrim;
1740 }
1741
1742
1743 public void addActivePrim(OdePrim activatePrim)
1744 {
1745 // adds active prim.. (ones that should be iterated over in collisions_optimized
1746 lock (_activeprims)
1747 {
1748 if (!_activeprims.Contains(activatePrim))
1749 _activeprims.Add(activatePrim);
1750 //else
1751 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1752 }
1753 }
1754
1755 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1756 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1757 {
1758 PhysicsActor result;
1759 IMesh mesh = null;
1760
1761 if (needsMeshing(pbs))
1762 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1763
1764 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical,false,0, localid);
1765
1766 return result;
1767 }
1768
1769 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1770 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1771 {
1772 PhysicsActor result;
1773 IMesh mesh = null;
1774
1775 if (needsMeshing(pbs))
1776 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1777
1778 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom,0, localid);
1779
1780 return result;
1781 }
1782
1783 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1784 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
1785 {
1786 PhysicsActor result;
1787 IMesh mesh = null;
1788
1789 if (needsMeshing(pbs))
1790 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1791
1792 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, isPhantom, shapetype, localid);
1793
1794 return result;
1795 }
1796
1797 public override float TimeDilation
1798 {
1799 get { return m_timeDilation; }
1800 }
1801
1802 public override bool SupportsNINJAJoints
1803 {
1804 get { return m_NINJA_physics_joints_enabled; }
1805 }
1806
1807 // internal utility function: must be called within a lock (OdeLock)
1808 private void InternalAddActiveJoint(PhysicsJoint joint)
1809 {
1810 activeJoints.Add(joint);
1811 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1812 }
1813
1814 // internal utility function: must be called within a lock (OdeLock)
1815 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1816 {
1817 pendingJoints.Add(joint);
1818 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1819 }
1820
1821 // internal utility function: must be called within a lock (OdeLock)
1822 private void InternalRemovePendingJoint(PhysicsJoint joint)
1823 {
1824 pendingJoints.Remove(joint);
1825 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1826 }
1827
1828 // internal utility function: must be called within a lock (OdeLock)
1829 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1830 {
1831 activeJoints.Remove(joint);
1832 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1833 }
1834
1835 public override void DumpJointInfo()
1836 {
1837 string hdr = "[NINJA] JOINTINFO: ";
1838 foreach (PhysicsJoint j in pendingJoints)
1839 {
1840 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1841 }
1842 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1843 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1844 {
1845 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1846 }
1847 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1848 foreach (PhysicsJoint j in activeJoints)
1849 {
1850 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1851 }
1852 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1853 foreach (string jointName in SOPName_to_activeJoint.Keys)
1854 {
1855 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1856 }
1857 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1858
1859 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1860 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1861 foreach (string actorName in joints_connecting_actor.Keys)
1862 {
1863 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1864 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1865 {
1866 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1867 }
1868 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1869 }
1870 }
1871
1872 public override void RequestJointDeletion(string ObjectNameInScene)
1873 {
1874 lock (externalJointRequestsLock)
1875 {
1876 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1877 {
1878 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1879 }
1880 }
1881 }
1882
1883 private void DeleteRequestedJoints()
1884 {
1885 List<string> myRequestedJointsToBeDeleted;
1886 lock (externalJointRequestsLock)
1887 {
1888 // make a local copy of the shared list for processing (threading issues)
1889 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1890 }
1891
1892 foreach (string jointName in myRequestedJointsToBeDeleted)
1893 {
1894 lock (OdeLock)
1895 {
1896 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1897 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1898 {
1899 OdePhysicsJoint joint = null;
1900 if (SOPName_to_activeJoint.ContainsKey(jointName))
1901 {
1902 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1903 InternalRemoveActiveJoint(joint);
1904 }
1905 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1906 {
1907 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1908 InternalRemovePendingJoint(joint);
1909 }
1910
1911 if (joint != null)
1912 {
1913 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1914 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1915 {
1916 string bodyName = joint.BodyNames[iBodyName];
1917 if (bodyName != "NULL")
1918 {
1919 joints_connecting_actor[bodyName].Remove(joint);
1920 if (joints_connecting_actor[bodyName].Count == 0)
1921 {
1922 joints_connecting_actor.Remove(bodyName);
1923 }
1924 }
1925 }
1926
1927 DoJointDeactivated(joint);
1928 if (joint.jointID != IntPtr.Zero)
1929 {
1930 d.JointDestroy(joint.jointID);
1931 joint.jointID = IntPtr.Zero;
1932 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1933 }
1934 else
1935 {
1936 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1937 }
1938 }
1939 else
1940 {
1941 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1942 }
1943 }
1944 else
1945 {
1946 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1947 }
1948 }
1949 }
1950
1951 // remove processed joints from the shared list
1952 lock (externalJointRequestsLock)
1953 {
1954 foreach (string jointName in myRequestedJointsToBeDeleted)
1955 {
1956 requestedJointsToBeDeleted.Remove(jointName);
1957 }
1958 }
1959 }
1960
1961 // for pending joints we don't know if their associated bodies exist yet or not.
1962 // the joint is actually created during processing of the taints
1963 private void CreateRequestedJoints()
1964 {
1965 List<PhysicsJoint> myRequestedJointsToBeCreated;
1966 lock (externalJointRequestsLock)
1967 {
1968 // make a local copy of the shared list for processing (threading issues)
1969 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1970 }
1971
1972 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1973 {
1974 lock (OdeLock)
1975 {
1976 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
1977 {
1978 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
1979 continue;
1980 }
1981 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
1982 {
1983 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
1984 continue;
1985 }
1986
1987 InternalAddPendingJoint(joint as OdePhysicsJoint);
1988
1989 if (joint.BodyNames.Count >= 2)
1990 {
1991 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1992 {
1993 string bodyName = joint.BodyNames[iBodyName];
1994 if (bodyName != "NULL")
1995 {
1996 if (!joints_connecting_actor.ContainsKey(bodyName))
1997 {
1998 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
1999 }
2000 joints_connecting_actor[bodyName].Add(joint);
2001 }
2002 }
2003 }
2004 }
2005 }
2006
2007 // remove processed joints from shared list
2008 lock (externalJointRequestsLock)
2009 {
2010 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2011 {
2012 requestedJointsToBeCreated.Remove(joint);
2013 }
2014 }
2015
2016 }
2017
2018 // public function to add an request for joint creation
2019 // this joint will just be added to a waiting list that is NOT processed during the main
2020 // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2021
2022 public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2023 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2024
2025 {
2026
2027 OdePhysicsJoint joint = new OdePhysicsJoint();
2028 joint.ObjectNameInScene = objectNameInScene;
2029 joint.Type = jointType;
2030 joint.Position = position;
2031 joint.Rotation = rotation;
2032 joint.RawParams = parms;
2033 joint.BodyNames = new List<string>(bodyNames);
2034 joint.TrackedBodyName = trackedBodyName;
2035 joint.LocalRotation = localRotation;
2036 joint.jointID = IntPtr.Zero;
2037 joint.ErrorMessageCount = 0;
2038
2039 lock (externalJointRequestsLock)
2040 {
2041 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2042 {
2043 requestedJointsToBeCreated.Add(joint);
2044 }
2045 }
2046 return joint;
2047 }
2048
2049 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2050 {
2051 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2052 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2053 {
2054
2055 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2056 //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
2057 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2058 {
2059 jointsToRemove.Add(j);
2060 }
2061 foreach (PhysicsJoint j in jointsToRemove)
2062 {
2063 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2064 RequestJointDeletion(j.ObjectNameInScene);
2065 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2066 j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
2067 }
2068 }
2069 }
2070
2071 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2072 {
2073 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2074 lock (OdeLock)
2075 {
2076 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2077 RemoveAllJointsConnectedToActor(actor);
2078 }
2079 }
2080
2081 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2082 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2083 {
2084 Debug.Assert(joint.IsInPhysicsEngine);
2085 d.Vector3 pos = new d.Vector3();
2086
2087 if (!(joint is OdePhysicsJoint))
2088 {
2089 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2090 }
2091 else
2092 {
2093 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2094 switch (odeJoint.Type)
2095 {
2096 case PhysicsJointType.Ball:
2097 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2098 break;
2099 case PhysicsJointType.Hinge:
2100 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2101 break;
2102 }
2103 }
2104 return new Vector3(pos.X, pos.Y, pos.Z);
2105 }
2106
2107 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2108 // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2109 // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2110 // keeping track of the joint's original orientation relative to one of the involved bodies.
2111 public override Vector3 GetJointAxis(PhysicsJoint joint)
2112 {
2113 Debug.Assert(joint.IsInPhysicsEngine);
2114 d.Vector3 axis = new d.Vector3();
2115
2116 if (!(joint is OdePhysicsJoint))
2117 {
2118 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2119 }
2120 else
2121 {
2122 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2123 switch (odeJoint.Type)
2124 {
2125 case PhysicsJointType.Ball:
2126 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2127 break;
2128 case PhysicsJointType.Hinge:
2129 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2130 break;
2131 }
2132 }
2133 return new Vector3(axis.X, axis.Y, axis.Z);
2134 }
2135
2136
2137 public void remActivePrim(OdePrim deactivatePrim)
2138 {
2139 lock (_activeprims)
2140 {
2141 _activeprims.Remove(deactivatePrim);
2142 }
2143 }
2144
2145 public override void RemovePrim(PhysicsActor prim)
2146 {
2147 if (prim is OdePrim)
2148 {
2149 lock (OdeLock)
2150 {
2151 OdePrim p = (OdePrim) prim;
2152
2153 p.setPrimForRemoval();
2154 AddPhysicsActorTaint(prim);
2155 //RemovePrimThreadLocked(p);
2156 }
2157 }
2158 }
2159
2160 /// <summary>
2161 /// This is called from within simulate but outside the locked portion
2162 /// We need to do our own locking here
2163 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2164 ///
2165 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2166 /// that the space was using.
2167 /// </summary>
2168 /// <param name="prim"></param>
2169 public void RemovePrimThreadLocked(OdePrim prim)
2170 {
2171//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2172 lock (prim)
2173 {
2174 remCollisionEventReporting(prim);
2175 lock (ode)
2176 {
2177 if (prim.prim_geom != IntPtr.Zero)
2178 {
2179 prim.ResetTaints();
2180
2181 try
2182 {
2183 if (prim._triMeshData != IntPtr.Zero)
2184 {
2185 d.GeomTriMeshDataDestroy(prim._triMeshData);
2186 prim._triMeshData = IntPtr.Zero;
2187 }
2188 }
2189 catch { };
2190
2191 if (prim.IsPhysical)
2192 {
2193 prim.disableBody();
2194 if (prim.childPrim)
2195 {
2196 prim.childPrim = false;
2197 prim.Body = IntPtr.Zero;
2198 prim.m_disabled = true;
2199 prim.IsPhysical = false;
2200 }
2201
2202 }
2203 // we don't want to remove the main space
2204
2205 // If the geometry is in the targetspace, remove it from the target space
2206 //m_log.Warn(prim.m_targetSpace);
2207
2208 //if (prim.m_targetSpace != IntPtr.Zero)
2209 //{
2210 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2211 //{
2212
2213 //if (d.GeomIsSpace(prim.m_targetSpace))
2214 //{
2215 //waitForSpaceUnlock(prim.m_targetSpace);
2216 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2217 prim.m_targetSpace = IntPtr.Zero;
2218 //}
2219 //else
2220 //{
2221 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2222 //((OdePrim)prim).m_targetSpace.ToString());
2223 //}
2224
2225 //}
2226 //}
2227 //m_log.Warn(prim.prim_geom);
2228 try
2229 {
2230 if (prim.prim_geom != IntPtr.Zero)
2231 {
2232
2233//string tPA;
2234//geom_name_map.TryGetValue(prim.prim_geom, out tPA);
2235//Console.WriteLine("**** Remove {0}", tPA);
2236 if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom);
2237 if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom);
2238 d.GeomDestroy(prim.prim_geom);
2239 prim.prim_geom = IntPtr.Zero;
2240 }
2241 else
2242 {
2243 m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
2244 }
2245 }
2246 catch (AccessViolationException)
2247 {
2248 m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
2249 }
2250 lock (_prims)
2251 _prims.Remove(prim);
2252
2253 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2254 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2255 //{
2256 //if (prim.m_targetSpace != null)
2257 //{
2258 //if (d.GeomIsSpace(prim.m_targetSpace))
2259 //{
2260 //waitForSpaceUnlock(prim.m_targetSpace);
2261 //d.SpaceRemove(space, prim.m_targetSpace);
2262 // free up memory used by the space.
2263 //d.SpaceDestroy(prim.m_targetSpace);
2264 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2265 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2266 //}
2267 //else
2268 //{
2269 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2270 //((OdePrim) prim).m_targetSpace.ToString());
2271 //}
2272 //}
2273 //}
2274
2275 if (SupportsNINJAJoints)
2276 {
2277 RemoveAllJointsConnectedToActorThreadLocked(prim);
2278 }
2279 }
2280 }
2281 }
2282 }
2283
2284 #endregion
2285
2286 #region Space Separation Calculation
2287
2288 /// <summary>
2289 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2290 /// </summary>
2291 /// <param name="pSpace"></param>
2292 public void resetSpaceArrayItemToZero(IntPtr pSpace)
2293 {
2294 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2295 {
2296 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2297 {
2298 if (staticPrimspace[x, y] == pSpace)
2299 staticPrimspace[x, y] = IntPtr.Zero;
2300 }
2301 }
2302 }
2303
2304 public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2305 {
2306 staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2307 }
2308
2309 /// <summary>
2310 /// Called when a static prim moves. Allocates a space for the prim based on its position
2311 /// </summary>
2312 /// <param name="geom">the pointer to the geom that moved</param>
2313 /// <param name="pos">the position that the geom moved to</param>
2314 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2315 /// <returns>a pointer to the new space it's in</returns>
2316 public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2317 {
2318 // Called from setting the Position and Size of an ODEPrim so
2319 // it's already in locked space.
2320
2321 // we don't want to remove the main space
2322 // we don't need to test physical here because this function should
2323 // never be called if the prim is physical(active)
2324
2325 // All physical prim end up in the root space
2326 //Thread.Sleep(20);
2327 if (currentspace != space)
2328 {
2329 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2330 //if (currentspace == IntPtr.Zero)
2331 //{
2332 //int adfadf = 0;
2333 //}
2334 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2335 {
2336 if (d.GeomIsSpace(currentspace))
2337 {
2338 waitForSpaceUnlock(currentspace);
2339 d.SpaceRemove(currentspace, geom);
2340 }
2341 else
2342 {
2343 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2344 " Geom:" + geom);
2345 }
2346 }
2347 else
2348 {
2349 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2350 if (sGeomIsIn != IntPtr.Zero)
2351 {
2352 if (d.GeomIsSpace(currentspace))
2353 {
2354 waitForSpaceUnlock(sGeomIsIn);
2355 d.SpaceRemove(sGeomIsIn, geom);
2356 }
2357 else
2358 {
2359 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2360 sGeomIsIn + " Geom:" + geom);
2361 }
2362 }
2363 }
2364
2365 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2366 if (d.SpaceGetNumGeoms(currentspace) == 0)
2367 {
2368 if (currentspace != IntPtr.Zero)
2369 {
2370 if (d.GeomIsSpace(currentspace))
2371 {
2372 waitForSpaceUnlock(currentspace);
2373 waitForSpaceUnlock(space);
2374 d.SpaceRemove(space, currentspace);
2375 // free up memory used by the space.
2376
2377 //d.SpaceDestroy(currentspace);
2378 resetSpaceArrayItemToZero(currentspace);
2379 }
2380 else
2381 {
2382 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2383 currentspace + " Geom:" + geom);
2384 }
2385 }
2386 }
2387 }
2388 else
2389 {
2390 // this is a physical object that got disabled. ;.;
2391 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2392 {
2393 if (d.SpaceQuery(currentspace, geom))
2394 {
2395 if (d.GeomIsSpace(currentspace))
2396 {
2397 waitForSpaceUnlock(currentspace);
2398 d.SpaceRemove(currentspace, geom);
2399 }
2400 else
2401 {
2402 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2403 currentspace + " Geom:" + geom);
2404 }
2405 }
2406 else
2407 {
2408 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2409 if (sGeomIsIn != IntPtr.Zero)
2410 {
2411 if (d.GeomIsSpace(sGeomIsIn))
2412 {
2413 waitForSpaceUnlock(sGeomIsIn);
2414 d.SpaceRemove(sGeomIsIn, geom);
2415 }
2416 else
2417 {
2418 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2419 sGeomIsIn + " Geom:" + geom);
2420 }
2421 }
2422 }
2423 }
2424 }
2425
2426 // The routines in the Position and Size sections do the 'inserting' into the space,
2427 // so all we have to do is make sure that the space that we're putting the prim into
2428 // is in the 'main' space.
2429 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2430 IntPtr newspace = calculateSpaceForGeom(pos);
2431
2432 if (newspace == IntPtr.Zero)
2433 {
2434 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2435 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2436 }
2437
2438 return newspace;
2439 }
2440
2441 /// <summary>
2442 /// Creates a new space at X Y
2443 /// </summary>
2444 /// <param name="iprimspaceArrItemX"></param>
2445 /// <param name="iprimspaceArrItemY"></param>
2446 /// <returns>A pointer to the created space</returns>
2447 public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2448 {
2449 // creating a new space for prim and inserting it into main space.
2450 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2451 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2452 waitForSpaceUnlock(space);
2453 d.SpaceSetSublevel(space, 1);
2454 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2455 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2456 }
2457
2458 /// <summary>
2459 /// Calculates the space the prim should be in by its position
2460 /// </summary>
2461 /// <param name="pos"></param>
2462 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2463 public IntPtr calculateSpaceForGeom(Vector3 pos)
2464 {
2465 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2466 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2467 return staticPrimspace[xyspace[0], xyspace[1]];
2468 }
2469
2470 /// <summary>
2471 /// Holds the space allocation logic
2472 /// </summary>
2473 /// <param name="pos"></param>
2474 /// <returns>an array item based on the position</returns>
2475 public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2476 {
2477 int[] returnint = new int[2];
2478
2479 returnint[0] = (int) (pos.X/metersInSpace);
2480
2481 if (returnint[0] > ((int) (259f/metersInSpace)))
2482 returnint[0] = ((int) (259f/metersInSpace));
2483 if (returnint[0] < 0)
2484 returnint[0] = 0;
2485
2486 returnint[1] = (int) (pos.Y/metersInSpace);
2487 if (returnint[1] > ((int) (259f/metersInSpace)))
2488 returnint[1] = ((int) (259f/metersInSpace));
2489 if (returnint[1] < 0)
2490 returnint[1] = 0;
2491
2492 return returnint;
2493 }
2494
2495 #endregion
2496
2497 /// <summary>
2498 /// Routine to figure out if we need to mesh this prim with our mesher
2499 /// </summary>
2500 /// <param name="pbs"></param>
2501 /// <returns></returns>
2502 public bool needsMeshing(PrimitiveBaseShape pbs)
2503 {
2504 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2505 // but we still need to check for sculptie meshing being enabled so this is the most
2506 // convenient place to do it for now...
2507
2508 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2509 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2510 int iPropertiesNotSupportedDefault = 0;
2511
2512 if (pbs.SculptEntry && !meshSculptedPrim)
2513 {
2514#if SPAM
2515 m_log.Warn("NonMesh");
2516#endif
2517 return false;
2518 }
2519
2520 // 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
2521 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2522 {
2523 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2524 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2525 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2526 {
2527
2528 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2529 && pbs.ProfileHollow == 0
2530 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2531 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2532 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2533 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2534 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2535 {
2536#if SPAM
2537 m_log.Warn("NonMesh");
2538#endif
2539 return false;
2540 }
2541 }
2542 }
2543
2544 if (forceSimplePrimMeshing)
2545 return true;
2546
2547 if (pbs.ProfileHollow != 0)
2548 iPropertiesNotSupportedDefault++;
2549
2550 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2551 iPropertiesNotSupportedDefault++;
2552
2553 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2554 iPropertiesNotSupportedDefault++;
2555
2556 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2557 iPropertiesNotSupportedDefault++;
2558
2559 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2560 iPropertiesNotSupportedDefault++;
2561
2562 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2563 iPropertiesNotSupportedDefault++;
2564
2565 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))
2566 iPropertiesNotSupportedDefault++;
2567
2568 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2569 iPropertiesNotSupportedDefault++;
2570
2571 // test for torus
2572 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2573 {
2574 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2575 {
2576 iPropertiesNotSupportedDefault++;
2577 }
2578 }
2579 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2580 {
2581 if (pbs.PathCurve == (byte)Extrusion.Straight)
2582 {
2583 iPropertiesNotSupportedDefault++;
2584 }
2585
2586 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2587 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2588 {
2589 iPropertiesNotSupportedDefault++;
2590 }
2591 }
2592 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2593 {
2594 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2595 {
2596 iPropertiesNotSupportedDefault++;
2597 }
2598 }
2599 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2600 {
2601 if (pbs.PathCurve == (byte)Extrusion.Straight)
2602 {
2603 iPropertiesNotSupportedDefault++;
2604 }
2605 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2606 {
2607 iPropertiesNotSupportedDefault++;
2608 }
2609 }
2610
2611 if (pbs.SculptEntry && meshSculptedPrim)
2612 iPropertiesNotSupportedDefault++;
2613
2614 if (iPropertiesNotSupportedDefault == 0)
2615 {
2616#if SPAM
2617 m_log.Warn("NonMesh");
2618#endif
2619 return false;
2620 }
2621#if SPAM
2622 m_log.Debug("Mesh");
2623#endif
2624 return true;
2625 }
2626
2627 /// <summary>
2628 /// Called after our prim properties are set Scale, position etc.
2629 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2630 /// This assures us that we have no race conditions
2631 /// </summary>
2632 /// <param name="prim"></param>
2633 public override void AddPhysicsActorTaint(PhysicsActor prim)
2634 {
2635
2636 if (prim is OdePrim)
2637 {
2638 OdePrim taintedprim = ((OdePrim) prim);
2639 lock (_taintedPrimLock)
2640 {
2641 if (!(_taintedPrimH.Contains(taintedprim)))
2642 {
2643//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2644 _taintedPrimH.Add(taintedprim); // HashSet for searching
2645 _taintedPrimL.Add(taintedprim); // List for ordered readout
2646 }
2647 }
2648 return;
2649 }
2650 else if (prim is OdeCharacter)
2651 {
2652 OdeCharacter taintedchar = ((OdeCharacter)prim);
2653 lock (_taintedActors)
2654 {
2655 if (!(_taintedActors.Contains(taintedchar)))
2656 {
2657 _taintedActors.Add(taintedchar);
2658 if (taintedchar.bad)
2659 m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2660 }
2661 }
2662 }
2663 }
2664
2665 /// <summary>
2666 /// This is our main simulate loop
2667 /// It's thread locked by a Mutex in the scene.
2668 /// It holds Collisions, it instructs ODE to step through the physical reactions
2669 /// It moves the objects around in memory
2670 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2671 /// </summary>
2672 /// <param name="timeStep"></param>
2673 /// <returns></returns>
2674 public override float Simulate(float timeStep)
2675 {
2676 if (framecount >= int.MaxValue)
2677 framecount = 0;
2678 //if (m_worldOffset != Vector3.Zero)
2679 // return 0;
2680
2681 framecount++;
2682
2683 DateTime now = DateTime.UtcNow;
2684 TimeSpan SinceLastFrame = now - m_lastframe;
2685 m_lastframe = now;
2686 float realtime = (float)SinceLastFrame.TotalSeconds;
2687// Console.WriteLine("ts={0} rt={1}", timeStep, realtime);
2688 timeStep = realtime;
2689
2690 // float fps = 1.0f / realtime;
2691 float fps = 0.0f; // number of ODE steps in this Simulate step
2692 //m_log.Info(timeStep.ToString());
2693 step_time += timeStep;
2694
2695 // If We're loaded down by something else,
2696 // or debugging with the Visual Studio project on pause
2697 // skip a few frames to catch up gracefully.
2698 // without shooting the physicsactors all over the place
2699
2700 if (step_time >= m_SkipFramesAtms)
2701 {
2702 // Instead of trying to catch up, it'll do 5 physics frames only
2703 step_time = ODE_STEPSIZE;
2704 m_physicsiterations = 5;
2705 }
2706 else
2707 {
2708 m_physicsiterations = 10;
2709 }
2710
2711 if (SupportsNINJAJoints)
2712 {
2713 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2714 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2715 }
2716
2717 lock (OdeLock)
2718 {
2719 // Process 10 frames if the sim is running normal..
2720 // process 5 frames if the sim is running slow
2721 //try
2722 //{
2723 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2724 //}
2725 //catch (StackOverflowException)
2726 //{
2727 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2728 // ode.drelease(world);
2729 //base.TriggerPhysicsBasedRestart();
2730 //}
2731
2732 int i = 0;
2733
2734 // Figure out the Frames Per Second we're going at.
2735 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
2736
2737 // fps = (step_time / ODE_STEPSIZE) * 1000;
2738 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
2739 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
2740
2741 // step_time = 0.09375f;
2742
2743 while (step_time > 0.0f)
2744 {
2745 //lock (ode)
2746 //{
2747 //if (!ode.lockquery())
2748 //{
2749 // ode.dlock(world);
2750 try
2751 {
2752 // Insert, remove Characters
2753 bool processedtaints = false;
2754
2755 lock (_taintedActors)
2756 {
2757 if (_taintedActors.Count > 0)
2758 {
2759 foreach (OdeCharacter character in _taintedActors)
2760 {
2761
2762 character.ProcessTaints(ODE_STEPSIZE);
2763
2764 processedtaints = true;
2765 //character.m_collisionscore = 0;
2766 }
2767
2768 if (processedtaints)
2769 _taintedActors.Clear();
2770 }
2771 } // end lock _taintedActors
2772
2773 // Modify other objects in the scene.
2774 processedtaints = false;
2775
2776 lock (_taintedPrimLock)
2777 {
2778 foreach (OdePrim prim in _taintedPrimL)
2779 {
2780 if (prim.m_taintremove)
2781 {
2782 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2783 RemovePrimThreadLocked(prim);
2784 }
2785 else
2786 {
2787 //Console.WriteLine("Simulate calls ProcessTaints");
2788 prim.ProcessTaints(ODE_STEPSIZE);
2789 }
2790 processedtaints = true;
2791 prim.m_collisionscore = 0;
2792
2793 // This loop can block up the Heartbeat for a very long time on large regions.
2794 // We need to let the Watchdog know that the Heartbeat is not dead
2795 // NOTE: This is currently commented out, but if things like OAR loading are
2796 // timing the heartbeat out we will need to uncomment it
2797 //Watchdog.UpdateThread();
2798 }
2799
2800 if (SupportsNINJAJoints)
2801 {
2802 // Create pending joints, if possible
2803
2804 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2805 // a joint requires specifying the body id of both involved bodies
2806 if (pendingJoints.Count > 0)
2807 {
2808 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2809 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2810 foreach (PhysicsJoint joint in pendingJoints)
2811 {
2812 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2813 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(),
2814 System.StringSplitOptions.RemoveEmptyEntries);
2815 List<IntPtr> jointBodies = new List<IntPtr>();
2816 bool allJointBodiesAreReady = true;
2817 foreach (string jointParam in jointParams)
2818 {
2819 if (jointParam == "NULL")
2820 {
2821 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2822 jointBodies.Add(IntPtr.Zero);
2823 }
2824 else
2825 {
2826 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2827 bool foundPrim = false;
2828 lock (_prims)
2829 {
2830 foreach (OdePrim prim in _prims) // FIXME: inefficient
2831 {
2832 if (prim.SOPName == jointParam)
2833 {
2834 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2835 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2836 {
2837 jointBodies.Add(prim.Body);
2838 foundPrim = true;
2839 break;
2840 }
2841 else
2842 {
2843 DoJointErrorMessage(joint, "prim name " + jointParam +
2844 " exists but is not (yet) physical; deferring joint creation. " +
2845 "IsPhysical property is " + prim.IsPhysical +
2846 " and body is " + prim.Body);
2847 foundPrim = false;
2848 break;
2849 }
2850 }
2851 }
2852 }
2853 if (foundPrim)
2854 {
2855 // all is fine
2856 }
2857 else
2858 {
2859 allJointBodiesAreReady = false;
2860 break;
2861 }
2862 }
2863 }
2864 if (allJointBodiesAreReady)
2865 {
2866 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2867 if (jointBodies[0] == jointBodies[1])
2868 {
2869 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2870 }
2871 else
2872 {
2873 switch (joint.Type)
2874 {
2875 case PhysicsJointType.Ball:
2876 {
2877 IntPtr odeJoint;
2878 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2879 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2880 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2881 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2882 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2883 d.JointSetBallAnchor(odeJoint,
2884 joint.Position.X,
2885 joint.Position.Y,
2886 joint.Position.Z);
2887 //DoJointErrorMessage(joint, "ODE joint setting OK");
2888 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2889 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2890 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2891 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2892
2893 if (joint is OdePhysicsJoint)
2894 {
2895 ((OdePhysicsJoint)joint).jointID = odeJoint;
2896 }
2897 else
2898 {
2899 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2900 }
2901 }
2902 break;
2903 case PhysicsJointType.Hinge:
2904 {
2905 IntPtr odeJoint;
2906 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2907 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2908 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2909 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2910 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2911 d.JointSetHingeAnchor(odeJoint,
2912 joint.Position.X,
2913 joint.Position.Y,
2914 joint.Position.Z);
2915 // We use the orientation of the x-axis of the joint's coordinate frame
2916 // as the axis for the hinge.
2917
2918 // Therefore, we must get the joint's coordinate frame based on the
2919 // joint.Rotation field, which originates from the orientation of the
2920 // joint's proxy object in the scene.
2921
2922 // The joint's coordinate frame is defined as the transformation matrix
2923 // that converts a vector from joint-local coordinates into world coordinates.
2924 // World coordinates are defined as the XYZ coordinate system of the sim,
2925 // as shown in the top status-bar of the viewer.
2926
2927 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2928 // and use that as the hinge axis.
2929
2930 //joint.Rotation.Normalize();
2931 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2932
2933 // Now extract the X axis of the joint's coordinate frame.
2934
2935 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2936 // tar pit of transposed, inverted, and generally messed-up orientations.
2937 // (In other words, Matrix4.AtAxis() is borked.)
2938 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2939
2940 // Instead, compute the X axis of the coordinate frame by transforming
2941 // the (1,0,0) vector. At least that works.
2942
2943 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2944 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2945 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2946 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2947 d.JointSetHingeAxis(odeJoint,
2948 jointAxis.X,
2949 jointAxis.Y,
2950 jointAxis.Z);
2951 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2952 if (joint is OdePhysicsJoint)
2953 {
2954 ((OdePhysicsJoint)joint).jointID = odeJoint;
2955 }
2956 else
2957 {
2958 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2959 }
2960 }
2961 break;
2962 }
2963 successfullyProcessedPendingJoints.Add(joint);
2964 }
2965 }
2966 else
2967 {
2968 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2969 }
2970 }
2971 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2972 {
2973 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2974 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2975 InternalRemovePendingJoint(successfullyProcessedJoint);
2976 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2977 InternalAddActiveJoint(successfullyProcessedJoint);
2978 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2979 }
2980 }
2981 } // end SupportsNINJAJoints
2982
2983 if (processedtaints)
2984//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2985 _taintedPrimH.Clear(); // ??? if this only ???
2986 _taintedPrimL.Clear();
2987 } // end lock _taintedPrimLock
2988
2989 // Move characters
2990 lock (_characters)
2991 {
2992 List<OdeCharacter> defects = new List<OdeCharacter>();
2993 foreach (OdeCharacter actor in _characters)
2994 {
2995 if (actor != null)
2996 actor.Move(ODE_STEPSIZE, defects);
2997 }
2998 if (0 != defects.Count)
2999 {
3000 foreach (OdeCharacter defect in defects)
3001 {
3002 RemoveCharacter(defect);
3003 }
3004 }
3005 } // end lock _characters
3006
3007 // Move other active objects
3008 lock (_activeprims)
3009 {
3010 foreach (OdePrim prim in _activeprims)
3011 {
3012 prim.m_collisionscore = 0;
3013 prim.Move(ODE_STEPSIZE);
3014 }
3015 } // end lock _activeprims
3016
3017 //if ((framecount % m_randomizeWater) == 0)
3018 // randomizeWater(waterlevel);
3019
3020 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
3021 m_rayCastManager.ProcessQueuedRequests();
3022
3023 collision_optimized(ODE_STEPSIZE);
3024
3025 lock (_collisionEventPrim)
3026 {
3027 foreach (PhysicsActor obj in _collisionEventPrim)
3028 {
3029 if (obj == null)
3030 continue;
3031
3032 switch ((ActorTypes)obj.PhysicsActorType)
3033 {
3034 case ActorTypes.Agent:
3035 OdeCharacter cobj = (OdeCharacter)obj;
3036 cobj.AddCollisionFrameTime(100);
3037 cobj.SendCollisions();
3038 break;
3039 case ActorTypes.Prim:
3040 OdePrim pobj = (OdePrim)obj;
3041 pobj.SendCollisions();
3042 break;
3043 }
3044 }
3045 } // end lock _collisionEventPrim
3046
3047 //if (m_global_contactcount > 5)
3048 //{
3049 // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
3050 //}
3051
3052 m_global_contactcount = 0;
3053
3054 d.WorldQuickStep(world, ODE_STEPSIZE);
3055 d.JointGroupEmpty(contactgroup);
3056 fps++;
3057 //ode.dunlock(world);
3058 } // end try
3059 catch (Exception e)
3060 {
3061 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3062 ode.dunlock(world);
3063 }
3064
3065 step_time -= ODE_STEPSIZE;
3066 i++;
3067 //}
3068 //else
3069 //{
3070 //fps = 0;
3071 //}
3072 //}
3073 } // end while (step_time > 0.0f)
3074
3075 lock (_characters)
3076 {
3077 foreach (OdeCharacter actor in _characters)
3078 {
3079 if (actor != null)
3080 {
3081 if (actor.bad)
3082 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3083 actor.UpdatePositionAndVelocity();
3084 }
3085 }
3086 }
3087
3088 lock (_badCharacter)
3089 {
3090 if (_badCharacter.Count > 0)
3091 {
3092 foreach (OdeCharacter chr in _badCharacter)
3093 {
3094 RemoveCharacter(chr);
3095 }
3096 _badCharacter.Clear();
3097 }
3098 }
3099
3100 lock (_activeprims)
3101 {
3102 //if (timeStep < 0.2f)
3103 {
3104 foreach (OdePrim actor in _activeprims)
3105 {
3106 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
3107 {
3108 actor.UpdatePositionAndVelocity();
3109
3110 if (SupportsNINJAJoints)
3111 {
3112 // If an actor moved, move its joint proxy objects as well.
3113 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3114 // for this purpose but it is never called! So we just do the joint
3115 // movement code here.
3116
3117 if (actor.SOPName != null &&
3118 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3119 joints_connecting_actor[actor.SOPName] != null &&
3120 joints_connecting_actor[actor.SOPName].Count > 0)
3121 {
3122 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3123 {
3124 if (affectedJoint.IsInPhysicsEngine)
3125 {
3126 DoJointMoved(affectedJoint);
3127 }
3128 else
3129 {
3130 DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
3131 }
3132 }
3133 }
3134 }
3135 }
3136 }
3137 }
3138 } // end lock _activeprims
3139
3140 //DumpJointInfo();
3141
3142 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3143 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3144 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3145 if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
3146 {
3147 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3148 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3149
3150 if (physics_logging_append_existing_logfile)
3151 {
3152 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3153 TextWriter fwriter = File.AppendText(fname);
3154 fwriter.WriteLine(header);
3155 fwriter.Close();
3156 }
3157 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3158 }
3159 } // end lock OdeLock
3160
3161 return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
3162 } // end Simulate
3163
3164 public override void GetResults()
3165 {
3166 }
3167
3168 public override bool IsThreaded
3169 {
3170 // for now we won't be multithreaded
3171 get { return (false); }
3172 }
3173
3174 #region ODE Specific Terrain Fixes
3175 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3176 {
3177 float[] returnarr = new float[262144];
3178 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3179
3180 // Filling out the array into its multi-dimensional components
3181 for (int y = 0; y < WorldExtents.Y; y++)
3182 {
3183 for (int x = 0; x < WorldExtents.X; x++)
3184 {
3185 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3186 }
3187 }
3188
3189 // Resize using Nearest Neighbour
3190
3191 // This particular way is quick but it only works on a multiple of the original
3192
3193 // The idea behind this method can be described with the following diagrams
3194 // second pass and third pass happen in the same loop really.. just separated
3195 // them to show what this does.
3196
3197 // First Pass
3198 // ResultArr:
3199 // 1,1,1,1,1,1
3200 // 1,1,1,1,1,1
3201 // 1,1,1,1,1,1
3202 // 1,1,1,1,1,1
3203 // 1,1,1,1,1,1
3204 // 1,1,1,1,1,1
3205
3206 // Second Pass
3207 // ResultArr2:
3208 // 1,,1,,1,,1,,1,,1,
3209 // ,,,,,,,,,,
3210 // 1,,1,,1,,1,,1,,1,
3211 // ,,,,,,,,,,
3212 // 1,,1,,1,,1,,1,,1,
3213 // ,,,,,,,,,,
3214 // 1,,1,,1,,1,,1,,1,
3215 // ,,,,,,,,,,
3216 // 1,,1,,1,,1,,1,,1,
3217 // ,,,,,,,,,,
3218 // 1,,1,,1,,1,,1,,1,
3219
3220 // Third pass fills in the blanks
3221 // ResultArr2:
3222 // 1,1,1,1,1,1,1,1,1,1,1,1
3223 // 1,1,1,1,1,1,1,1,1,1,1,1
3224 // 1,1,1,1,1,1,1,1,1,1,1,1
3225 // 1,1,1,1,1,1,1,1,1,1,1,1
3226 // 1,1,1,1,1,1,1,1,1,1,1,1
3227 // 1,1,1,1,1,1,1,1,1,1,1,1
3228 // 1,1,1,1,1,1,1,1,1,1,1,1
3229 // 1,1,1,1,1,1,1,1,1,1,1,1
3230 // 1,1,1,1,1,1,1,1,1,1,1,1
3231 // 1,1,1,1,1,1,1,1,1,1,1,1
3232 // 1,1,1,1,1,1,1,1,1,1,1,1
3233
3234 // X,Y = .
3235 // X+1,y = ^
3236 // X,Y+1 = *
3237 // X+1,Y+1 = #
3238
3239 // Filling in like this;
3240 // .*
3241 // ^#
3242 // 1st .
3243 // 2nd *
3244 // 3rd ^
3245 // 4th #
3246 // on single loop.
3247
3248 float[,] resultarr2 = new float[512, 512];
3249 for (int y = 0; y < WorldExtents.Y; y++)
3250 {
3251 for (int x = 0; x < WorldExtents.X; x++)
3252 {
3253 resultarr2[y * 2, x * 2] = resultarr[y, x];
3254
3255 if (y < WorldExtents.Y)
3256 {
3257 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3258 }
3259 if (x < WorldExtents.X)
3260 {
3261 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3262 }
3263 if (x < WorldExtents.X && y < WorldExtents.Y)
3264 {
3265 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3266 }
3267 }
3268 }
3269
3270 //Flatten out the array
3271 int i = 0;
3272 for (int y = 0; y < 512; y++)
3273 {
3274 for (int x = 0; x < 512; x++)
3275 {
3276 if (resultarr2[y, x] <= 0)
3277 returnarr[i] = 0.0000001f;
3278 else
3279 returnarr[i] = resultarr2[y, x];
3280
3281 i++;
3282 }
3283 }
3284
3285 return returnarr;
3286 }
3287
3288 public float[] ResizeTerrain512Interpolation(float[] heightMap)
3289 {
3290 float[] returnarr = new float[262144];
3291 float[,] resultarr = new float[512,512];
3292
3293 // Filling out the array into its multi-dimensional components
3294 for (int y = 0; y < 256; y++)
3295 {
3296 for (int x = 0; x < 256; x++)
3297 {
3298 resultarr[y, x] = heightMap[y * 256 + x];
3299 }
3300 }
3301
3302 // Resize using interpolation
3303
3304 // This particular way is quick but it only works on a multiple of the original
3305
3306 // The idea behind this method can be described with the following diagrams
3307 // second pass and third pass happen in the same loop really.. just separated
3308 // them to show what this does.
3309
3310 // First Pass
3311 // ResultArr:
3312 // 1,1,1,1,1,1
3313 // 1,1,1,1,1,1
3314 // 1,1,1,1,1,1
3315 // 1,1,1,1,1,1
3316 // 1,1,1,1,1,1
3317 // 1,1,1,1,1,1
3318
3319 // Second Pass
3320 // ResultArr2:
3321 // 1,,1,,1,,1,,1,,1,
3322 // ,,,,,,,,,,
3323 // 1,,1,,1,,1,,1,,1,
3324 // ,,,,,,,,,,
3325 // 1,,1,,1,,1,,1,,1,
3326 // ,,,,,,,,,,
3327 // 1,,1,,1,,1,,1,,1,
3328 // ,,,,,,,,,,
3329 // 1,,1,,1,,1,,1,,1,
3330 // ,,,,,,,,,,
3331 // 1,,1,,1,,1,,1,,1,
3332
3333 // Third pass fills in the blanks
3334 // ResultArr2:
3335 // 1,1,1,1,1,1,1,1,1,1,1,1
3336 // 1,1,1,1,1,1,1,1,1,1,1,1
3337 // 1,1,1,1,1,1,1,1,1,1,1,1
3338 // 1,1,1,1,1,1,1,1,1,1,1,1
3339 // 1,1,1,1,1,1,1,1,1,1,1,1
3340 // 1,1,1,1,1,1,1,1,1,1,1,1
3341 // 1,1,1,1,1,1,1,1,1,1,1,1
3342 // 1,1,1,1,1,1,1,1,1,1,1,1
3343 // 1,1,1,1,1,1,1,1,1,1,1,1
3344 // 1,1,1,1,1,1,1,1,1,1,1,1
3345 // 1,1,1,1,1,1,1,1,1,1,1,1
3346
3347 // X,Y = .
3348 // X+1,y = ^
3349 // X,Y+1 = *
3350 // X+1,Y+1 = #
3351
3352 // Filling in like this;
3353 // .*
3354 // ^#
3355 // 1st .
3356 // 2nd *
3357 // 3rd ^
3358 // 4th #
3359 // on single loop.
3360
3361 float[,] resultarr2 = new float[512,512];
3362 for (int y = 0; y < (int)Constants.RegionSize; y++)
3363 {
3364 for (int x = 0; x < (int)Constants.RegionSize; x++)
3365 {
3366 resultarr2[y*2, x*2] = resultarr[y, x];
3367
3368 if (y < (int)Constants.RegionSize)
3369 {
3370 if (y + 1 < (int)Constants.RegionSize)
3371 {
3372 if (x + 1 < (int)Constants.RegionSize)
3373 {
3374 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3375 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3376 }
3377 else
3378 {
3379 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3380 }
3381 }
3382 else
3383 {
3384 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3385 }
3386 }
3387 if (x < (int)Constants.RegionSize)
3388 {
3389 if (x + 1 < (int)Constants.RegionSize)
3390 {
3391 if (y + 1 < (int)Constants.RegionSize)
3392 {
3393 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3394 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3395 }
3396 else
3397 {
3398 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3399 }
3400 }
3401 else
3402 {
3403 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3404 }
3405 }
3406 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3407 {
3408 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3409 {
3410 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3411 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3412 }
3413 else
3414 {
3415 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3416 }
3417 }
3418 }
3419 }
3420 //Flatten out the array
3421 int i = 0;
3422 for (int y = 0; y < 512; y++)
3423 {
3424 for (int x = 0; x < 512; x++)
3425 {
3426 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3427 {
3428 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
3429 resultarr2[y, x] = 0;
3430 }
3431 returnarr[i] = resultarr2[y, x];
3432 i++;
3433 }
3434 }
3435
3436 return returnarr;
3437 }
3438
3439 #endregion
3440
3441 public override void SetTerrain(float[] heightMap)
3442 {
3443 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3444 {
3445 if (m_parentScene is OdeScene)
3446 {
3447 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3448 }
3449 }
3450 else
3451 {
3452 SetTerrain(heightMap, m_worldOffset);
3453 }
3454 }
3455
3456 public void SetTerrain(float[] heightMap, Vector3 pOffset)
3457 {
3458
3459 int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M)
3460
3461 int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra
3462 int heightmapHeight = regionsize + 2;
3463
3464 int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples
3465 int heightmapHeightSamples = (int)regionsize + 2;
3466
3467 // Array of height samples for ODE
3468 float[] _heightmap;
3469 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258
3470
3471 // Other ODE parameters
3472 const float scale = 1.0f;
3473 const float offset = 0.0f;
3474 const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
3475 const int wrap = 0;
3476
3477 float hfmin = 2000f;
3478 float hfmax = -2000f;
3479 float minele = 0.0f; // Dont allow -ve heights
3480
3481 int x = 0;
3482 int y = 0;
3483 int xx = 0;
3484 int yy = 0;
3485
3486 // load the height samples array from the heightMap
3487 for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257
3488 {
3489 for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257
3490 {
3491 xx = x - 1;
3492 if (xx < 0) xx = 0;
3493 if (xx > (regionsize - 1)) xx = regionsize - 1;
3494
3495 yy = y - 1;
3496 if (yy < 0) yy = 0;
3497 if (yy > (regionsize - 1)) yy = regionsize - 1;
3498 // Input xx = 0 0 1 2 ..... 254 255 255 256 total in
3499 // Output x = 0 1 2 3 ..... 255 256 257 258 total out
3500 float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255>
3501 if (val < minele) val = minele;
3502 _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257>
3503 hfmin = (val < hfmin) ? val : hfmin;
3504 hfmax = (val > hfmax) ? val : hfmax;
3505 }
3506 }
3507
3508 lock (OdeLock)
3509 {
3510 IntPtr GroundGeom = IntPtr.Zero;
3511 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3512 {
3513 RegionTerrain.Remove(pOffset);
3514 if (GroundGeom != IntPtr.Zero)
3515 {
3516 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3517 {
3518 TerrainHeightFieldHeights.Remove(GroundGeom);
3519 }
3520 d.SpaceRemove(space, GroundGeom);
3521 d.GeomDestroy(GroundGeom);
3522 }
3523 }
3524 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3525 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3526 heightmapWidth, heightmapHeight, (int)heightmapWidthSamples,
3527 (int)heightmapHeightSamples, scale, offset, thickness, wrap);
3528 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3529 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3530 if (GroundGeom != IntPtr.Zero)
3531 {
3532 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3533 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3534 }
3535 geom_name_map[GroundGeom] = "Terrain";
3536
3537 d.Matrix3 R = new d.Matrix3();
3538
3539 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3540 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3541 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3542
3543 q1 = q1 * q2;
3544 //q1 = q1 * q3;
3545 Vector3 v3;
3546 float angle;
3547 q1.GetAxisAngle(out v3, out angle);
3548
3549 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3550 d.GeomSetRotation(GroundGeom, ref R);
3551 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
3552 IntPtr testGround = IntPtr.Zero;
3553 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3554 {
3555 RegionTerrain.Remove(pOffset);
3556 }
3557 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3558 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3559 }
3560 }
3561
3562 public override void DeleteTerrain()
3563 {
3564 }
3565
3566 public float GetWaterLevel()
3567 {
3568 return waterlevel;
3569 }
3570
3571 public override bool SupportsCombining()
3572 {
3573 return true;
3574 }
3575
3576 public override void UnCombine(PhysicsScene pScene)
3577 {
3578 IntPtr localGround = IntPtr.Zero;
3579// float[] localHeightfield;
3580 bool proceed = false;
3581 List<IntPtr> geomDestroyList = new List<IntPtr>();
3582
3583 lock (OdeLock)
3584 {
3585 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3586 {
3587 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3588 {
3589 if (geom == localGround)
3590 {
3591// localHeightfield = TerrainHeightFieldHeights[geom];
3592 proceed = true;
3593 }
3594 else
3595 {
3596 geomDestroyList.Add(geom);
3597 }
3598 }
3599
3600 if (proceed)
3601 {
3602 m_worldOffset = Vector3.Zero;
3603 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3604 m_parentScene = null;
3605
3606 foreach (IntPtr g in geomDestroyList)
3607 {
3608 // removingHeightField needs to be done or the garbage collector will
3609 // collect the terrain data before we tell ODE to destroy it causing
3610 // memory corruption
3611 if (TerrainHeightFieldHeights.ContainsKey(g))
3612 {
3613// float[] removingHeightField = TerrainHeightFieldHeights[g];
3614 TerrainHeightFieldHeights.Remove(g);
3615
3616 if (RegionTerrain.ContainsKey(g))
3617 {
3618 RegionTerrain.Remove(g);
3619 }
3620
3621 d.GeomDestroy(g);
3622 //removingHeightField = new float[0];
3623 }
3624 }
3625
3626 }
3627 else
3628 {
3629 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3630
3631 }
3632 }
3633 }
3634 }
3635
3636 public override void SetWaterLevel(float baseheight)
3637 {
3638 waterlevel = baseheight;
3639 randomizeWater(waterlevel);
3640 }
3641
3642 public void randomizeWater(float baseheight)
3643 {
3644 const uint heightmapWidth = m_regionWidth + 2;
3645 const uint heightmapHeight = m_regionHeight + 2;
3646 const uint heightmapWidthSamples = m_regionWidth + 2;
3647 const uint heightmapHeightSamples = m_regionHeight + 2;
3648 const float scale = 1.0f;
3649 const float offset = 0.0f;
3650 const float thickness = 2.9f;
3651 const int wrap = 0;
3652
3653 for (int i = 0; i < (258 * 258); i++)
3654 {
3655 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
3656 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
3657 }
3658
3659 lock (OdeLock)
3660 {
3661 if (WaterGeom != IntPtr.Zero)
3662 {
3663 d.SpaceRemove(space, WaterGeom);
3664 }
3665 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3666 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
3667 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3668 offset, thickness, wrap);
3669 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
3670 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
3671 if (WaterGeom != IntPtr.Zero)
3672 {
3673 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
3674 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
3675
3676 }
3677 geom_name_map[WaterGeom] = "Water";
3678
3679 d.Matrix3 R = new d.Matrix3();
3680
3681 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3682 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3683 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3684
3685 q1 = q1 * q2;
3686 //q1 = q1 * q3;
3687 Vector3 v3;
3688 float angle;
3689 q1.GetAxisAngle(out v3, out angle);
3690
3691 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3692 d.GeomSetRotation(WaterGeom, ref R);
3693 d.GeomSetPosition(WaterGeom, 128, 128, 0);
3694
3695 }
3696
3697 }
3698
3699 public override void Dispose()
3700 {
3701 m_rayCastManager.Dispose();
3702 m_rayCastManager = null;
3703
3704 lock (OdeLock)
3705 {
3706 lock (_prims)
3707 {
3708 foreach (OdePrim prm in _prims)
3709 {
3710 RemovePrim(prm);
3711 }
3712 }
3713
3714 //foreach (OdeCharacter act in _characters)
3715 //{
3716 //RemoveAvatar(act);
3717 //}
3718 d.WorldDestroy(world);
3719 //d.CloseODE();
3720 }
3721 }
3722 public override Dictionary<uint, float> GetTopColliders()
3723 {
3724 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
3725 int cnt = 0;
3726 lock (_prims)
3727 {
3728 foreach (OdePrim prm in _prims)
3729 {
3730 if (prm.CollisionScore > 0)
3731 {
3732 returncolliders.Add(prm.m_localID, prm.CollisionScore);
3733 cnt++;
3734 prm.CollisionScore = 0f;
3735 if (cnt > 25)
3736 {
3737 break;
3738 }
3739 }
3740 }
3741 }
3742 return returncolliders;
3743 }
3744
3745 public override bool SupportsRayCast()
3746 {
3747 return true;
3748 }
3749
3750 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3751 {
3752 if (retMethod != null)
3753 {
3754 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3755 }
3756 }
3757
3758#if USE_DRAWSTUFF
3759 // Keyboard callback
3760 public void command(int cmd)
3761 {
3762 IntPtr geom;
3763 d.Mass mass;
3764 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
3765
3766
3767
3768 Char ch = Char.ToLower((Char)cmd);
3769 switch ((Char)ch)
3770 {
3771 case 'w':
3772 try
3773 {
3774 Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
3775
3776 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
3777 ds.SetViewpoint(ref xyz, ref hpr);
3778 }
3779 catch (ArgumentException)
3780 { hpr.X = 0; }
3781 break;
3782
3783 case 'a':
3784 hpr.X++;
3785 ds.SetViewpoint(ref xyz, ref hpr);
3786 break;
3787
3788 case 's':
3789 try
3790 {
3791 Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
3792
3793 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
3794 ds.SetViewpoint(ref xyz, ref hpr);
3795 }
3796 catch (ArgumentException)
3797 { hpr.X = 0; }
3798 break;
3799 case 'd':
3800 hpr.X--;
3801 ds.SetViewpoint(ref xyz, ref hpr);
3802 break;
3803 case 'r':
3804 xyz.Z++;
3805 ds.SetViewpoint(ref xyz, ref hpr);
3806 break;
3807 case 'f':
3808 xyz.Z--;
3809 ds.SetViewpoint(ref xyz, ref hpr);
3810 break;
3811 case 'e':
3812 xyz.Y++;
3813 ds.SetViewpoint(ref xyz, ref hpr);
3814 break;
3815 case 'q':
3816 xyz.Y--;
3817 ds.SetViewpoint(ref xyz, ref hpr);
3818 break;
3819 }
3820 }
3821
3822 public void step(int pause)
3823 {
3824
3825 ds.SetColor(1.0f, 1.0f, 0.0f);
3826 ds.SetTexture(ds.Texture.Wood);
3827 lock (_prims)
3828 {
3829 foreach (OdePrim prm in _prims)
3830 {
3831 //IntPtr body = d.GeomGetBody(prm.prim_geom);
3832 if (prm.prim_geom != IntPtr.Zero)
3833 {
3834 d.Vector3 pos;
3835 d.GeomCopyPosition(prm.prim_geom, out pos);
3836 //d.BodyCopyPosition(body, out pos);
3837
3838 d.Matrix3 R;
3839 d.GeomCopyRotation(prm.prim_geom, out R);
3840 //d.BodyCopyRotation(body, out R);
3841
3842
3843 d.Vector3 sides = new d.Vector3();
3844 sides.X = prm.Size.X;
3845 sides.Y = prm.Size.Y;
3846 sides.Z = prm.Size.Z;
3847
3848 ds.DrawBox(ref pos, ref R, ref sides);
3849 }
3850 }
3851 }
3852 ds.SetColor(1.0f, 0.0f, 0.0f);
3853 lock (_characters)
3854 {
3855 foreach (OdeCharacter chr in _characters)
3856 {
3857 if (chr.Shell != IntPtr.Zero)
3858 {
3859 IntPtr body = d.GeomGetBody(chr.Shell);
3860
3861 d.Vector3 pos;
3862 d.GeomCopyPosition(chr.Shell, out pos);
3863 //d.BodyCopyPosition(body, out pos);
3864
3865 d.Matrix3 R;
3866 d.GeomCopyRotation(chr.Shell, out R);
3867 //d.BodyCopyRotation(body, out R);
3868
3869 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
3870 d.Vector3 sides = new d.Vector3();
3871 sides.X = 0.5f;
3872 sides.Y = 0.5f;
3873 sides.Z = 0.5f;
3874
3875 ds.DrawBox(ref pos, ref R, ref sides);
3876 }
3877 }
3878 }
3879 }
3880
3881 public void start(int unused)
3882 {
3883 ds.SetViewpoint(ref xyz, ref hpr);
3884 }
3885#endif
3886 }
3887}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
new file mode 100644
index 0000000..e7e7bb3
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
@@ -0,0 +1,353 @@
1/* Ubit 2012
2 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
3 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
4 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
5 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
6 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
7 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
8 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
9 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
10 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
11 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
12*/
13
14// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian
15
16using System;
17using System.IO;
18using OpenMetaverse;
19
20namespace OpenSim.Region.Physics.OdePlugin
21{
22
23 unsafe public class wstreamer
24 {
25 byte[] buf;
26 int index;
27 byte* src;
28
29 public wstreamer()
30 {
31 buf = new byte[1024];
32 index = 0;
33 }
34 public wstreamer(int size)
35 {
36 buf = new byte[size];
37 index = 0;
38 }
39
40 public byte[] close()
41 {
42 byte[] data = new byte[index];
43 Buffer.BlockCopy(buf, 0, data, 0, index);
44 return data;
45 }
46
47 public void Seek(int pos)
48 {
49 index = pos;
50 }
51
52 public void Seekrel(int pos)
53 {
54 index += pos;
55 }
56
57 public void Wbyte(byte value)
58 {
59 buf[index++] = value;
60 }
61 public void Wshort(short value)
62 {
63 src = (byte*)&value;
64 buf[index++] = *src++;
65 buf[index++] = *src;
66 }
67 public void Wushort(ushort value)
68 {
69 src = (byte*)&value;
70 buf[index++] = *src++;
71 buf[index++] = *src;
72 }
73 public void Wint(int value)
74 {
75 src = (byte*)&value;
76 buf[index++] = *src++;
77 buf[index++] = *src++;
78 buf[index++] = *src++;
79 buf[index++] = *src;
80 }
81 public void Wuint(uint value)
82 {
83 src = (byte*)&value;
84 buf[index++] = *src++;
85 buf[index++] = *src++;
86 buf[index++] = *src++;
87 buf[index++] = *src;
88 }
89 public void Wlong(long value)
90 {
91 src = (byte*)&value;
92 buf[index++] = *src++;
93 buf[index++] = *src++;
94 buf[index++] = *src++;
95 buf[index++] = *src++;
96 buf[index++] = *src++;
97 buf[index++] = *src++;
98 buf[index++] = *src++;
99 buf[index++] = *src;
100 }
101 public void Wulong(ulong value)
102 {
103 src = (byte*)&value;
104 buf[index++] = *src++;
105 buf[index++] = *src++;
106 buf[index++] = *src++;
107 buf[index++] = *src++;
108 buf[index++] = *src++;
109 buf[index++] = *src++;
110 buf[index++] = *src++;
111 buf[index++] = *src;
112 }
113
114 public void Wfloat(float value)
115 {
116 src = (byte*)&value;
117 buf[index++] = *src++;
118 buf[index++] = *src++;
119 buf[index++] = *src++;
120 buf[index++] = *src;
121 }
122
123 public void Wdouble(double value)
124 {
125 src = (byte*)&value;
126 buf[index++] = *src++;
127 buf[index++] = *src++;
128 buf[index++] = *src++;
129 buf[index++] = *src++;
130 buf[index++] = *src++;
131 buf[index++] = *src++;
132 buf[index++] = *src++;
133 buf[index++] = *src;
134 }
135
136 public void Wvector3(Vector3 value)
137 {
138 src = (byte*)&value.X;
139 buf[index++] = *src++;
140 buf[index++] = *src++;
141 buf[index++] = *src++;
142 buf[index++] = *src;
143 src = (byte*)&value.Y; // it may have padding ??
144 buf[index++] = *src++;
145 buf[index++] = *src++;
146 buf[index++] = *src++;
147 buf[index++] = *src;
148 src = (byte*)&value.Z;
149 buf[index++] = *src++;
150 buf[index++] = *src++;
151 buf[index++] = *src++;
152 buf[index++] = *src;
153 }
154 public void Wquat(Quaternion value)
155 {
156 src = (byte*)&value.X;
157 buf[index++] = *src++;
158 buf[index++] = *src++;
159 buf[index++] = *src++;
160 buf[index++] = *src;
161 src = (byte*)&value.Y; // it may have padding ??
162 buf[index++] = *src++;
163 buf[index++] = *src++;
164 buf[index++] = *src++;
165 buf[index++] = *src;
166 src = (byte*)&value.Z;
167 buf[index++] = *src++;
168 buf[index++] = *src++;
169 buf[index++] = *src++;
170 buf[index++] = *src;
171 src = (byte*)&value.W;
172 buf[index++] = *src++;
173 buf[index++] = *src++;
174 buf[index++] = *src++;
175 buf[index++] = *src;
176 }
177 }
178
179 unsafe public class rstreamer
180 {
181 private byte[] rbuf;
182 private int ptr;
183 private byte* dst;
184
185 public rstreamer(byte[] data)
186 {
187 rbuf = data;
188 ptr = 0;
189 }
190
191 public void close()
192 {
193 }
194
195 public void Seek(int pos)
196 {
197 ptr = pos;
198 }
199
200 public void Seekrel(int pos)
201 {
202 ptr += pos;
203 }
204
205 public byte Rbyte()
206 {
207 return (byte)rbuf[ptr++];
208 }
209
210 public short Rshort()
211 {
212 short v;
213 dst = (byte*)&v;
214 *dst++ = rbuf[ptr++];
215 *dst = rbuf[ptr++];
216 return v;
217 }
218 public ushort Rushort()
219 {
220 ushort v;
221 dst = (byte*)&v;
222 *dst++ = rbuf[ptr++];
223 *dst = rbuf[ptr++];
224 return v;
225 }
226 public int Rint()
227 {
228 int v;
229 dst = (byte*)&v;
230 *dst++ = rbuf[ptr++];
231 *dst++ = rbuf[ptr++];
232 *dst++ = rbuf[ptr++];
233 *dst = rbuf[ptr++];
234 return v;
235 }
236 public uint Ruint()
237 {
238 uint v;
239 dst = (byte*)&v;
240 *dst++ = rbuf[ptr++];
241 *dst++ = rbuf[ptr++];
242 *dst++ = rbuf[ptr++];
243 *dst = rbuf[ptr++];
244 return v;
245 }
246 public long Rlong()
247 {
248 long v;
249 dst = (byte*)&v;
250 *dst++ = rbuf[ptr++];
251 *dst++ = rbuf[ptr++];
252 *dst++ = rbuf[ptr++];
253 *dst++ = rbuf[ptr++];
254 *dst++ = rbuf[ptr++];
255 *dst++ = rbuf[ptr++];
256 *dst++ = rbuf[ptr++];
257 *dst = rbuf[ptr++];
258 return v;
259 }
260 public ulong Rulong()
261 {
262 ulong v;
263 dst = (byte*)&v;
264 *dst++ = rbuf[ptr++];
265 *dst++ = rbuf[ptr++];
266 *dst++ = rbuf[ptr++];
267 *dst++ = rbuf[ptr++];
268 *dst++ = rbuf[ptr++];
269 *dst++ = rbuf[ptr++];
270 *dst++ = rbuf[ptr++];
271 *dst = rbuf[ptr++];
272 return v;
273 }
274 public float Rfloat()
275 {
276 float v;
277 dst = (byte*)&v;
278 *dst++ = rbuf[ptr++];
279 *dst++ = rbuf[ptr++];
280 *dst++ = rbuf[ptr++];
281 *dst = rbuf[ptr++];
282 return v;
283 }
284
285 public double Rdouble()
286 {
287 double v;
288 dst = (byte*)&v;
289 *dst++ = rbuf[ptr++];
290 *dst++ = rbuf[ptr++];
291 *dst++ = rbuf[ptr++];
292 *dst++ = rbuf[ptr++];
293 *dst++ = rbuf[ptr++];
294 *dst++ = rbuf[ptr++];
295 *dst++ = rbuf[ptr++];
296 *dst = rbuf[ptr++];
297 return v;
298 }
299
300 public Vector3 Rvector3()
301 {
302 Vector3 v;
303 dst = (byte*)&v.X;
304 *dst++ = rbuf[ptr++];
305 *dst++ = rbuf[ptr++];
306 *dst++ = rbuf[ptr++];
307 *dst = rbuf[ptr++];
308
309 dst = (byte*)&v.Y;
310 *dst++ = rbuf[ptr++];
311 *dst++ = rbuf[ptr++];
312 *dst++ = rbuf[ptr++];
313 *dst = rbuf[ptr++];
314
315 dst = (byte*)&v.Z;
316 *dst++ = rbuf[ptr++];
317 *dst++ = rbuf[ptr++];
318 *dst++ = rbuf[ptr++];
319 *dst = rbuf[ptr++];
320 return v;
321 }
322
323 public Quaternion Rquat()
324 {
325 Quaternion v;
326 dst = (byte*)&v.X;
327 *dst++ = rbuf[ptr++];
328 *dst++ = rbuf[ptr++];
329 *dst++ = rbuf[ptr++];
330 *dst = rbuf[ptr++];
331
332 dst = (byte*)&v.Y;
333 *dst++ = rbuf[ptr++];
334 *dst++ = rbuf[ptr++];
335 *dst++ = rbuf[ptr++];
336 *dst = rbuf[ptr++];
337
338 dst = (byte*)&v.Z;
339 *dst++ = rbuf[ptr++];
340 *dst++ = rbuf[ptr++];
341 *dst++ = rbuf[ptr++];
342 *dst = rbuf[ptr++];
343
344 dst = (byte*)&v.W;
345 *dst++ = rbuf[ptr++];
346 *dst++ = rbuf[ptr++];
347 *dst++ = rbuf[ptr++];
348 *dst = rbuf[ptr++];
349
350 return v;
351 }
352 }
353}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
new file mode 100644
index 0000000..69e2d03
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/Tests/ODETestClass.cs
@@ -0,0 +1,122 @@
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 Nini.Config;
30using NUnit.Framework;
31using OpenMetaverse;
32using OpenSim.Framework;
33using OpenSim.Region.Physics.Manager;
34using log4net;
35using System.Reflection;
36
37namespace OpenSim.Region.Physics.OdePlugin
38{
39 [TestFixture]
40 public class ODETestClass
41 {
42 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
43
44 private OdePlugin cbt;
45 private PhysicsScene ps;
46 private IMeshingPlugin imp;
47
48 [SetUp]
49 public void Initialize()
50 {
51 // Loading ODEPlugin
52 cbt = new OdePlugin();
53 // Loading Zero Mesher
54 imp = new ZeroMesherPlugin();
55 // Getting Physics Scene
56 ps = cbt.GetScene("test");
57 // Initializing Physics Scene.
58 ps.Initialise(imp.GetMesher(),null);
59 float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
60 for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++)
61 {
62 _heightmap[i] = 21f;
63 }
64 ps.SetTerrain(_heightmap);
65 }
66
67 [TearDown]
68 public void Terminate()
69 {
70 ps.DeleteTerrain();
71 ps.Dispose();
72
73 }
74
75 [Test]
76 public void CreateAndDropPhysicalCube()
77 {
78 PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
79 Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
80 Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
81 Quaternion rot = Quaternion.Identity;
82 PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
83 OdePrim oprim = (OdePrim)prim;
84 OdeScene pscene = (OdeScene) ps;
85
86 Assert.That(oprim.m_taintadd);
87
88 prim.LocalID = 5;
89
90 for (int i = 0; i < 58; i++)
91 {
92 ps.Simulate(0.133f);
93
94 Assert.That(oprim.prim_geom != (IntPtr)0);
95
96 Assert.That(oprim.m_targetSpace != (IntPtr)0);
97
98 //Assert.That(oprim.m_targetSpace == pscene.space);
99 m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
100
101 Assert.That(!oprim.m_taintadd);
102 m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString());
103
104 // Make sure we're above the ground
105 //Assert.That(prim.Position.Z > 20f);
106 //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
107
108 // Make sure we've got a Body
109 Assert.That(oprim.Body != (IntPtr)0);
110 //m_log.Info(
111 }
112
113 // Make sure we're not somewhere above the ground
114 Assert.That(prim.Position.Z < 21.5f);
115
116 ps.RemovePrim(prim);
117 Assert.That(oprim.m_taintremove);
118 ps.Simulate(0.133f);
119 Assert.That(oprim.Body == (IntPtr)0);
120 }
121 }
122}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/drawstuff.cs
@@ -0,0 +1,98 @@
1/*
2 * Copyright ODE
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 *
39 */
40
41using System;
42using System.Runtime.InteropServices;
43using Ode.NET;
44
45namespace Drawstuff.NET
46{
47#if dDOUBLE
48 using dReal = System.Double;
49#else
50 using dReal = System.Single;
51#endif
52
53 public static class ds
54 {
55 public const int VERSION = 2;
56
57 public enum Texture
58 {
59 None,
60 Wood
61 }
62
63 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
64 public delegate void CallbackFunction(int arg);
65
66 [StructLayout(LayoutKind.Sequential)]
67 public struct Functions
68 {
69 public int version;
70 public CallbackFunction start;
71 public CallbackFunction step;
72 public CallbackFunction command;
73 public CallbackFunction stop;
74 public string path_to_textures;
75 }
76
77 [DllImport("drawstuff", EntryPoint = "dsDrawBox")]
78 public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
79
80 [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
81 public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
82
83 [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
84 public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
85
86 [DllImport("drawstuff", EntryPoint = "dsSetColor")]
87 public static extern void SetColor(float red, float green, float blue);
88
89 [DllImport("drawstuff", EntryPoint = "dsSetTexture")]
90 public static extern void SetTexture(Texture texture);
91
92 [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")]
93 public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
94
95 [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")]
96 public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
97 }
98}
diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs
index 2e7bb5d..5485eb7 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -27,6 +27,7 @@
27 27
28using System; 28using System;
29using System.Collections.Generic; 29using System.Collections.Generic;
30using System.Runtime.InteropServices;
30using OpenSim.Framework; 31using OpenSim.Framework;
31using OpenMetaverse; 32using OpenMetaverse;
32 33
@@ -36,7 +37,11 @@ namespace OpenSim.Region.Physics.Manager
36 { 37 {
37 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); 38 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod);
38 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); 39 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical);
39 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache); 40 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde);
41 IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex);
42 void ReleaseMesh(IMesh mesh);
43 void ExpireReleaseMeshs();
44 void ExpireFileCache();
40 } 45 }
41 46
42 // Values for level of detail to be passed to the mesher. 47 // Values for level of detail to be passed to the mesher.
@@ -54,6 +59,25 @@ namespace OpenSim.Region.Physics.Manager
54 { 59 {
55 } 60 }
56 61
62 [Serializable()]
63 [StructLayout(LayoutKind.Explicit)]
64 public struct AMeshKey
65 {
66 [FieldOffset(0)]
67 public UUID uuid;
68 [FieldOffset(0)]
69 public ulong hashA;
70 [FieldOffset(8)]
71 public ulong hashB;
72 [FieldOffset(16)]
73 public ulong hashC;
74
75 public override string ToString()
76 {
77 return uuid.ToString() + "-" + hashC.ToString("x") ;
78 }
79 }
80
57 public interface IMesh 81 public interface IMesh
58 { 82 {
59 List<Vector3> getVertexList(); 83 List<Vector3> getVertexList();
@@ -67,5 +91,7 @@ namespace OpenSim.Region.Physics.Manager
67 void releasePinned(); 91 void releasePinned();
68 void Append(IMesh newMesh); 92 void Append(IMesh newMesh);
69 void TransformLinear(float[,] matrix, float[] offset); 93 void TransformLinear(float[,] matrix, float[] offset);
94 Vector3 GetCentroid();
95 Vector3 GetOBB();
70 } 96 }
71} 97}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index bd806eb..7cd364b 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -43,7 +43,8 @@ namespace OpenSim.Region.Physics.Manager
43 Unknown = 0, 43 Unknown = 0,
44 Agent = 1, 44 Agent = 1,
45 Prim = 2, 45 Prim = 2,
46 Ground = 3 46 Ground = 3,
47 Water = 4
47 } 48 }
48 49
49 public enum PIDHoverType 50 public enum PIDHoverType
@@ -59,15 +60,41 @@ namespace OpenSim.Region.Physics.Manager
59 public Vector3 Position; 60 public Vector3 Position;
60 public Vector3 SurfaceNormal; 61 public Vector3 SurfaceNormal;
61 public float PenetrationDepth; 62 public float PenetrationDepth;
63 public float RelativeSpeed;
64 public bool CharacterFeet;
62 65
63 public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth) 66 public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth)
64 { 67 {
65 Position = position; 68 Position = position;
66 SurfaceNormal = surfaceNormal; 69 SurfaceNormal = surfaceNormal;
67 PenetrationDepth = penetrationDepth; 70 PenetrationDepth = penetrationDepth;
71 RelativeSpeed = 0f; // for now let this one be set explicity
72 CharacterFeet = true; // keep other plugins work as before
73 }
74
75 public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth, bool feet)
76 {
77 Position = position;
78 SurfaceNormal = surfaceNormal;
79 PenetrationDepth = penetrationDepth;
80 RelativeSpeed = 0f; // for now let this one be set explicity
81 CharacterFeet = feet; // keep other plugins work as before
68 } 82 }
69 } 83 }
70 84
85 public struct ContactData
86 {
87 public float mu;
88 public float bounce;
89 public bool softcolide;
90
91 public ContactData(float _mu, float _bounce, bool _softcolide)
92 {
93 mu = _mu;
94 bounce = _bounce;
95 softcolide = _softcolide;
96 }
97 }
71 /// <summary> 98 /// <summary>
72 /// Used to pass collision information to OnCollisionUpdate listeners. 99 /// Used to pass collision information to OnCollisionUpdate listeners.
73 /// </summary> 100 /// </summary>
@@ -99,7 +126,7 @@ namespace OpenSim.Region.Physics.Manager
99 m_objCollisionList.Add(localID, contact); 126 m_objCollisionList.Add(localID, contact);
100 } 127 }
101 else 128 else
102 { 129 {
103 if (m_objCollisionList[localID].PenetrationDepth < contact.PenetrationDepth) 130 if (m_objCollisionList[localID].PenetrationDepth < contact.PenetrationDepth)
104 m_objCollisionList[localID] = contact; 131 m_objCollisionList[localID] = contact;
105 } 132 }
@@ -135,6 +162,8 @@ namespace OpenSim.Region.Physics.Manager
135 /// </summary> 162 /// </summary>
136 public event CollisionUpdate OnCollisionUpdate; 163 public event CollisionUpdate OnCollisionUpdate;
137 164
165 public virtual void SetVehicle(object vdata) { }
166
138 public event OutOfBounds OnOutOfBounds; 167 public event OutOfBounds OnOutOfBounds;
139#pragma warning restore 67 168#pragma warning restore 67
140 169
@@ -142,11 +171,32 @@ namespace OpenSim.Region.Physics.Manager
142 { 171 {
143 get { return new NullPhysicsActor(); } 172 get { return new NullPhysicsActor(); }
144 } 173 }
174
175 public virtual bool Building { get; set; }
176
177 public virtual void getContactData(ref ContactData cdata)
178 {
179 cdata.mu = 0;
180 cdata.bounce = 0;
181 }
145 182
146 public abstract bool Stopped { get; } 183 public abstract bool Stopped { get; }
147 184
148 public abstract Vector3 Size { get; set; } 185 public abstract Vector3 Size { get; set; }
149 186
187 public virtual void setAvatarSize(Vector3 size, float feetOffset)
188 {
189 Size = size;
190 }
191
192 public virtual bool Phantom { get; set; }
193
194 public virtual bool IsVolumeDtc
195 {
196 get { return false; }
197 set { return; }
198 }
199
150 public virtual byte PhysicsShapeType { get; set; } 200 public virtual byte PhysicsShapeType { get; set; }
151 201
152 public abstract PrimitiveBaseShape Shape { set; } 202 public abstract PrimitiveBaseShape Shape { set; }
@@ -169,7 +219,7 @@ namespace OpenSim.Region.Physics.Manager
169 /// XXX: Bizarrely, this cannot be "Terrain" or "Water" right now unless it really is simulating terrain or 219 /// XXX: Bizarrely, this cannot be "Terrain" or "Water" right now unless it really is simulating terrain or
170 /// water. This is not a problem due to the formatting of names given by prims and avatars. 220 /// water. This is not a problem due to the formatting of names given by prims and avatars.
171 /// </remarks> 221 /// </remarks>
172 public string Name { get; protected set; } 222 public string Name { get; set; }
173 223
174 /// <summary> 224 /// <summary>
175 /// This is being used by ODE joint code. 225 /// This is being used by ODE joint code.
@@ -253,6 +303,51 @@ namespace OpenSim.Region.Physics.Manager
253 public abstract Vector3 GeometricCenter { get; } 303 public abstract Vector3 GeometricCenter { get; }
254 public abstract Vector3 CenterOfMass { get; } 304 public abstract Vector3 CenterOfMass { get; }
255 305
306 public virtual Vector3 OOBsize
307 {
308 get
309 {
310 Vector3 s=Size;
311 s.X *=0.5f;
312 s.Y *=0.5f;
313 s.Z *=0.5f;
314 return s;
315 }
316 }
317
318 public virtual Vector3 OOBoffset
319 {
320 get
321 {
322 return Vector3.Zero;
323 }
324 }
325
326 public virtual float OOBRadiusSQ
327 {
328 get
329 {
330 return Size.LengthSquared() * 0.25f; // ((0.5^2)
331 }
332 }
333
334
335 public virtual float PhysicsCost
336 {
337 get
338 {
339 return 0.1f;
340 }
341 }
342
343 public virtual float StreamCost
344 {
345 get
346 {
347 return 1.0f;
348 }
349 }
350
256 /// <summary> 351 /// <summary>
257 /// The desired velocity of this actor. 352 /// The desired velocity of this actor.
258 /// </summary> 353 /// </summary>
@@ -313,13 +408,22 @@ namespace OpenSim.Region.Physics.Manager
313 public abstract void SubscribeEvents(int ms); 408 public abstract void SubscribeEvents(int ms);
314 public abstract void UnSubscribeEvents(); 409 public abstract void UnSubscribeEvents();
315 public abstract bool SubscribedEvents(); 410 public abstract bool SubscribedEvents();
411
412 public virtual void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { }
413
414 // Warning in a parent part it returns itself, not null
415 public virtual PhysicsActor ParentActor { get { return this; } }
416
417
316 } 418 }
317 419
318 public class NullPhysicsActor : PhysicsActor 420 public class NullPhysicsActor : PhysicsActor
319 { 421 {
422 private ActorTypes m_actorType = ActorTypes.Unknown;
423
320 public override bool Stopped 424 public override bool Stopped
321 { 425 {
322 get{ return false; } 426 get{ return true; }
323 } 427 }
324 428
325 public override Vector3 Position 429 public override Vector3 Position
@@ -336,6 +440,7 @@ namespace OpenSim.Region.Physics.Manager
336 440
337 public override uint LocalID 441 public override uint LocalID
338 { 442 {
443 get { return 0; }
339 set { return; } 444 set { return; }
340 } 445 }
341 446
@@ -395,50 +500,17 @@ namespace OpenSim.Region.Physics.Manager
395 set { return; } 500 set { return; }
396 } 501 }
397 502
398 public override void VehicleFloatParam(int param, float value) 503 public override void VehicleFloatParam(int param, float value) {}
399 { 504 public override void VehicleVectorParam(int param, Vector3 value) { }
400 505 public override void VehicleRotationParam(int param, Quaternion rotation) { }
401 } 506 public override void VehicleFlags(int param, bool remove) { }
402 507 public override void SetVolumeDetect(int param) {}
403 public override void VehicleVectorParam(int param, Vector3 value) 508 public override void SetMaterial(int material) {}
404 { 509 public override Vector3 CenterOfMass { get { return Vector3.Zero; }}
405
406 }
407
408 public override void VehicleRotationParam(int param, Quaternion rotation)
409 {
410 510
411 } 511 public override Vector3 GeometricCenter { get { return Vector3.Zero; }}
412
413 public override void VehicleFlags(int param, bool remove)
414 {
415
416 }
417
418 public override void SetVolumeDetect(int param)
419 {
420
421 }
422 512
423 public override void SetMaterial(int material) 513 public override PrimitiveBaseShape Shape { set { return; }}
424 {
425
426 }
427
428 public override Vector3 CenterOfMass
429 {
430 get { return Vector3.Zero; }
431 }
432
433 public override Vector3 GeometricCenter
434 {
435 get { return Vector3.Zero; }
436 }
437
438 public override PrimitiveBaseShape Shape
439 {
440 set { return; }
441 }
442 514
443 public override Vector3 Velocity 515 public override Vector3 Velocity
444 { 516 {
@@ -458,9 +530,7 @@ namespace OpenSim.Region.Physics.Manager
458 set { } 530 set { }
459 } 531 }
460 532
461 public override void CrossingFailure() 533 public override void CrossingFailure() {}
462 {
463 }
464 534
465 public override Quaternion Orientation 535 public override Quaternion Orientation
466 { 536 {
@@ -500,8 +570,20 @@ namespace OpenSim.Region.Physics.Manager
500 570
501 public override int PhysicsActorType 571 public override int PhysicsActorType
502 { 572 {
503 get { return (int) ActorTypes.Unknown; } 573 get { return (int)m_actorType; }
504 set { return; } 574 set {
575 ActorTypes type = (ActorTypes)value;
576 switch (type)
577 {
578 case ActorTypes.Ground:
579 case ActorTypes.Water:
580 m_actorType = type;
581 break;
582 default:
583 m_actorType = ActorTypes.Unknown;
584 break;
585 }
586 }
505 } 587 }
506 588
507 public override bool Kinematic 589 public override bool Kinematic
@@ -510,26 +592,11 @@ namespace OpenSim.Region.Physics.Manager
510 set { return; } 592 set { return; }
511 } 593 }
512 594
513 public override void link(PhysicsActor obj) 595 public override void link(PhysicsActor obj) { }
514 { 596 public override void delink() { }
515 } 597 public override void LockAngularMotion(Vector3 axis) { }
516 598 public override void AddForce(Vector3 force, bool pushforce) { }
517 public override void delink() 599 public override void AddAngularForce(Vector3 force, bool pushforce) { }
518 {
519 }
520
521 public override void LockAngularMotion(Vector3 axis)
522 {
523 }
524
525 public override void AddForce(Vector3 force, bool pushforce)
526 {
527 }
528
529 public override void AddAngularForce(Vector3 force, bool pushforce)
530 {
531
532 }
533 600
534 public override Vector3 RotationalVelocity 601 public override Vector3 RotationalVelocity
535 { 602 {
@@ -551,21 +618,10 @@ namespace OpenSim.Region.Physics.Manager
551 public override float APIDStrength { set { return; } } 618 public override float APIDStrength { set { return; } }
552 public override float APIDDamping { set { return; } } 619 public override float APIDDamping { set { return; } }
553 620
554 public override void SetMomentum(Vector3 momentum) 621 public override void SetMomentum(Vector3 momentum) { }
555 {
556 }
557
558 public override void SubscribeEvents(int ms)
559 {
560
561 }
562 public override void UnSubscribeEvents()
563 {
564 622
565 } 623 public override void SubscribeEvents(int ms) { }
566 public override bool SubscribedEvents() 624 public override void UnSubscribeEvents() { }
567 { 625 public override bool SubscribedEvents() { return false; }
568 return false;
569 }
570 } 626 }
571} 627}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 290b72e..20a70b4 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -38,6 +38,10 @@ namespace OpenSim.Region.Physics.Manager
38 38
39 public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); 39 public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal);
40 public delegate void RayCallback(List<ContactResult> list); 40 public delegate void RayCallback(List<ContactResult> list);
41 public delegate void ProbeBoxCallback(List<ContactResult> list);
42 public delegate void ProbeSphereCallback(List<ContactResult> list);
43 public delegate void ProbePlaneCallback(List<ContactResult> list);
44 public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation);
41 45
42 public delegate void JointMoved(PhysicsJoint joint); 46 public delegate void JointMoved(PhysicsJoint joint);
43 public delegate void JointDeactivated(PhysicsJoint joint); 47 public delegate void JointDeactivated(PhysicsJoint joint);
@@ -86,6 +90,8 @@ namespace OpenSim.Region.Physics.Manager
86 public Vector3 Normal; 90 public Vector3 Normal;
87 } 91 }
88 92
93
94
89 public abstract class PhysicsScene 95 public abstract class PhysicsScene
90 { 96 {
91// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 97// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -133,8 +139,10 @@ namespace OpenSim.Region.Physics.Manager
133 /// <param name="size"></param> 139 /// <param name="size"></param>
134 /// <param name="isFlying"></param> 140 /// <param name="isFlying"></param>
135 /// <returns></returns> 141 /// <returns></returns>
136 public abstract PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying); 142 public virtual PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
137 143 {
144 return null;
145 }
138 /// <summary> 146 /// <summary>
139 /// Add an avatar 147 /// Add an avatar
140 /// </summary> 148 /// </summary>
@@ -151,6 +159,12 @@ namespace OpenSim.Region.Physics.Manager
151 return ret; 159 return ret;
152 } 160 }
153 161
162 public virtual PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
163 {
164 PhysicsActor ret = AddAvatar(localID, avName, position, size, isFlying);
165 return ret;
166 }
167
154 /// <summary> 168 /// <summary>
155 /// Remove an avatar. 169 /// Remove an avatar.
156 /// </summary> 170 /// </summary>
@@ -166,6 +180,19 @@ namespace OpenSim.Region.Physics.Manager
166 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 180 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
167 Vector3 size, Quaternion rotation, bool isPhysical, uint localid); 181 Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
168 182
183 public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
184 uint localid, byte[] sdata)
185 {
186 return null;
187 }
188
189 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
190 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
191 {
192 return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
193 }
194
195
169 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 196 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
170 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid) 197 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
171 { 198 {
@@ -239,6 +266,9 @@ namespace OpenSim.Region.Physics.Manager
239 266
240 public abstract void AddPhysicsActorTaint(PhysicsActor prim); 267 public abstract void AddPhysicsActorTaint(PhysicsActor prim);
241 268
269
270 public virtual void PrepareSimulation() { }
271
242 /// <summary> 272 /// <summary>
243 /// Perform a simulation of the current physics scene over the given timestep. 273 /// Perform a simulation of the current physics scene over the given timestep.
244 /// </summary> 274 /// </summary>
@@ -283,7 +313,7 @@ namespace OpenSim.Region.Physics.Manager
283 } 313 }
284 314
285 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} 315 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
286 316 public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {}
287 public virtual void UnCombine(PhysicsScene pScene) {} 317 public virtual void UnCombine(PhysicsScene pScene) {}
288 318
289 /// <summary> 319 /// <summary>
@@ -331,5 +361,30 @@ namespace OpenSim.Region.Physics.Manager
331 { 361 {
332 return false; 362 return false;
333 } 363 }
364
365 public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
366 {
367 return new List<ContactResult>();
368 }
369
370 public virtual List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
371 {
372 return new List<ContactResult>();
373 }
374
375 public virtual List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
376 {
377 return new List<ContactResult>();
378 }
379
380 public virtual List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
381 {
382 return new List<ContactResult>();
383 }
384
385 public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
386 {
387 return 0;
388 }
334 } 389 }
335} 390}
diff --git a/OpenSim/Region/Physics/Manager/VehicleConstants.cs b/OpenSim/Region/Physics/Manager/VehicleConstants.cs
index f0775c1..8e24b4c 100644
--- a/OpenSim/Region/Physics/Manager/VehicleConstants.cs
+++ b/OpenSim/Region/Physics/Manager/VehicleConstants.cs
@@ -26,6 +26,7 @@
26 */ 26 */
27 27
28using System; 28using System;
29using OpenMetaverse;
29 30
30namespace OpenSim.Region.Physics.Manager 31namespace OpenSim.Region.Physics.Manager
31{ 32{
@@ -117,5 +118,47 @@ namespace OpenSim.Region.Physics.Manager
117 NO_DEFLECTION = 16392, 118 NO_DEFLECTION = 16392,
118 LOCK_ROTATION = 32784 119 LOCK_ROTATION = 32784
119 } 120 }
120 121
122 public struct VehicleData
123 {
124 public Vehicle m_type;
125 public VehicleFlag m_flags;
126
127 // Linear properties
128 public Vector3 m_linearMotorDirection;
129 public Vector3 m_linearFrictionTimescale;
130 public float m_linearMotorDecayTimescale;
131 public float m_linearMotorTimescale;
132 public Vector3 m_linearMotorOffset;
133
134 //Angular properties
135 public Vector3 m_angularMotorDirection;
136 public float m_angularMotorTimescale;
137 public float m_angularMotorDecayTimescale;
138 public Vector3 m_angularFrictionTimescale;
139
140 //Deflection properties
141 public float m_angularDeflectionEfficiency;
142 public float m_angularDeflectionTimescale;
143 public float m_linearDeflectionEfficiency;
144 public float m_linearDeflectionTimescale;
145
146 //Banking properties
147 public float m_bankingEfficiency;
148 public float m_bankingMix;
149 public float m_bankingTimescale;
150
151 //Hover and Buoyancy properties
152 public float m_VhoverHeight;
153 public float m_VhoverEfficiency;
154 public float m_VhoverTimescale;
155 public float m_VehicleBuoyancy;
156
157 //Attractor properties
158 public float m_verticalAttractionEfficiency;
159 public float m_verticalAttractionTimescale;
160
161 // Axis
162 public Quaternion m_referenceFrame;
163 }
121} 164}
diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
index 270d2ec..80ecf66 100644
--- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs
+++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
@@ -67,6 +67,11 @@ namespace OpenSim.Region.Physics.Manager
67 return CreateMesh(primName, primShape, size, lod, false, false); 67 return CreateMesh(primName, primShape, size, lod, false, false);
68 } 68 }
69 69
70 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex,bool forOde)
71 {
72 return CreateMesh(primName, primShape, size, lod, false);
73 }
74
70 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) 75 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
71 { 76 {
72 return CreateMesh(primName, primShape, size, lod, false, false); 77 return CreateMesh(primName, primShape, size, lod, false, false);
@@ -79,5 +84,14 @@ namespace OpenSim.Region.Physics.Manager
79 84
80 return null; 85 return null;
81 } 86 }
87
88 public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
89 {
90 return null;
91 }
92
93 public void ReleaseMesh(IMesh mesh) { }
94 public void ExpireReleaseMeshs() { }
95 public void ExpireFileCache() { }
82 } 96 }
83} 97}
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
index bd8e306..e6b32e7 100644
--- a/OpenSim/Region/Physics/Meshing/Mesh.cs
+++ b/OpenSim/Region/Physics/Meshing/Mesh.cs
@@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing
46 IntPtr m_indicesPtr = IntPtr.Zero; 46 IntPtr m_indicesPtr = IntPtr.Zero;
47 int m_indexCount = 0; 47 int m_indexCount = 0;
48 public float[] m_normals; 48 public float[] m_normals;
49 Vector3 _centroid;
50 int _centroidDiv;
51
52 private class vertexcomp : IEqualityComparer<Vertex>
53 {
54 public bool Equals(Vertex v1, Vertex v2)
55 {
56 if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
57 return true;
58 else
59 return false;
60 }
61 public int GetHashCode(Vertex v)
62 {
63 int a = v.X.GetHashCode();
64 int b = v.Y.GetHashCode();
65 int c = v.Z.GetHashCode();
66 return (a << 16) ^ (b << 8) ^ c;
67 }
68
69 }
49 70
50 public Mesh() 71 public Mesh()
51 { 72 {
52 m_vertices = new Dictionary<Vertex, int>(); 73 vertexcomp vcomp = new vertexcomp();
74
75 m_vertices = new Dictionary<Vertex, int>(vcomp);
53 m_triangles = new List<Triangle>(); 76 m_triangles = new List<Triangle>();
77 _centroid = Vector3.Zero;
78 _centroidDiv = 0;
54 } 79 }
55 80
56 public Mesh Clone() 81 public Mesh Clone()
@@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing
61 { 86 {
62 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); 87 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
63 } 88 }
64 89 result._centroid = _centroid;
90 result._centroidDiv = _centroidDiv;
65 return result; 91 return result;
66 } 92 }
67 93
@@ -71,15 +97,63 @@ namespace OpenSim.Region.Physics.Meshing
71 throw new NotSupportedException("Attempt to Add to a pinned Mesh"); 97 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
72 // If a vertex of the triangle is not yet in the vertices list, 98 // If a vertex of the triangle is not yet in the vertices list,
73 // add it and set its index to the current index count 99 // add it and set its index to the current index count
100 // vertex == seems broken
101 // skip colapsed triangles
102 if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z)
103 || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z)
104 || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z)
105 )
106 {
107 return;
108 }
109
110 if (m_vertices.Count == 0)
111 {
112 _centroidDiv = 0;
113 _centroid = Vector3.Zero;
114 }
115
74 if (!m_vertices.ContainsKey(triangle.v1)) 116 if (!m_vertices.ContainsKey(triangle.v1))
117 {
75 m_vertices[triangle.v1] = m_vertices.Count; 118 m_vertices[triangle.v1] = m_vertices.Count;
119 _centroid.X += triangle.v1.X;
120 _centroid.Y += triangle.v1.Y;
121 _centroid.Z += triangle.v1.Z;
122 _centroidDiv++;
123 }
76 if (!m_vertices.ContainsKey(triangle.v2)) 124 if (!m_vertices.ContainsKey(triangle.v2))
125 {
77 m_vertices[triangle.v2] = m_vertices.Count; 126 m_vertices[triangle.v2] = m_vertices.Count;
127 _centroid.X += triangle.v2.X;
128 _centroid.Y += triangle.v2.Y;
129 _centroid.Z += triangle.v2.Z;
130 _centroidDiv++;
131 }
78 if (!m_vertices.ContainsKey(triangle.v3)) 132 if (!m_vertices.ContainsKey(triangle.v3))
133 {
79 m_vertices[triangle.v3] = m_vertices.Count; 134 m_vertices[triangle.v3] = m_vertices.Count;
135 _centroid.X += triangle.v3.X;
136 _centroid.Y += triangle.v3.Y;
137 _centroid.Z += triangle.v3.Z;
138 _centroidDiv++;
139 }
80 m_triangles.Add(triangle); 140 m_triangles.Add(triangle);
81 } 141 }
82 142
143 public Vector3 GetCentroid()
144 {
145 if (_centroidDiv > 0)
146 return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv);
147 else
148 return Vector3.Zero;
149 }
150
151 // not functional
152 public Vector3 GetOBB()
153 {
154 return new Vector3(0.5f, 0.5f, 0.5f);
155 }
156
83 public void CalcNormals() 157 public void CalcNormals()
84 { 158 {
85 int iTriangles = m_triangles.Count; 159 int iTriangles = m_triangles.Count;
@@ -185,6 +259,7 @@ namespace OpenSim.Region.Physics.Meshing
185 public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) 259 public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount)
186 { 260 {
187 // A vertex is 3 floats 261 // A vertex is 3 floats
262
188 vertexStride = 3 * sizeof(float); 263 vertexStride = 3 * sizeof(float);
189 264
190 // If there isn't an unmanaged array allocated yet, do it now 265 // If there isn't an unmanaged array allocated yet, do it now
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index 8145d61..d181b78 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -705,6 +705,11 @@ namespace OpenSim.Region.Physics.Meshing
705 return CreateMesh(primName, primShape, size, lod, false, true); 705 return CreateMesh(primName, primShape, size, lod, false, true);
706 } 706 }
707 707
708 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
709 {
710 return CreateMesh(primName, primShape, size, lod, false);
711 }
712
708 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) 713 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
709 { 714 {
710 return CreateMesh(primName, primShape, size, lod, isPhysical, true); 715 return CreateMesh(primName, primShape, size, lod, isPhysical, true);
@@ -757,5 +762,13 @@ namespace OpenSim.Region.Physics.Meshing
757 762
758 return mesh; 763 return mesh;
759 } 764 }
765 public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
766 {
767 return null;
768 }
769
770 public void ReleaseMesh(IMesh imesh) { }
771 public void ExpireReleaseMeshs() { }
772 public void ExpireFileCache() { }
760 } 773 }
761} 774}
diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs
index 740424e..b3d9cb6 100644
--- a/OpenSim/Region/Physics/Meshing/SculptMap.cs
+++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs
@@ -58,28 +58,24 @@ namespace PrimMesher
58 if (bmW == 0 || bmH == 0) 58 if (bmW == 0 || bmH == 0)
59 throw new Exception("SculptMap: bitmap has no data"); 59 throw new Exception("SculptMap: bitmap has no data");
60 60
61 int numLodPixels = lod * 2 * lod * 2; // (32 * 2)^2 = 64^2 pixels for default sculpt map image 61 int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image
62 62
63 bool smallMap = bmW * bmH <= numLodPixels;
63 bool needsScaling = false; 64 bool needsScaling = false;
64 65
65 bool smallMap = bmW * bmH <= lod * lod;
66
67 width = bmW; 66 width = bmW;
68 height = bmH; 67 height = bmH;
69 while (width * height > numLodPixels) 68 while (width * height > numLodPixels * 4)
70 { 69 {
71 width >>= 1; 70 width >>= 1;
72 height >>= 1; 71 height >>= 1;
73 needsScaling = true; 72 needsScaling = true;
74 } 73 }
75 74
76
77
78 try 75 try
79 { 76 {
80 if (needsScaling) 77 if (needsScaling)
81 bm = ScaleImage(bm, width, height, 78 bm = ScaleImage(bm, width, height);
82 System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor);
83 } 79 }
84 80
85 catch (Exception e) 81 catch (Exception e)
@@ -87,7 +83,7 @@ namespace PrimMesher
87 throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); 83 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
88 } 84 }
89 85
90 if (width * height > lod * lod) 86 if (width * height > numLodPixels)
91 { 87 {
92 width >>= 1; 88 width >>= 1;
93 height >>= 1; 89 height >>= 1;
@@ -144,15 +140,17 @@ namespace PrimMesher
144 int rowNdx, colNdx; 140 int rowNdx, colNdx;
145 int smNdx = 0; 141 int smNdx = 0;
146 142
143
147 for (rowNdx = 0; rowNdx < numRows; rowNdx++) 144 for (rowNdx = 0; rowNdx < numRows; rowNdx++)
148 { 145 {
149 List<Coord> row = new List<Coord>(numCols); 146 List<Coord> row = new List<Coord>(numCols);
150 for (colNdx = 0; colNdx < numCols; colNdx++) 147 for (colNdx = 0; colNdx < numCols; colNdx++)
151 { 148 {
149
152 if (mirror) 150 if (mirror)
153 row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f)); 151 row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
154 else 152 else
155 row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f)); 153 row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
156 154
157 ++smNdx; 155 ++smNdx;
158 } 156 }
@@ -161,23 +159,39 @@ namespace PrimMesher
161 return rows; 159 return rows;
162 } 160 }
163 161
164 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight, 162 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
165 System.Drawing.Drawing2D.InterpolationMode interpMode)
166 { 163 {
167 Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight);
168 scaledImage.SetResolution(96.0f, 96.0f);
169
170 Graphics grPhoto = Graphics.FromImage(scaledImage);
171 grPhoto.InterpolationMode = interpMode;
172 164
173 grPhoto.DrawImage(srcImage, 165 Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
174 new Rectangle(0, 0, destWidth, destHeight), 166
175 new Rectangle(0, 0, srcImage.Width, srcImage.Height), 167 Color c;
176 GraphicsUnit.Pixel); 168 float xscale = srcImage.Width / destWidth;
169 float yscale = srcImage.Height / destHeight;
170
171 float sy = 0.5f;
172 for (int y = 0; y < destHeight; y++)
173 {
174 float sx = 0.5f;
175 for (int x = 0; x < destWidth; x++)
176 {
177 try
178 {
179 c = srcImage.GetPixel((int)(sx), (int)(sy));
180 scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
181 }
182 catch (IndexOutOfRangeException)
183 {
184 }
177 185
178 grPhoto.Dispose(); 186 sx += xscale;
187 }
188 sy += yscale;
189 }
190 srcImage.Dispose();
179 return scaledImage; 191 return scaledImage;
180 } 192 }
193
194 }
195
181 } 196 }
182}
183#endif 197#endif
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 0d66496..d09aa62 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -352,10 +352,11 @@ namespace OpenSim.Region.Physics.OdePlugin
352 if (m_assetFailed) 352 if (m_assetFailed)
353 { 353 {
354 d.GeomSetCategoryBits(prim_geom, 0); 354 d.GeomSetCategoryBits(prim_geom, 0);
355 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); 355 d.GeomSetCollideBits(prim_geom, BadAssetColideBits());
356 } 356 }
357 else 357 else
358 { 358 {
359
359 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 360 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
360 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 361 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
361 } 362 }
@@ -425,7 +426,7 @@ namespace OpenSim.Region.Physics.OdePlugin
425 if (m_assetFailed) 426 if (m_assetFailed)
426 { 427 {
427 d.GeomSetCategoryBits(prim_geom, 0); 428 d.GeomSetCategoryBits(prim_geom, 0);
428 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); 429 d.GeomSetCollideBits(prim_geom, BadAssetColideBits());
429 } 430 }
430 else 431 else
431 { 432 {
@@ -858,6 +859,11 @@ namespace OpenSim.Region.Physics.OdePlugin
858 859
859 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); 860 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
860 861
862 public int BadAssetColideBits()
863 {
864 return (m_isphysical ? (int)CollisionCategories.Land : 0);
865 }
866
861 private void setMesh(OdeScene parent_scene, IMesh mesh) 867 private void setMesh(OdeScene parent_scene, IMesh mesh)
862 { 868 {
863// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); 869// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
@@ -1139,7 +1145,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
1139 if (prm.m_assetFailed) 1145 if (prm.m_assetFailed)
1140 { 1146 {
1141 d.GeomSetCategoryBits(prm.prim_geom, 0); 1147 d.GeomSetCategoryBits(prm.prim_geom, 0);
1142 d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits); 1148 d.GeomSetCollideBits(prm.prim_geom, prm.BadAssetColideBits());
1143 } 1149 }
1144 else 1150 else
1145 { 1151 {
@@ -1193,7 +1199,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
1193 if (m_assetFailed) 1199 if (m_assetFailed)
1194 { 1200 {
1195 d.GeomSetCategoryBits(prim_geom, 0); 1201 d.GeomSetCategoryBits(prim_geom, 0);
1196 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); 1202 d.GeomSetCollideBits(prim_geom, BadAssetColideBits());
1197 } 1203 }
1198 else 1204 else
1199 { 1205 {
@@ -1395,7 +1401,7 @@ Console.WriteLine("ZProcessTaints for " + Name);
1395 if (m_assetFailed) 1401 if (m_assetFailed)
1396 { 1402 {
1397 d.GeomSetCategoryBits(prim_geom, 0); 1403 d.GeomSetCategoryBits(prim_geom, 0);
1398 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); 1404 d.GeomSetCollideBits(prim_geom, BadAssetColideBits());
1399 } 1405 }
1400 else 1406 else
1401 { 1407 {
@@ -2139,7 +2145,7 @@ Console.WriteLine(" JointCreateFixed");
2139 } 2145 }
2140 2146
2141 if (m_assetFailed) 2147 if (m_assetFailed)
2142 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); 2148 d.GeomSetCollideBits(prim_geom, BadAssetColideBits());
2143 else 2149 else
2144 2150
2145 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2151 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -2950,7 +2956,7 @@ Console.WriteLine(" JointCreateFixed");
2950 } 2956 }
2951 public override bool PIDActive { set { m_usePID = value; } } 2957 public override bool PIDActive { set { m_usePID = value; } }
2952 public override float PIDTau { set { m_PIDTau = value; } } 2958 public override float PIDTau { set { m_PIDTau = value; } }
2953 2959
2954 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 2960 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2955 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 2961 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2956 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 2962 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
@@ -3377,4 +3383,4 @@ Console.WriteLine(" JointCreateFixed");
3377 } 3383 }
3378 } 3384 }
3379 } 3385 }
3380} \ No newline at end of file 3386}
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
index e4fd7eb..2ea8bfc 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
@@ -297,7 +297,7 @@ namespace OpenSim.Region.Physics.POSPlugin
297 { 297 {
298 set { return; } 298 set { return; }
299 } 299 }
300 300
301 public override Quaternion APIDTarget 301 public override Quaternion APIDTarget
302 { 302 {
303 set { return; } 303 set { return; }
diff --git a/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs
new file mode 100644
index 0000000..2938257
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/HelperTypes.cs
@@ -0,0 +1,340 @@
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.Diagnostics;
31using System.Globalization;
32using OpenMetaverse;
33using OpenSim.Region.Physics.Manager;
34using OpenSim.Region.Physics.Meshing;
35
36public class Vertex : IComparable<Vertex>
37{
38 Vector3 vector;
39
40 public float X
41 {
42 get { return vector.X; }
43 set { vector.X = value; }
44 }
45
46 public float Y
47 {
48 get { return vector.Y; }
49 set { vector.Y = value; }
50 }
51
52 public float Z
53 {
54 get { return vector.Z; }
55 set { vector.Z = value; }
56 }
57
58 public Vertex(float x, float y, float z)
59 {
60 vector.X = x;
61 vector.Y = y;
62 vector.Z = z;
63 }
64
65 public Vertex normalize()
66 {
67 float tlength = vector.Length();
68 if (tlength != 0f)
69 {
70 float mul = 1.0f / tlength;
71 return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul);
72 }
73 else
74 {
75 return new Vertex(0f, 0f, 0f);
76 }
77 }
78
79 public Vertex cross(Vertex v)
80 {
81 return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X);
82 }
83
84 // disable warning: mono compiler moans about overloading
85 // operators hiding base operator but should not according to C#
86 // language spec
87#pragma warning disable 0108
88 public static Vertex operator *(Vertex v, Quaternion q)
89 {
90 // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
91
92 Vertex v2 = new Vertex(0f, 0f, 0f);
93
94 v2.X = q.W * q.W * v.X +
95 2f * q.Y * q.W * v.Z -
96 2f * q.Z * q.W * v.Y +
97 q.X * q.X * v.X +
98 2f * q.Y * q.X * v.Y +
99 2f * q.Z * q.X * v.Z -
100 q.Z * q.Z * v.X -
101 q.Y * q.Y * v.X;
102
103 v2.Y =
104 2f * q.X * q.Y * v.X +
105 q.Y * q.Y * v.Y +
106 2f * q.Z * q.Y * v.Z +
107 2f * q.W * q.Z * v.X -
108 q.Z * q.Z * v.Y +
109 q.W * q.W * v.Y -
110 2f * q.X * q.W * v.Z -
111 q.X * q.X * v.Y;
112
113 v2.Z =
114 2f * q.X * q.Z * v.X +
115 2f * q.Y * q.Z * v.Y +
116 q.Z * q.Z * v.Z -
117 2f * q.W * q.Y * v.X -
118 q.Y * q.Y * v.Z +
119 2f * q.W * q.X * v.Y -
120 q.X * q.X * v.Z +
121 q.W * q.W * v.Z;
122
123 return v2;
124 }
125
126 public static Vertex operator +(Vertex v1, Vertex v2)
127 {
128 return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
129 }
130
131 public static Vertex operator -(Vertex v1, Vertex v2)
132 {
133 return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
134 }
135
136 public static Vertex operator *(Vertex v1, Vertex v2)
137 {
138 return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z);
139 }
140
141 public static Vertex operator +(Vertex v1, float am)
142 {
143 v1.X += am;
144 v1.Y += am;
145 v1.Z += am;
146 return v1;
147 }
148
149 public static Vertex operator -(Vertex v1, float am)
150 {
151 v1.X -= am;
152 v1.Y -= am;
153 v1.Z -= am;
154 return v1;
155 }
156
157 public static Vertex operator *(Vertex v1, float am)
158 {
159 v1.X *= am;
160 v1.Y *= am;
161 v1.Z *= am;
162 return v1;
163 }
164
165 public static Vertex operator /(Vertex v1, float am)
166 {
167 if (am == 0f)
168 {
169 return new Vertex(0f,0f,0f);
170 }
171 float mul = 1.0f / am;
172 v1.X *= mul;
173 v1.Y *= mul;
174 v1.Z *= mul;
175 return v1;
176 }
177#pragma warning restore 0108
178
179
180 public float dot(Vertex v)
181 {
182 return X * v.X + Y * v.Y + Z * v.Z;
183 }
184
185 public Vertex(Vector3 v)
186 {
187 vector = v;
188 }
189
190 public Vertex Clone()
191 {
192 return new Vertex(X, Y, Z);
193 }
194
195 public static Vertex FromAngle(double angle)
196 {
197 return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f);
198 }
199
200 public float Length()
201 {
202 return vector.Length();
203 }
204
205 public virtual bool Equals(Vertex v, float tolerance)
206 {
207 Vertex diff = this - v;
208 float d = diff.Length();
209 if (d < tolerance)
210 return true;
211
212 return false;
213 }
214
215
216 public int CompareTo(Vertex other)
217 {
218 if (X < other.X)
219 return -1;
220
221 if (X > other.X)
222 return 1;
223
224 if (Y < other.Y)
225 return -1;
226
227 if (Y > other.Y)
228 return 1;
229
230 if (Z < other.Z)
231 return -1;
232
233 if (Z > other.Z)
234 return 1;
235
236 return 0;
237 }
238
239 public static bool operator >(Vertex me, Vertex other)
240 {
241 return me.CompareTo(other) > 0;
242 }
243
244 public static bool operator <(Vertex me, Vertex other)
245 {
246 return me.CompareTo(other) < 0;
247 }
248
249 public String ToRaw()
250 {
251 // Why this stuff with the number formatter?
252 // Well, the raw format uses the english/US notation of numbers
253 // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1.
254 // The german notation uses these characters exactly vice versa!
255 // The Float.ToString() routine is a localized one, giving different results depending on the country
256 // settings your machine works with. Unusable for a machine readable file format :-(
257 NumberFormatInfo nfi = new NumberFormatInfo();
258 nfi.NumberDecimalSeparator = ".";
259 nfi.NumberDecimalDigits = 3;
260
261 String s1 = X.ToString("N2", nfi) + " " + Y.ToString("N2", nfi) + " " + Z.ToString("N2", nfi);
262
263 return s1;
264 }
265}
266
267public class Triangle
268{
269 public Vertex v1;
270 public Vertex v2;
271 public Vertex v3;
272
273 public Triangle(Vertex _v1, Vertex _v2, Vertex _v3)
274 {
275 v1 = _v1;
276 v2 = _v2;
277 v3 = _v3;
278 }
279
280 public Triangle(float _v1x,float _v1y,float _v1z,
281 float _v2x,float _v2y,float _v2z,
282 float _v3x,float _v3y,float _v3z)
283 {
284 v1 = new Vertex(_v1x, _v1y, _v1z);
285 v2 = new Vertex(_v2x, _v2y, _v2z);
286 v3 = new Vertex(_v3x, _v3y, _v3z);
287 }
288
289 public override String ToString()
290 {
291 NumberFormatInfo nfi = new NumberFormatInfo();
292 nfi.CurrencyDecimalDigits = 2;
293 nfi.CurrencyDecimalSeparator = ".";
294
295 String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">";
296 String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">";
297 String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">";
298
299 return s1 + ";" + s2 + ";" + s3;
300 }
301
302 public Vector3 getNormal()
303 {
304 // Vertices
305
306 // Vectors for edges
307 Vector3 e1;
308 Vector3 e2;
309
310 e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
311 e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z);
312
313 // Cross product for normal
314 Vector3 n = Vector3.Cross(e1, e2);
315
316 // Length
317 float l = n.Length();
318
319 // Normalized "normal"
320 n = n/l;
321
322 return n;
323 }
324
325 public void invertNormal()
326 {
327 Vertex vt;
328 vt = v1;
329 v1 = v2;
330 v2 = vt;
331 }
332
333 // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and
334 // debugging purposes
335 public String ToStringRaw()
336 {
337 String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw();
338 return output;
339 }
340}
diff --git a/OpenSim/Region/Physics/UbitMeshing/Mesh.cs b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
new file mode 100644
index 0000000..fa06926
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
@@ -0,0 +1,614 @@
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.IO;
31using System.Runtime.InteropServices;
32using OpenSim.Region.Physics.Manager;
33using PrimMesher;
34using OpenMetaverse;
35using System.Runtime.Serialization;
36using System.Runtime.Serialization.Formatters.Binary;
37
38namespace OpenSim.Region.Physics.Meshing
39{
40 public class MeshBuildingData
41 {
42 public Dictionary<Vertex, int> m_vertices;
43 public List<Triangle> m_triangles;
44 public float m_obbXmin;
45 public float m_obbXmax;
46 public float m_obbYmin;
47 public float m_obbYmax;
48 public float m_obbZmin;
49 public float m_obbZmax;
50 public Vector3 m_centroid;
51 public int m_centroidDiv;
52 }
53
54 [Serializable()]
55 public class Mesh : IMesh
56 {
57 float[] vertices;
58 int[] indexes;
59 Vector3 m_obb;
60 Vector3 m_obboffset;
61 [NonSerialized()]
62 MeshBuildingData m_bdata;
63 [NonSerialized()]
64 GCHandle vhandler;
65 [NonSerialized()]
66 GCHandle ihandler;
67 [NonSerialized()]
68 IntPtr m_verticesPtr = IntPtr.Zero;
69 [NonSerialized()]
70 IntPtr m_indicesPtr = IntPtr.Zero;
71 [NonSerialized()]
72 int m_vertexCount = 0;
73 [NonSerialized()]
74 int m_indexCount = 0;
75
76 public int RefCount { get; set; }
77 public AMeshKey Key { get; set; }
78
79 private class vertexcomp : IEqualityComparer<Vertex>
80 {
81 public bool Equals(Vertex v1, Vertex v2)
82 {
83 if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
84 return true;
85 else
86 return false;
87 }
88 public int GetHashCode(Vertex v)
89 {
90 int a = v.X.GetHashCode();
91 int b = v.Y.GetHashCode();
92 int c = v.Z.GetHashCode();
93 return (a << 16) ^ (b << 8) ^ c;
94 }
95 }
96
97 public Mesh()
98 {
99 vertexcomp vcomp = new vertexcomp();
100
101 m_bdata = new MeshBuildingData();
102 m_bdata.m_vertices = new Dictionary<Vertex, int>(vcomp);
103 m_bdata.m_triangles = new List<Triangle>();
104 m_bdata.m_centroid = Vector3.Zero;
105 m_bdata.m_centroidDiv = 0;
106 m_bdata.m_obbXmin = float.MaxValue;
107 m_bdata.m_obbXmax = float.MinValue;
108 m_bdata.m_obbYmin = float.MaxValue;
109 m_bdata.m_obbYmax = float.MinValue;
110 m_bdata.m_obbZmin = float.MaxValue;
111 m_bdata.m_obbZmax = float.MinValue;
112 m_obb = new Vector3(0.5f, 0.5f, 0.5f);
113 m_obboffset = Vector3.Zero;
114 }
115
116
117 public Mesh Scale(Vector3 scale)
118 {
119 if (m_verticesPtr == null || m_indicesPtr == null)
120 return null;
121
122 Mesh result = new Mesh();
123
124 float x = scale.X;
125 float y = scale.Y;
126 float z = scale.Z;
127
128 result.m_obb.X = m_obb.X * x;
129 result.m_obb.Y = m_obb.Y * y;
130 result.m_obb.Z = m_obb.Z * z;
131 result.m_obboffset.X = m_obboffset.X * x;
132 result.m_obboffset.Y = m_obboffset.Y * y;
133 result.m_obboffset.Z = m_obboffset.Z * z;
134
135 result.vertices = new float[vertices.Length];
136 int j = 0;
137 for (int i = 0; i < m_vertexCount; i++)
138 {
139 result.vertices[j] = vertices[j] * x;
140 j++;
141 result.vertices[j] = vertices[j] * y;
142 j++;
143 result.vertices[j] = vertices[j] * z;
144 j++;
145 }
146
147 result.indexes = new int[indexes.Length];
148 indexes.CopyTo(result.indexes,0);
149
150 result.pinMemory();
151
152 return result;
153 }
154
155 public Mesh Clone()
156 {
157 Mesh result = new Mesh();
158
159 if (m_bdata != null)
160 {
161 result.m_bdata = new MeshBuildingData();
162 foreach (Triangle t in m_bdata.m_triangles)
163 {
164 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
165 }
166 result.m_bdata.m_centroid = m_bdata.m_centroid;
167 result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv;
168 result.m_bdata.m_obbXmin = m_bdata.m_obbXmin;
169 result.m_bdata.m_obbXmax = m_bdata.m_obbXmax;
170 result.m_bdata.m_obbYmin = m_bdata.m_obbYmin;
171 result.m_bdata.m_obbYmax = m_bdata.m_obbYmax;
172 result.m_bdata.m_obbZmin = m_bdata.m_obbZmin;
173 result.m_bdata.m_obbZmax = m_bdata.m_obbZmax;
174 }
175 result.m_obb = m_obb;
176 result.m_obboffset = m_obboffset;
177 return result;
178 }
179
180 public void addVertexLStats(Vertex v)
181 {
182 float x = v.X;
183 float y = v.Y;
184 float z = v.Z;
185
186 m_bdata.m_centroid.X += x;
187 m_bdata.m_centroid.Y += y;
188 m_bdata.m_centroid.Z += z;
189 m_bdata.m_centroidDiv++;
190
191 if (x > m_bdata.m_obbXmax)
192 m_bdata.m_obbXmax = x;
193 else if (x < m_bdata.m_obbXmin)
194 m_bdata.m_obbXmin = x;
195
196 if (y > m_bdata.m_obbYmax)
197 m_bdata.m_obbYmax = y;
198 else if (y < m_bdata.m_obbYmin)
199 m_bdata.m_obbYmin = y;
200
201 if (z > m_bdata.m_obbZmax)
202 m_bdata.m_obbZmax = z;
203 else if (z < m_bdata.m_obbZmin)
204 m_bdata.m_obbZmin = z;
205
206 }
207
208 private float fRound(float f)
209 {
210 int i;
211 if (f == 0f)
212 return f;
213 else if (f > 0f)
214 i = (int)(1e5f * f + 0.5f);
215 else
216 i = (int)(1e5f * f - 0.5f);
217
218 return ((float)i * 1e-5f);
219 }
220
221 public void Add(Triangle triangle)
222 {
223 if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
224 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
225
226 // round down
227 triangle.v1.X = fRound(triangle.v1.X);
228 triangle.v1.Y = fRound(triangle.v1.Y);
229 triangle.v1.Z = fRound(triangle.v1.Z);
230 triangle.v2.X = fRound(triangle.v2.X);
231 triangle.v2.Y = fRound(triangle.v2.Y);
232 triangle.v2.Z = fRound(triangle.v2.Z);
233 triangle.v3.X = fRound(triangle.v3.X);
234 triangle.v3.Y = fRound(triangle.v3.Y);
235 triangle.v3.Z = fRound(triangle.v3.Z);
236
237 if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z)
238 || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z)
239 || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z)
240 )
241 {
242 return;
243 }
244
245 if (m_bdata.m_vertices.Count == 0)
246 {
247 m_bdata.m_centroidDiv = 0;
248 m_bdata.m_centroid = Vector3.Zero;
249 }
250
251 if (!m_bdata.m_vertices.ContainsKey(triangle.v1))
252 {
253 m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count;
254 addVertexLStats(triangle.v1);
255 }
256 if (!m_bdata.m_vertices.ContainsKey(triangle.v2))
257 {
258 m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count;
259 addVertexLStats(triangle.v2);
260 }
261 if (!m_bdata.m_vertices.ContainsKey(triangle.v3))
262 {
263 m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count;
264 addVertexLStats(triangle.v3);
265 }
266 m_bdata.m_triangles.Add(triangle);
267 }
268
269 public Vector3 GetCentroid()
270 {
271 return m_obboffset;
272
273 }
274
275 public Vector3 GetOBB()
276 {
277 return m_obb;
278 float x, y, z;
279 if (m_bdata.m_centroidDiv > 0)
280 {
281 x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
282 y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
283 z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
284 }
285 else // ??
286 {
287 x = 0.5f;
288 y = 0.5f;
289 z = 0.5f;
290 }
291 return new Vector3(x, y, z);
292 }
293
294 public List<Vector3> getVertexList()
295 {
296 List<Vector3> result = new List<Vector3>();
297 foreach (Vertex v in m_bdata.m_vertices.Keys)
298 {
299 result.Add(new Vector3(v.X, v.Y, v.Z));
300 }
301 return result;
302 }
303
304 public float[] getVertexListAsFloat()
305 {
306 if (m_bdata.m_vertices == null)
307 throw new NotSupportedException();
308 float[] result = new float[m_bdata.m_vertices.Count * 3];
309 foreach (KeyValuePair<Vertex, int> kvp in m_bdata.m_vertices)
310 {
311 Vertex v = kvp.Key;
312 int i = kvp.Value;
313 result[3 * i + 0] = v.X;
314 result[3 * i + 1] = v.Y;
315 result[3 * i + 2] = v.Z;
316 }
317 return result;
318 }
319
320 public float[] getVertexListAsFloatLocked()
321 {
322 return null;
323 }
324
325 public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount)
326 {
327 // A vertex is 3 floats
328 vertexStride = 3 * sizeof(float);
329
330 // If there isn't an unmanaged array allocated yet, do it now
331 if (m_verticesPtr == IntPtr.Zero && m_bdata != null)
332 {
333 vertices = getVertexListAsFloat();
334 // Each vertex is 3 elements (floats)
335 m_vertexCount = vertices.Length / 3;
336 vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
337 m_verticesPtr = vhandler.AddrOfPinnedObject();
338 GC.AddMemoryPressure(Buffer.ByteLength(vertices));
339 }
340 _vertices = m_verticesPtr;
341 vertexCount = m_vertexCount;
342 }
343
344 public int[] getIndexListAsInt()
345 {
346 if (m_bdata.m_triangles == null)
347 throw new NotSupportedException();
348 int[] result = new int[m_bdata.m_triangles.Count * 3];
349 for (int i = 0; i < m_bdata.m_triangles.Count; i++)
350 {
351 Triangle t = m_bdata.m_triangles[i];
352 result[3 * i + 0] = m_bdata.m_vertices[t.v1];
353 result[3 * i + 1] = m_bdata.m_vertices[t.v2];
354 result[3 * i + 2] = m_bdata.m_vertices[t.v3];
355 }
356 return result;
357 }
358
359 /// <summary>
360 /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA
361 /// </summary>
362 /// <returns></returns>
363 public int[] getIndexListAsIntLocked()
364 {
365 return null;
366 }
367
368 public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount)
369 {
370 // If there isn't an unmanaged array allocated yet, do it now
371 if (m_indicesPtr == IntPtr.Zero && m_bdata != null)
372 {
373 indexes = getIndexListAsInt();
374 m_indexCount = indexes.Length;
375 ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
376 m_indicesPtr = ihandler.AddrOfPinnedObject();
377 GC.AddMemoryPressure(Buffer.ByteLength(indexes));
378 }
379 // A triangle is 3 ints (indices)
380 triStride = 3 * sizeof(int);
381 indices = m_indicesPtr;
382 indexCount = m_indexCount;
383 }
384
385 public void releasePinned()
386 {
387 if (m_verticesPtr != IntPtr.Zero)
388 {
389 vhandler.Free();
390 vertices = null;
391 m_verticesPtr = IntPtr.Zero;
392 }
393 if (m_indicesPtr != IntPtr.Zero)
394 {
395 ihandler.Free();
396 indexes = null;
397 m_indicesPtr = IntPtr.Zero;
398 }
399 }
400
401 /// <summary>
402 /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions
403 /// </summary>
404 public void releaseSourceMeshData()
405 {
406 if (m_bdata != null)
407 {
408 m_bdata.m_triangles = null;
409 m_bdata.m_vertices = null;
410 }
411 }
412
413 public void releaseBuildingMeshData()
414 {
415 if (m_bdata != null)
416 {
417 m_bdata.m_triangles = null;
418 m_bdata.m_vertices = null;
419 m_bdata = null;
420 }
421 }
422
423 public void Append(IMesh newMesh)
424 {
425 if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
426 throw new NotSupportedException("Attempt to Append to a pinned Mesh");
427
428 if (!(newMesh is Mesh))
429 return;
430
431 foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles)
432 Add(t);
433 }
434
435 // Do a linear transformation of mesh.
436 public void TransformLinear(float[,] matrix, float[] offset)
437 {
438 if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
439 throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh");
440
441 foreach (Vertex v in m_bdata.m_vertices.Keys)
442 {
443 if (v == null)
444 continue;
445 float x, y, z;
446 x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0];
447 y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1];
448 z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2];
449 v.X = x + offset[0];
450 v.Y = y + offset[1];
451 v.Z = z + offset[2];
452 }
453 }
454
455 public void DumpRaw(String path, String name, String title)
456 {
457 if (path == null)
458 return;
459 if (m_bdata == null)
460 return;
461 String fileName = name + "_" + title + ".raw";
462 String completePath = System.IO.Path.Combine(path, fileName);
463 StreamWriter sw = new StreamWriter(completePath);
464 foreach (Triangle t in m_bdata.m_triangles)
465 {
466 String s = t.ToStringRaw();
467 sw.WriteLine(s);
468 }
469 sw.Close();
470 }
471
472 public void TrimExcess()
473 {
474 m_bdata.m_triangles.TrimExcess();
475 }
476
477 public void pinMemory()
478 {
479 m_vertexCount = vertices.Length / 3;
480 vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned);
481 m_verticesPtr = vhandler.AddrOfPinnedObject();
482 GC.AddMemoryPressure(Buffer.ByteLength(vertices));
483
484 m_indexCount = indexes.Length;
485 ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned);
486 m_indicesPtr = ihandler.AddrOfPinnedObject();
487 GC.AddMemoryPressure(Buffer.ByteLength(indexes));
488 }
489
490 public void PrepForOde()
491 {
492 // If there isn't an unmanaged array allocated yet, do it now
493 if (m_verticesPtr == IntPtr.Zero)
494 vertices = getVertexListAsFloat();
495
496 // If there isn't an unmanaged array allocated yet, do it now
497 if (m_indicesPtr == IntPtr.Zero)
498 indexes = getIndexListAsInt();
499
500 pinMemory();
501
502 float x, y, z;
503
504 if (m_bdata.m_centroidDiv > 0)
505 {
506 m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv);
507 x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f;
508 y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f;
509 z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f;
510 }
511
512 else
513 {
514 m_obboffset = Vector3.Zero;
515 x = 0.5f;
516 y = 0.5f;
517 z = 0.5f;
518 }
519 m_obb = new Vector3(x, y, z);
520
521 releaseBuildingMeshData();
522 }
523 public bool ToStream(Stream st)
524 {
525 if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero)
526 return false;
527
528 BinaryWriter bw = new BinaryWriter(st);
529 bool ok = true;
530
531 try
532 {
533
534 bw.Write(m_vertexCount);
535 bw.Write(m_indexCount);
536
537 for (int i = 0; i < 3 * m_vertexCount; i++)
538 bw.Write(vertices[i]);
539 for (int i = 0; i < m_indexCount; i++)
540 bw.Write(indexes[i]);
541 bw.Write(m_obb.X);
542 bw.Write(m_obb.Y);
543 bw.Write(m_obb.Z);
544 bw.Write(m_obboffset.X);
545 bw.Write(m_obboffset.Y);
546 bw.Write(m_obboffset.Z);
547 }
548 catch
549 {
550 ok = false;
551 }
552
553 if (bw != null)
554 {
555 bw.Flush();
556 bw.Close();
557 }
558
559 return ok;
560 }
561
562 public static Mesh FromStream(Stream st, AMeshKey key)
563 {
564 Mesh mesh = new Mesh();
565 mesh.releaseBuildingMeshData();
566
567 BinaryReader br = new BinaryReader(st);
568
569 bool ok = true;
570 try
571 {
572 mesh.m_vertexCount = br.ReadInt32();
573 mesh.m_indexCount = br.ReadInt32();
574
575 int n = 3 * mesh.m_vertexCount;
576 mesh.vertices = new float[n];
577 for (int i = 0; i < n; i++)
578 mesh.vertices[i] = br.ReadSingle();
579
580 mesh.indexes = new int[mesh.m_indexCount];
581 for (int i = 0; i < mesh.m_indexCount; i++)
582 mesh.indexes[i] = br.ReadInt32();
583
584 mesh.m_obb.X = br.ReadSingle();
585 mesh.m_obb.Y = br.ReadSingle();
586 mesh.m_obb.Z = br.ReadSingle();
587
588 mesh.m_obboffset.X = br.ReadSingle();
589 mesh.m_obboffset.Y = br.ReadSingle();
590 mesh.m_obboffset.Z = br.ReadSingle();
591 }
592 catch
593 {
594 ok = false;
595 }
596
597 br.Close();
598
599 if (ok)
600 {
601 mesh.pinMemory();
602
603 mesh.Key = key;
604 mesh.RefCount = 1;
605
606 return mesh;
607 }
608
609 mesh.vertices = null;
610 mesh.indexes = null;
611 return null;
612 }
613 }
614}
diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
new file mode 100644
index 0000000..00cbfbd
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
@@ -0,0 +1,1410 @@
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//#define SPAM
28
29using System;
30using System.Collections.Generic;
31using OpenSim.Framework;
32using OpenSim.Region.Physics.Manager;
33using OpenMetaverse;
34using OpenMetaverse.StructuredData;
35using System.Drawing;
36using System.Drawing.Imaging;
37using System.IO.Compression;
38using PrimMesher;
39using log4net;
40using Nini.Config;
41using System.Reflection;
42using System.IO;
43using ComponentAce.Compression.Libs.zlib;
44using OpenSim.Region.Physics.ConvexDecompositionDotNet;
45using System.Runtime.Serialization;
46using System.Runtime.Serialization.Formatters.Binary;
47
48namespace OpenSim.Region.Physics.Meshing
49{
50 public class MeshmerizerPlugin : IMeshingPlugin
51 {
52 public MeshmerizerPlugin()
53 {
54 }
55
56 public string GetName()
57 {
58 return "UbitMeshmerizer";
59 }
60
61 public IMesher GetMesher(IConfigSource config)
62 {
63 return new Meshmerizer(config);
64 }
65 }
66
67 public class Meshmerizer : IMesher
68 {
69 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
70
71 // Setting baseDir to a path will enable the dumping of raw files
72 // raw files can be imported by blender so a visual inspection of the results can be done
73
74 public object diskLock = new object();
75
76 public bool doMeshFileCache = true;
77
78 public string cachePath = "MeshCache";
79 public TimeSpan CacheExpire;
80 public bool doCacheExpire = true;
81
82// const string baseDir = "rawFiles";
83 private const string baseDir = null; //"rawFiles";
84
85 private bool useMeshiesPhysicsMesh = false;
86
87 private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh
88
89 private Dictionary<AMeshKey, Mesh> m_uniqueMeshes = new Dictionary<AMeshKey, Mesh>();
90 private Dictionary<AMeshKey, Mesh> m_uniqueReleasedMeshes = new Dictionary<AMeshKey, Mesh>();
91
92 public Meshmerizer(IConfigSource config)
93 {
94 IConfig start_config = config.Configs["Startup"];
95 IConfig mesh_config = config.Configs["Mesh"];
96
97
98 float fcache = 48.0f;
99// float fcache = 0.02f;
100
101 if(mesh_config != null)
102 {
103 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
104 if (useMeshiesPhysicsMesh)
105 {
106 doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache);
107 cachePath = mesh_config.GetString("MeshFileCachePath", cachePath);
108 fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache);
109 doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire);
110 }
111 else
112 {
113 doMeshFileCache = false;
114 doCacheExpire = false;
115 }
116 }
117
118 CacheExpire = TimeSpan.FromHours(fcache);
119
120 }
121
122 /// <summary>
123 /// creates a simple box mesh of the specified size. This mesh is of very low vertex count and may
124 /// be useful as a backup proxy when level of detail is not needed or when more complex meshes fail
125 /// for some reason
126 /// </summary>
127 /// <param name="minX"></param>
128 /// <param name="maxX"></param>
129 /// <param name="minY"></param>
130 /// <param name="maxY"></param>
131 /// <param name="minZ"></param>
132 /// <param name="maxZ"></param>
133 /// <returns></returns>
134 private static Mesh CreateSimpleBoxMesh(float minX, float maxX, float minY, float maxY, float minZ, float maxZ)
135 {
136 Mesh box = new Mesh();
137 List<Vertex> vertices = new List<Vertex>();
138 // bottom
139
140 vertices.Add(new Vertex(minX, maxY, minZ));
141 vertices.Add(new Vertex(maxX, maxY, minZ));
142 vertices.Add(new Vertex(maxX, minY, minZ));
143 vertices.Add(new Vertex(minX, minY, minZ));
144
145 box.Add(new Triangle(vertices[0], vertices[1], vertices[2]));
146 box.Add(new Triangle(vertices[0], vertices[2], vertices[3]));
147
148 // top
149
150 vertices.Add(new Vertex(maxX, maxY, maxZ));
151 vertices.Add(new Vertex(minX, maxY, maxZ));
152 vertices.Add(new Vertex(minX, minY, maxZ));
153 vertices.Add(new Vertex(maxX, minY, maxZ));
154
155 box.Add(new Triangle(vertices[4], vertices[5], vertices[6]));
156 box.Add(new Triangle(vertices[4], vertices[6], vertices[7]));
157
158 // sides
159
160 box.Add(new Triangle(vertices[5], vertices[0], vertices[3]));
161 box.Add(new Triangle(vertices[5], vertices[3], vertices[6]));
162
163 box.Add(new Triangle(vertices[1], vertices[0], vertices[5]));
164 box.Add(new Triangle(vertices[1], vertices[5], vertices[4]));
165
166 box.Add(new Triangle(vertices[7], vertices[1], vertices[4]));
167 box.Add(new Triangle(vertices[7], vertices[2], vertices[1]));
168
169 box.Add(new Triangle(vertices[3], vertices[2], vertices[7]));
170 box.Add(new Triangle(vertices[3], vertices[7], vertices[6]));
171
172 return box;
173 }
174
175 /// <summary>
176 /// Creates a simple bounding box mesh for a complex input mesh
177 /// </summary>
178 /// <param name="meshIn"></param>
179 /// <returns></returns>
180 private static Mesh CreateBoundingBoxMesh(Mesh meshIn)
181 {
182 float minX = float.MaxValue;
183 float maxX = float.MinValue;
184 float minY = float.MaxValue;
185 float maxY = float.MinValue;
186 float minZ = float.MaxValue;
187 float maxZ = float.MinValue;
188
189 foreach (Vector3 v in meshIn.getVertexList())
190 {
191 if (v.X < minX) minX = v.X;
192 if (v.Y < minY) minY = v.Y;
193 if (v.Z < minZ) minZ = v.Z;
194
195 if (v.X > maxX) maxX = v.X;
196 if (v.Y > maxY) maxY = v.Y;
197 if (v.Z > maxZ) maxZ = v.Z;
198 }
199
200 return CreateSimpleBoxMesh(minX, maxX, minY, maxY, minZ, maxZ);
201 }
202
203 private void ReportPrimError(string message, string primName, PrimMesh primMesh)
204 {
205 m_log.Error(message);
206 m_log.Error("\nPrim Name: " + primName);
207 m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString());
208 }
209
210 /// <summary>
211 /// Add a submesh to an existing list of coords and faces.
212 /// </summary>
213 /// <param name="subMeshData"></param>
214 /// <param name="size">Size of entire object</param>
215 /// <param name="coords"></param>
216 /// <param name="faces"></param>
217 private void AddSubMesh(OSDMap subMeshData, List<Coord> coords, List<Face> faces)
218 {
219 // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap));
220
221 // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level
222 // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no
223 // geometry for this submesh.
224 if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"]))
225 return;
226
227 OpenMetaverse.Vector3 posMax;
228 OpenMetaverse.Vector3 posMin;
229 if (subMeshData.ContainsKey("PositionDomain"))
230 {
231 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3();
232 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3();
233 }
234 else
235 {
236 posMax = new Vector3(0.5f, 0.5f, 0.5f);
237 posMin = new Vector3(-0.5f, -0.5f, -0.5f);
238 }
239
240 ushort faceIndexOffset = (ushort)coords.Count;
241
242 byte[] posBytes = subMeshData["Position"].AsBinary();
243 for (int i = 0; i < posBytes.Length; i += 6)
244 {
245 ushort uX = Utils.BytesToUInt16(posBytes, i);
246 ushort uY = Utils.BytesToUInt16(posBytes, i + 2);
247 ushort uZ = Utils.BytesToUInt16(posBytes, i + 4);
248
249 Coord c = new Coord(
250 Utils.UInt16ToFloat(uX, posMin.X, posMax.X),
251 Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y),
252 Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z));
253
254 coords.Add(c);
255 }
256
257 byte[] triangleBytes = subMeshData["TriangleList"].AsBinary();
258 for (int i = 0; i < triangleBytes.Length; i += 6)
259 {
260 ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset);
261 ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset);
262 ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset);
263 Face f = new Face(v1, v2, v3);
264 faces.Add(f);
265 }
266 }
267
268 /// <summary>
269 /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type.
270 /// </summary>
271 /// <param name="primName"></param>
272 /// <param name="primShape"></param>
273 /// <param name="size"></param>
274 /// <param name="lod"></param>
275 /// <returns></returns>
276 private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex)
277 {
278// m_log.DebugFormat(
279// "[MESH]: Creating physics proxy for {0}, shape {1}",
280// primName, (OpenMetaverse.SculptType)primShape.SculptType);
281
282 List<Coord> coords;
283 List<Face> faces;
284
285 if (primShape.SculptEntry)
286 {
287 if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh)
288 {
289 if (!useMeshiesPhysicsMesh)
290 return null;
291
292 if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex))
293 return null;
294 }
295 else
296 {
297 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces))
298 return null;
299 }
300 }
301 else
302 {
303 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, out coords, out faces))
304 return null;
305 }
306
307 primShape.SculptData = Utils.EmptyBytes;
308
309 int numCoords = coords.Count;
310 int numFaces = faces.Count;
311
312 Mesh mesh = new Mesh();
313 // Add the corresponding triangles to the mesh
314 for (int i = 0; i < numFaces; i++)
315 {
316 Face f = faces[i];
317 mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z,
318 coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z,
319 coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z));
320 }
321
322 coords.Clear();
323 faces.Clear();
324
325 return mesh;
326 }
327
328 /// <summary>
329 /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim.
330 /// </summary>
331 /// <param name="primName"></param>
332 /// <param name="primShape"></param>
333 /// <param name="size"></param>
334 /// <param name="coords">Coords are added to this list by the method.</param>
335 /// <param name="faces">Faces are added to this list by the method.</param>
336 /// <returns>true if coords and faces were successfully generated, false if not</returns>
337 private bool GenerateCoordsAndFacesFromPrimMeshData(
338 string primName, PrimitiveBaseShape primShape, out List<Coord> coords, out List<Face> faces, bool convex)
339 {
340// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName);
341
342 bool usemesh = false;
343
344 coords = new List<Coord>();
345 faces = new List<Face>();
346 OSD meshOsd = null;
347
348 if (primShape.SculptData.Length <= 0)
349 {
350// m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName);
351 return false;
352 }
353
354 long start = 0;
355 using (MemoryStream data = new MemoryStream(primShape.SculptData))
356 {
357 try
358 {
359 OSD osd = OSDParser.DeserializeLLSDBinary(data);
360 if (osd is OSDMap)
361 meshOsd = (OSDMap)osd;
362 else
363 {
364 m_log.Warn("[Mesh}: unable to cast mesh asset to OSDMap");
365 return false;
366 }
367 }
368 catch (Exception e)
369 {
370 m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString());
371 }
372
373 start = data.Position;
374 }
375
376 if (meshOsd is OSDMap)
377 {
378 OSDMap physicsParms = null;
379 OSDMap map = (OSDMap)meshOsd;
380
381 if (!convex)
382 {
383 if (map.ContainsKey("physics_shape"))
384 physicsParms = (OSDMap)map["physics_shape"]; // old asset format
385 else if (map.ContainsKey("physics_mesh"))
386 physicsParms = (OSDMap)map["physics_mesh"]; // new asset format
387
388 if (physicsParms != null)
389 usemesh = true;
390 }
391
392 if(!usemesh && (map.ContainsKey("physics_convex")))
393 physicsParms = (OSDMap)map["physics_convex"];
394
395
396 if (physicsParms == null)
397 {
398 m_log.Warn("[MESH]: unknown mesh type");
399 return false;
400 }
401
402 int physOffset = physicsParms["offset"].AsInteger() + (int)start;
403 int physSize = physicsParms["size"].AsInteger();
404
405 if (physOffset < 0 || physSize == 0)
406 return false; // no mesh data in asset
407
408 OSD decodedMeshOsd = new OSD();
409 byte[] meshBytes = new byte[physSize];
410 System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize);
411
412 try
413 {
414 using (MemoryStream inMs = new MemoryStream(meshBytes))
415 {
416 using (MemoryStream outMs = new MemoryStream())
417 {
418 using (ZOutputStream zOut = new ZOutputStream(outMs))
419 {
420 byte[] readBuffer = new byte[2048];
421 int readLen = 0;
422 while ((readLen = inMs.Read(readBuffer, 0, readBuffer.Length)) > 0)
423 {
424 zOut.Write(readBuffer, 0, readLen);
425 }
426 zOut.Flush();
427 outMs.Seek(0, SeekOrigin.Begin);
428
429 byte[] decompressedBuf = outMs.GetBuffer();
430
431 decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf);
432 }
433 }
434 }
435 }
436 catch (Exception e)
437 {
438 m_log.Error("[MESH]: exception decoding physical mesh: " + e.ToString());
439 return false;
440 }
441
442 if (usemesh)
443 {
444 OSDArray decodedMeshOsdArray = null;
445
446 // physics_shape is an array of OSDMaps, one for each submesh
447 if (decodedMeshOsd is OSDArray)
448 {
449// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd));
450
451 decodedMeshOsdArray = (OSDArray)decodedMeshOsd;
452 foreach (OSD subMeshOsd in decodedMeshOsdArray)
453 {
454 if (subMeshOsd is OSDMap)
455 AddSubMesh(subMeshOsd as OSDMap, coords, faces);
456 }
457 }
458 }
459 else
460 {
461 OSDMap cmap = (OSDMap)decodedMeshOsd;
462 if (cmap == null)
463 return false;
464
465 byte[] data;
466
467 List<float3> vs = new List<float3>();
468 PHullResult hullr = new PHullResult();
469 float3 f3;
470 Coord c;
471 Face f;
472 Vector3 range;
473 Vector3 min;
474
475 const float invMaxU16 = 1.0f / 65535f;
476 int t1;
477 int t2;
478 int t3;
479 int i;
480 int nverts;
481 int nindexs;
482
483 if (cmap.ContainsKey("Max"))
484 range = cmap["Max"].AsVector3();
485 else
486 range = new Vector3(0.5f, 0.5f, 0.5f);
487
488 if (cmap.ContainsKey("Min"))
489 min = cmap["Min"].AsVector3();
490 else
491 min = new Vector3(-0.5f, -0.5f, -0.5f);
492
493 range = range - min;
494 range *= invMaxU16;
495
496 if (!convex && cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions"))
497 {
498 List<int> hsizes = new List<int>();
499 int totalpoints = 0;
500 data = cmap["HullList"].AsBinary();
501 for (i = 0; i < data.Length; i++)
502 {
503 t1 = data[i];
504 if (t1 == 0)
505 t1 = 256;
506 totalpoints += t1;
507 hsizes.Add(t1);
508 }
509
510 data = cmap["Positions"].AsBinary();
511 int ptr = 0;
512 int vertsoffset = 0;
513
514 if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point
515 {
516 foreach (int hullsize in hsizes)
517 {
518 for (i = 0; i < hullsize; i++ )
519 {
520 t1 = data[ptr++];
521 t1 += data[ptr++] << 8;
522 t2 = data[ptr++];
523 t2 += data[ptr++] << 8;
524 t3 = data[ptr++];
525 t3 += data[ptr++] << 8;
526
527 f3 = new float3((t1 * range.X + min.X),
528 (t2 * range.Y + min.Y),
529 (t3 * range.Z + min.Z));
530 vs.Add(f3);
531 }
532
533 if(hullsize <3)
534 {
535 vs.Clear();
536 continue;
537 }
538
539 if (hullsize <5)
540 {
541 foreach (float3 point in vs)
542 {
543 c.X = point.x;
544 c.Y = point.y;
545 c.Z = point.z;
546 coords.Add(c);
547 }
548 f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2);
549 faces.Add(f);
550
551 if (hullsize == 4)
552 {
553 // not sure about orientation..
554 f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3);
555 faces.Add(f);
556 f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1);
557 faces.Add(f);
558 f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1);
559 faces.Add(f);
560 }
561 vertsoffset += vs.Count;
562 vs.Clear();
563 continue;
564 }
565
566 if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f))
567 {
568 vs.Clear();
569 continue;
570 }
571
572 nverts = hullr.Vertices.Count;
573 nindexs = hullr.Indices.Count;
574
575 if (nindexs % 3 != 0)
576 {
577 vs.Clear();
578 continue;
579 }
580
581 for (i = 0; i < nverts; i++)
582 {
583 c.X = hullr.Vertices[i].x;
584 c.Y = hullr.Vertices[i].y;
585 c.Z = hullr.Vertices[i].z;
586 coords.Add(c);
587 }
588
589 for (i = 0; i < nindexs; i += 3)
590 {
591 t1 = hullr.Indices[i];
592 if (t1 > nverts)
593 break;
594 t2 = hullr.Indices[i + 1];
595 if (t2 > nverts)
596 break;
597 t3 = hullr.Indices[i + 2];
598 if (t3 > nverts)
599 break;
600 f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3);
601 faces.Add(f);
602 }
603 vertsoffset += nverts;
604 vs.Clear();
605 }
606 }
607 if (coords.Count > 0 && faces.Count > 0)
608 return true;
609 }
610
611 vs.Clear();
612
613 if (cmap.ContainsKey("BoundingVerts"))
614 {
615 data = cmap["BoundingVerts"].AsBinary();
616
617 for (i = 0; i < data.Length; )
618 {
619 t1 = data[i++];
620 t1 += data[i++] << 8;
621 t2 = data[i++];
622 t2 += data[i++] << 8;
623 t3 = data[i++];
624 t3 += data[i++] << 8;
625
626 f3 = new float3((t1 * range.X + min.X),
627 (t2 * range.Y + min.Y),
628 (t3 * range.Z + min.Z));
629 vs.Add(f3);
630 }
631
632 if (vs.Count < 3)
633 {
634 vs.Clear();
635 return false;
636 }
637
638 if (vs.Count < 5)
639 {
640 foreach (float3 point in vs)
641 {
642 c.X = point.x;
643 c.Y = point.y;
644 c.Z = point.z;
645 coords.Add(c);
646 }
647 f = new Face(0, 1, 2);
648 faces.Add(f);
649
650 if (vs.Count == 4)
651 {
652 f = new Face(0, 2, 3);
653 faces.Add(f);
654 f = new Face(0, 3, 1);
655 faces.Add(f);
656 f = new Face( 3, 2, 1);
657 faces.Add(f);
658 }
659 vs.Clear();
660 return true;
661 }
662
663 if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f))
664 return false;
665
666 nverts = hullr.Vertices.Count;
667 nindexs = hullr.Indices.Count;
668
669 if (nindexs % 3 != 0)
670 return false;
671
672 for (i = 0; i < nverts; i++)
673 {
674 c.X = hullr.Vertices[i].x;
675 c.Y = hullr.Vertices[i].y;
676 c.Z = hullr.Vertices[i].z;
677 coords.Add(c);
678 }
679 for (i = 0; i < nindexs; i += 3)
680 {
681 t1 = hullr.Indices[i];
682 if (t1 > nverts)
683 break;
684 t2 = hullr.Indices[i + 1];
685 if (t2 > nverts)
686 break;
687 t3 = hullr.Indices[i + 2];
688 if (t3 > nverts)
689 break;
690 f = new Face(t1, t2, t3);
691 faces.Add(f);
692 }
693
694 if (coords.Count > 0 && faces.Count > 0)
695 return true;
696 }
697 else
698 return false;
699 }
700 }
701
702 return true;
703 }
704
705 /// <summary>
706 /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim.
707 /// </summary>
708 /// <param name="primName"></param>
709 /// <param name="primShape"></param>
710 /// <param name="size"></param>
711 /// <param name="lod"></param>
712 /// <param name="coords">Coords are added to this list by the method.</param>
713 /// <param name="faces">Faces are added to this list by the method.</param>
714 /// <returns>true if coords and faces were successfully generated, false if not</returns>
715 private bool GenerateCoordsAndFacesFromPrimSculptData(
716 string primName, PrimitiveBaseShape primShape, float lod, out List<Coord> coords, out List<Face> faces)
717 {
718 coords = new List<Coord>();
719 faces = new List<Face>();
720 PrimMesher.SculptMesh sculptMesh;
721 Image idata = null;
722
723 if (primShape.SculptData == null || primShape.SculptData.Length == 0)
724 return false;
725
726 try
727 {
728 OpenMetaverse.Imaging.ManagedImage unusedData;
729 OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata);
730
731 unusedData = null;
732
733 if (idata == null)
734 {
735 // In some cases it seems that the decode can return a null bitmap without throwing
736 // an exception
737 m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName);
738 return false;
739 }
740 }
741 catch (DllNotFoundException)
742 {
743 m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed. Often times this is because of an old version of GLIBC. You must have version 2.4 or above!");
744 return false;
745 }
746 catch (IndexOutOfRangeException)
747 {
748 m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed");
749 return false;
750 }
751 catch (Exception ex)
752 {
753 m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message);
754 return false;
755 }
756
757 PrimMesher.SculptMesh.SculptType sculptType;
758 // remove mirror and invert bits
759 OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f));
760 switch (pbsSculptType)
761 {
762 case OpenMetaverse.SculptType.Cylinder:
763 sculptType = PrimMesher.SculptMesh.SculptType.cylinder;
764 break;
765 case OpenMetaverse.SculptType.Plane:
766 sculptType = PrimMesher.SculptMesh.SculptType.plane;
767 break;
768 case OpenMetaverse.SculptType.Torus:
769 sculptType = PrimMesher.SculptMesh.SculptType.torus;
770 break;
771 case OpenMetaverse.SculptType.Sphere:
772 sculptType = PrimMesher.SculptMesh.SculptType.sphere;
773 break;
774 default:
775 sculptType = PrimMesher.SculptMesh.SculptType.plane;
776 break;
777 }
778
779 bool mirror = ((primShape.SculptType & 128) != 0);
780 bool invert = ((primShape.SculptType & 64) != 0);
781
782 sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert);
783
784 idata.Dispose();
785
786// sculptMesh.DumpRaw(baseDir, primName, "primMesh");
787
788 coords = sculptMesh.coords;
789 faces = sculptMesh.faces;
790
791 return true;
792 }
793
794 /// <summary>
795 /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim.
796 /// </summary>
797 /// <param name="primName"></param>
798 /// <param name="primShape"></param>
799 /// <param name="size"></param>
800 /// <param name="coords">Coords are added to this list by the method.</param>
801 /// <param name="faces">Faces are added to this list by the method.</param>
802 /// <returns>true if coords and faces were successfully generated, false if not</returns>
803 private bool GenerateCoordsAndFacesFromPrimShapeData(
804 string primName, PrimitiveBaseShape primShape, float lod, out List<Coord> coords, out List<Face> faces)
805 {
806 PrimMesh primMesh;
807 coords = new List<Coord>();
808 faces = new List<Face>();
809
810 float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f;
811 float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f;
812 float pathBegin = (float)primShape.PathBegin * 2.0e-5f;
813 float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f;
814 float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f;
815 float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f;
816
817 float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f;
818 float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f;
819 float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f;
820 if (profileHollow > 0.95f)
821 profileHollow = 0.95f;
822
823 int sides = 4;
824 LevelOfDetail iLOD = (LevelOfDetail)lod;
825 if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
826 sides = 3;
827 else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
828 {
829 switch (iLOD)
830 {
831 case LevelOfDetail.High: sides = 24; break;
832 case LevelOfDetail.Medium: sides = 12; break;
833 case LevelOfDetail.Low: sides = 6; break;
834 case LevelOfDetail.VeryLow: sides = 3; break;
835 default: sides = 24; break;
836 }
837 }
838 else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
839 { // half circle, prim is a sphere
840 switch (iLOD)
841 {
842 case LevelOfDetail.High: sides = 24; break;
843 case LevelOfDetail.Medium: sides = 12; break;
844 case LevelOfDetail.Low: sides = 6; break;
845 case LevelOfDetail.VeryLow: sides = 3; break;
846 default: sides = 24; break;
847 }
848
849 profileBegin = 0.5f * profileBegin + 0.5f;
850 profileEnd = 0.5f * profileEnd + 0.5f;
851 }
852
853 int hollowSides = sides;
854 if (primShape.HollowShape == HollowShape.Circle)
855 {
856 switch (iLOD)
857 {
858 case LevelOfDetail.High: hollowSides = 24; break;
859 case LevelOfDetail.Medium: hollowSides = 12; break;
860 case LevelOfDetail.Low: hollowSides = 6; break;
861 case LevelOfDetail.VeryLow: hollowSides = 3; break;
862 default: hollowSides = 24; break;
863 }
864 }
865 else if (primShape.HollowShape == HollowShape.Square)
866 hollowSides = 4;
867 else if (primShape.HollowShape == HollowShape.Triangle)
868 hollowSides = 3;
869
870 primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides);
871
872 if (primMesh.errorMessage != null)
873 if (primMesh.errorMessage.Length > 0)
874 m_log.Error("[ERROR] " + primMesh.errorMessage);
875
876 primMesh.topShearX = pathShearX;
877 primMesh.topShearY = pathShearY;
878 primMesh.pathCutBegin = pathBegin;
879 primMesh.pathCutEnd = pathEnd;
880
881 if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible)
882 {
883 primMesh.twistBegin = primShape.PathTwistBegin * 18 / 10;
884 primMesh.twistEnd = primShape.PathTwist * 18 / 10;
885 primMesh.taperX = pathScaleX;
886 primMesh.taperY = pathScaleY;
887
888 if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f)
889 {
890 ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh);
891 if (profileBegin < 0.0f) profileBegin = 0.0f;
892 if (profileEnd > 1.0f) profileEnd = 1.0f;
893 }
894#if SPAM
895 m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString());
896#endif
897 try
898 {
899 primMesh.ExtrudeLinear();
900 }
901 catch (Exception ex)
902 {
903 ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
904 return false;
905 }
906 }
907 else
908 {
909 primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f;
910 primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f;
911 primMesh.radius = 0.01f * primShape.PathRadiusOffset;
912 primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions;
913 primMesh.skew = 0.01f * primShape.PathSkew;
914 primMesh.twistBegin = primShape.PathTwistBegin * 36 / 10;
915 primMesh.twistEnd = primShape.PathTwist * 36 / 10;
916 primMesh.taperX = primShape.PathTaperX * 0.01f;
917 primMesh.taperY = primShape.PathTaperY * 0.01f;
918
919 if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f)
920 {
921 ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh);
922 if (profileBegin < 0.0f) profileBegin = 0.0f;
923 if (profileEnd > 1.0f) profileEnd = 1.0f;
924 }
925#if SPAM
926 m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString());
927#endif
928 try
929 {
930 primMesh.ExtrudeCircular();
931 }
932 catch (Exception ex)
933 {
934 ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
935 return false;
936 }
937 }
938
939// primMesh.DumpRaw(baseDir, primName, "primMesh");
940
941 coords = primMesh.coords;
942 faces = primMesh.faces;
943
944 return true;
945 }
946
947 public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex)
948 {
949 AMeshKey key = new AMeshKey();
950 Byte[] someBytes;
951
952 key.hashB = 5181;
953 key.hashC = 5181;
954 ulong hash = 5381;
955
956 if (primShape.SculptEntry)
957 {
958 key.uuid = primShape.SculptTexture;
959 key.hashC = mdjb2(key.hashC, primShape.SculptType);
960 key.hashC = mdjb2(key.hashC, primShape.PCode);
961 }
962 else
963 {
964 hash = mdjb2(hash, primShape.PathCurve);
965 hash = mdjb2(hash, (byte)primShape.HollowShape);
966 hash = mdjb2(hash, (byte)primShape.ProfileShape);
967 hash = mdjb2(hash, primShape.PathBegin);
968 hash = mdjb2(hash, primShape.PathEnd);
969 hash = mdjb2(hash, primShape.PathScaleX);
970 hash = mdjb2(hash, primShape.PathScaleY);
971 hash = mdjb2(hash, primShape.PathShearX);
972 key.hashA = hash;
973 hash = key.hashB;
974 hash = mdjb2(hash, primShape.PathShearY);
975 hash = mdjb2(hash, (byte)primShape.PathTwist);
976 hash = mdjb2(hash, (byte)primShape.PathTwistBegin);
977 hash = mdjb2(hash, (byte)primShape.PathRadiusOffset);
978 hash = mdjb2(hash, (byte)primShape.PathTaperX);
979 hash = mdjb2(hash, (byte)primShape.PathTaperY);
980 hash = mdjb2(hash, primShape.PathRevolutions);
981 hash = mdjb2(hash, (byte)primShape.PathSkew);
982 hash = mdjb2(hash, primShape.ProfileBegin);
983 hash = mdjb2(hash, primShape.ProfileEnd);
984 hash = mdjb2(hash, primShape.ProfileHollow);
985 hash = mdjb2(hash, primShape.PCode);
986 key.hashB = hash;
987 }
988
989 hash = key.hashC;
990
991 hash = mdjb2(hash, lod);
992
993 if (size == m_MeshUnitSize)
994 {
995 hash = hash << 8;
996 hash |= 8;
997 }
998 else
999 {
1000 someBytes = size.GetBytes();
1001 for (int i = 0; i < someBytes.Length; i++)
1002 hash = mdjb2(hash, someBytes[i]);
1003 hash = hash << 8;
1004 }
1005
1006 if (convex)
1007 hash |= 4;
1008
1009 if (primShape.SculptEntry)
1010 {
1011 hash |= 1;
1012 if (primShape.SculptType == (byte)SculptType.Mesh)
1013 hash |= 2;
1014 }
1015
1016 key.hashC = hash;
1017
1018 return key;
1019 }
1020
1021 private ulong mdjb2(ulong hash, byte c)
1022 {
1023 return ((hash << 5) + hash) + (ulong)c;
1024 }
1025
1026 private ulong mdjb2(ulong hash, ushort c)
1027 {
1028 hash = ((hash << 5) + hash) + (ulong)((byte)c);
1029 return ((hash << 5) + hash) + (ulong)(c >> 8);
1030 }
1031
1032 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
1033 {
1034 return CreateMesh(primName, primShape, size, lod, false,false,false,false);
1035 }
1036
1037 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
1038 {
1039 return CreateMesh(primName, primShape, size, lod, false,false,false,false);
1040 }
1041
1042 public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
1043 {
1044 Mesh mesh = null;
1045
1046 if (size.X < 0.01f) size.X = 0.01f;
1047 if (size.Y < 0.01f) size.Y = 0.01f;
1048 if (size.Z < 0.01f) size.Z = 0.01f;
1049
1050 AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex);
1051 lock (m_uniqueMeshes)
1052 {
1053 m_uniqueMeshes.TryGetValue(key, out mesh);
1054
1055 if (mesh != null)
1056 {
1057 mesh.RefCount++;
1058 return mesh;
1059 }
1060
1061 // try to find a identical mesh on meshs recently released
1062 lock (m_uniqueReleasedMeshes)
1063 {
1064 m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
1065 if (mesh != null)
1066 {
1067 m_uniqueReleasedMeshes.Remove(key);
1068 try
1069 {
1070 m_uniqueMeshes.Add(key, mesh);
1071 }
1072 catch { }
1073 mesh.RefCount = 1;
1074 return mesh;
1075 }
1076 }
1077 }
1078 return null;
1079 }
1080
1081 private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f);
1082
1083 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde)
1084 {
1085#if SPAM
1086 m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
1087#endif
1088
1089 Mesh mesh = null;
1090
1091 if (size.X < 0.01f) size.X = 0.01f;
1092 if (size.Y < 0.01f) size.Y = 0.01f;
1093 if (size.Z < 0.01f) size.Z = 0.01f;
1094
1095 // try to find a identical mesh on meshs in use
1096
1097 AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex);
1098
1099 lock (m_uniqueMeshes)
1100 {
1101 m_uniqueMeshes.TryGetValue(key, out mesh);
1102
1103 if (mesh != null)
1104 {
1105 mesh.RefCount++;
1106 return mesh;
1107 }
1108
1109 // try to find a identical mesh on meshs recently released
1110 lock (m_uniqueReleasedMeshes)
1111 {
1112 m_uniqueReleasedMeshes.TryGetValue(key, out mesh);
1113 if (mesh != null)
1114 {
1115 m_uniqueReleasedMeshes.Remove(key);
1116 try
1117 {
1118 m_uniqueMeshes.Add(key, mesh);
1119 }
1120 catch { }
1121 mesh.RefCount = 1;
1122 return mesh;
1123 }
1124 }
1125 }
1126
1127 Mesh UnitMesh = null;
1128 AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex);
1129
1130 lock (m_uniqueReleasedMeshes)
1131 {
1132 m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh);
1133 if (UnitMesh != null)
1134 {
1135 UnitMesh.RefCount = 1;
1136 }
1137 }
1138
1139 if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache)
1140 UnitMesh = GetFromFileCache(unitKey);
1141
1142 if (UnitMesh == null)
1143 {
1144 UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex);
1145
1146 if (UnitMesh == null)
1147 return null;
1148
1149 UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z");
1150
1151 if (forOde)
1152 {
1153 // force pinned mem allocation
1154 UnitMesh.PrepForOde();
1155 }
1156 else
1157 UnitMesh.TrimExcess();
1158
1159 UnitMesh.Key = unitKey;
1160 UnitMesh.RefCount = 1;
1161
1162 if (doMeshFileCache && primShape.SculptEntry)
1163 StoreToFileCache(unitKey, UnitMesh);
1164
1165 lock (m_uniqueReleasedMeshes)
1166 {
1167 try
1168 {
1169 m_uniqueReleasedMeshes.Add(unitKey, UnitMesh);
1170 }
1171 catch { }
1172 }
1173 }
1174
1175 mesh = UnitMesh.Scale(size);
1176 mesh.Key = key;
1177 mesh.RefCount = 1;
1178 lock (m_uniqueMeshes)
1179 {
1180 try
1181 {
1182 m_uniqueMeshes.Add(key, mesh);
1183 }
1184 catch { }
1185 }
1186
1187 return mesh;
1188 }
1189
1190 public void ReleaseMesh(IMesh imesh)
1191 {
1192 if (imesh == null)
1193 return;
1194
1195 Mesh mesh = (Mesh)imesh;
1196
1197 lock (m_uniqueMeshes)
1198 {
1199 int curRefCount = mesh.RefCount;
1200 curRefCount--;
1201
1202 if (curRefCount > 0)
1203 {
1204 mesh.RefCount = curRefCount;
1205 return;
1206 }
1207
1208 mesh.RefCount = 0;
1209 m_uniqueMeshes.Remove(mesh.Key);
1210 lock (m_uniqueReleasedMeshes)
1211 {
1212 try
1213 {
1214 m_uniqueReleasedMeshes.Add(mesh.Key, mesh);
1215 }
1216 catch { }
1217 }
1218 }
1219 }
1220
1221 public void ExpireReleaseMeshs()
1222 {
1223 if (m_uniqueReleasedMeshes.Count == 0)
1224 return;
1225
1226 List<Mesh> meshstodelete = new List<Mesh>();
1227 int refcntr;
1228
1229 lock (m_uniqueReleasedMeshes)
1230 {
1231 foreach (Mesh m in m_uniqueReleasedMeshes.Values)
1232 {
1233 refcntr = m.RefCount;
1234 refcntr--;
1235 if (refcntr > -6)
1236 m.RefCount = refcntr;
1237 else
1238 meshstodelete.Add(m);
1239 }
1240
1241 foreach (Mesh m in meshstodelete)
1242 {
1243 m_uniqueReleasedMeshes.Remove(m.Key);
1244 m.releaseBuildingMeshData();
1245 m.releasePinned();
1246 }
1247 }
1248 }
1249
1250 public void FileNames(AMeshKey key, out string dir,out string fullFileName)
1251 {
1252 string id = key.ToString();
1253 string init = id.Substring(0, 1);
1254 dir = System.IO.Path.Combine(cachePath, init);
1255 fullFileName = System.IO.Path.Combine(dir, id);
1256 }
1257
1258 public string FullFileName(AMeshKey key)
1259 {
1260 string id = key.ToString();
1261 string init = id.Substring(0,1);
1262 id = System.IO.Path.Combine(init, id);
1263 id = System.IO.Path.Combine(cachePath, id);
1264 return id;
1265 }
1266
1267 private Mesh GetFromFileCache(AMeshKey key)
1268 {
1269 Mesh mesh = null;
1270 string filename = FullFileName(key);
1271 bool ok = true;
1272
1273 lock (diskLock)
1274 {
1275 if (File.Exists(filename))
1276 {
1277 FileStream stream = null;
1278 try
1279 {
1280 stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read);
1281 BinaryFormatter bformatter = new BinaryFormatter();
1282
1283 mesh = Mesh.FromStream(stream, key);
1284
1285 }
1286 catch (Exception e)
1287 {
1288 ok = false;
1289 m_log.ErrorFormat(
1290 "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}",
1291 filename, e.Message, e.StackTrace);
1292 }
1293
1294 if (stream != null)
1295 stream.Close();
1296
1297 if (mesh == null || !ok)
1298 File.Delete(filename);
1299 else
1300 File.SetLastAccessTimeUtc(filename, DateTime.UtcNow);
1301 }
1302 }
1303
1304 return mesh;
1305 }
1306
1307 private void StoreToFileCache(AMeshKey key, Mesh mesh)
1308 {
1309 Stream stream = null;
1310 bool ok = false;
1311
1312 // Make sure the target cache directory exists
1313 string dir = String.Empty;
1314 string filename = String.Empty;
1315
1316 FileNames(key, out dir, out filename);
1317
1318 lock (diskLock)
1319 {
1320 try
1321 {
1322 if (!Directory.Exists(dir))
1323 {
1324 Directory.CreateDirectory(dir);
1325 }
1326
1327 stream = File.Open(filename, FileMode.Create);
1328 ok = mesh.ToStream(stream);
1329 }
1330 catch (IOException e)
1331 {
1332 m_log.ErrorFormat(
1333 "[MESH CACHE]: Failed to write file {0}. Exception {1} {2}.",
1334 filename, e.Message, e.StackTrace);
1335 ok = false;
1336 }
1337
1338 if (stream != null)
1339 stream.Close();
1340
1341 if (File.Exists(filename))
1342 {
1343 if (ok)
1344 File.SetLastAccessTimeUtc(filename, DateTime.UtcNow);
1345 else
1346 File.Delete(filename);
1347 }
1348 }
1349 }
1350
1351 public void ExpireFileCache()
1352 {
1353 if (!doCacheExpire)
1354 return;
1355
1356 string controlfile = System.IO.Path.Combine(cachePath, "cntr");
1357
1358 lock (diskLock)
1359 {
1360 try
1361 {
1362 if (File.Exists(controlfile))
1363 {
1364 int ndeleted = 0;
1365 int totalfiles = 0;
1366 int ndirs = 0;
1367 DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire;
1368 File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow);
1369
1370 foreach (string dir in Directory.GetDirectories(cachePath))
1371 {
1372 try
1373 {
1374 foreach (string file in Directory.GetFiles(dir))
1375 {
1376 try
1377 {
1378 if (File.GetLastAccessTimeUtc(file) < OlderTime)
1379 {
1380 File.Delete(file);
1381 ndeleted++;
1382 }
1383 }
1384 catch { }
1385 totalfiles++;
1386 }
1387 }
1388 catch { }
1389 ndirs++;
1390 }
1391
1392 if (ndeleted == 0)
1393 m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires",
1394 totalfiles,ndirs);
1395 else
1396 m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}",
1397 totalfiles,ndirs, ndeleted, OlderTime.ToString());
1398 }
1399 else
1400 {
1401 m_log.Info("[MESH CACHE]: Expire delayed to next startup");
1402 FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough);
1403 fs.Close();
1404 }
1405 }
1406 catch { }
1407 }
1408 }
1409 }
1410}
diff --git a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs
new file mode 100644
index 0000000..4049ee1
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs
@@ -0,0 +1,2324 @@
1/*
2 * Copyright (c) Contributors
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.Text;
31using System.IO;
32
33namespace PrimMesher
34{
35 public struct Quat
36 {
37 /// <summary>X value</summary>
38 public float X;
39 /// <summary>Y value</summary>
40 public float Y;
41 /// <summary>Z value</summary>
42 public float Z;
43 /// <summary>W value</summary>
44 public float W;
45
46 public Quat(float x, float y, float z, float w)
47 {
48 X = x;
49 Y = y;
50 Z = z;
51 W = w;
52 }
53
54 public Quat(Coord axis, float angle)
55 {
56 axis = axis.Normalize();
57
58 angle *= 0.5f;
59 float c = (float)Math.Cos(angle);
60 float s = (float)Math.Sin(angle);
61
62 X = axis.X * s;
63 Y = axis.Y * s;
64 Z = axis.Z * s;
65 W = c;
66
67 Normalize();
68 }
69
70 public float Length()
71 {
72 return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
73 }
74
75 public Quat Normalize()
76 {
77 const float MAG_THRESHOLD = 0.0000001f;
78 float mag = Length();
79
80 // Catch very small rounding errors when normalizing
81 if (mag > MAG_THRESHOLD)
82 {
83 float oomag = 1f / mag;
84 X *= oomag;
85 Y *= oomag;
86 Z *= oomag;
87 W *= oomag;
88 }
89 else
90 {
91 X = 0f;
92 Y = 0f;
93 Z = 0f;
94 W = 1f;
95 }
96
97 return this;
98 }
99
100 public static Quat operator *(Quat q1, Quat q2)
101 {
102 float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
103 float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
104 float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
105 float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
106 return new Quat(x, y, z, w);
107 }
108
109 public override string ToString()
110 {
111 return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">";
112 }
113 }
114
115 public struct Coord
116 {
117 public float X;
118 public float Y;
119 public float Z;
120
121 public Coord(float x, float y, float z)
122 {
123 this.X = x;
124 this.Y = y;
125 this.Z = z;
126 }
127
128 public float Length()
129 {
130 return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
131 }
132
133 public Coord Invert()
134 {
135 this.X = -this.X;
136 this.Y = -this.Y;
137 this.Z = -this.Z;
138
139 return this;
140 }
141
142 public Coord Normalize()
143 {
144 const float MAG_THRESHOLD = 0.0000001f;
145 float mag = Length();
146
147 // Catch very small rounding errors when normalizing
148 if (mag > MAG_THRESHOLD)
149 {
150 float oomag = 1.0f / mag;
151 this.X *= oomag;
152 this.Y *= oomag;
153 this.Z *= oomag;
154 }
155 else
156 {
157 this.X = 0.0f;
158 this.Y = 0.0f;
159 this.Z = 0.0f;
160 }
161
162 return this;
163 }
164
165 public override string ToString()
166 {
167 return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString();
168 }
169
170 public static Coord Cross(Coord c1, Coord c2)
171 {
172 return new Coord(
173 c1.Y * c2.Z - c2.Y * c1.Z,
174 c1.Z * c2.X - c2.Z * c1.X,
175 c1.X * c2.Y - c2.X * c1.Y
176 );
177 }
178
179 public static Coord operator +(Coord v, Coord a)
180 {
181 return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z);
182 }
183
184 public static Coord operator *(Coord v, Coord m)
185 {
186 return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z);
187 }
188
189 public static Coord operator *(Coord v, Quat q)
190 {
191 // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
192
193 Coord c2 = new Coord(0.0f, 0.0f, 0.0f);
194
195 c2.X = q.W * q.W * v.X +
196 2f * q.Y * q.W * v.Z -
197 2f * q.Z * q.W * v.Y +
198 q.X * q.X * v.X +
199 2f * q.Y * q.X * v.Y +
200 2f * q.Z * q.X * v.Z -
201 q.Z * q.Z * v.X -
202 q.Y * q.Y * v.X;
203
204 c2.Y =
205 2f * q.X * q.Y * v.X +
206 q.Y * q.Y * v.Y +
207 2f * q.Z * q.Y * v.Z +
208 2f * q.W * q.Z * v.X -
209 q.Z * q.Z * v.Y +
210 q.W * q.W * v.Y -
211 2f * q.X * q.W * v.Z -
212 q.X * q.X * v.Y;
213
214 c2.Z =
215 2f * q.X * q.Z * v.X +
216 2f * q.Y * q.Z * v.Y +
217 q.Z * q.Z * v.Z -
218 2f * q.W * q.Y * v.X -
219 q.Y * q.Y * v.Z +
220 2f * q.W * q.X * v.Y -
221 q.X * q.X * v.Z +
222 q.W * q.W * v.Z;
223
224 return c2;
225 }
226 }
227
228 public struct UVCoord
229 {
230 public float U;
231 public float V;
232
233
234 public UVCoord(float u, float v)
235 {
236 this.U = u;
237 this.V = v;
238 }
239
240 public UVCoord Flip()
241 {
242 this.U = 1.0f - this.U;
243 this.V = 1.0f - this.V;
244 return this;
245 }
246 }
247
248 public struct Face
249 {
250 public int primFace;
251
252 // vertices
253 public int v1;
254 public int v2;
255 public int v3;
256
257 //normals
258 public int n1;
259 public int n2;
260 public int n3;
261
262 // uvs
263 public int uv1;
264 public int uv2;
265 public int uv3;
266
267 public Face(int v1, int v2, int v3)
268 {
269 primFace = 0;
270
271 this.v1 = v1;
272 this.v2 = v2;
273 this.v3 = v3;
274
275 this.n1 = 0;
276 this.n2 = 0;
277 this.n3 = 0;
278
279 this.uv1 = 0;
280 this.uv2 = 0;
281 this.uv3 = 0;
282
283 }
284
285 public Face(int v1, int v2, int v3, int n1, int n2, int n3)
286 {
287 primFace = 0;
288
289 this.v1 = v1;
290 this.v2 = v2;
291 this.v3 = v3;
292
293 this.n1 = n1;
294 this.n2 = n2;
295 this.n3 = n3;
296
297 this.uv1 = 0;
298 this.uv2 = 0;
299 this.uv3 = 0;
300 }
301
302 public Coord SurfaceNormal(List<Coord> coordList)
303 {
304 Coord c1 = coordList[this.v1];
305 Coord c2 = coordList[this.v2];
306 Coord c3 = coordList[this.v3];
307
308 Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
309 Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
310
311 return Coord.Cross(edge1, edge2).Normalize();
312 }
313 }
314
315 public struct ViewerFace
316 {
317 public int primFaceNumber;
318
319 public Coord v1;
320 public Coord v2;
321 public Coord v3;
322
323 public int coordIndex1;
324 public int coordIndex2;
325 public int coordIndex3;
326
327 public Coord n1;
328 public Coord n2;
329 public Coord n3;
330
331 public UVCoord uv1;
332 public UVCoord uv2;
333 public UVCoord uv3;
334
335 public ViewerFace(int primFaceNumber)
336 {
337 this.primFaceNumber = primFaceNumber;
338
339 this.v1 = new Coord();
340 this.v2 = new Coord();
341 this.v3 = new Coord();
342
343 this.coordIndex1 = this.coordIndex2 = this.coordIndex3 = -1; // -1 means not assigned yet
344
345 this.n1 = new Coord();
346 this.n2 = new Coord();
347 this.n3 = new Coord();
348
349 this.uv1 = new UVCoord();
350 this.uv2 = new UVCoord();
351 this.uv3 = new UVCoord();
352 }
353
354 public void Scale(float x, float y, float z)
355 {
356 this.v1.X *= x;
357 this.v1.Y *= y;
358 this.v1.Z *= z;
359
360 this.v2.X *= x;
361 this.v2.Y *= y;
362 this.v2.Z *= z;
363
364 this.v3.X *= x;
365 this.v3.Y *= y;
366 this.v3.Z *= z;
367 }
368
369 public void AddPos(float x, float y, float z)
370 {
371 this.v1.X += x;
372 this.v2.X += x;
373 this.v3.X += x;
374
375 this.v1.Y += y;
376 this.v2.Y += y;
377 this.v3.Y += y;
378
379 this.v1.Z += z;
380 this.v2.Z += z;
381 this.v3.Z += z;
382 }
383
384 public void AddRot(Quat q)
385 {
386 this.v1 *= q;
387 this.v2 *= q;
388 this.v3 *= q;
389
390 this.n1 *= q;
391 this.n2 *= q;
392 this.n3 *= q;
393 }
394
395 public void CalcSurfaceNormal()
396 {
397
398 Coord edge1 = new Coord(this.v2.X - this.v1.X, this.v2.Y - this.v1.Y, this.v2.Z - this.v1.Z);
399 Coord edge2 = new Coord(this.v3.X - this.v1.X, this.v3.Y - this.v1.Y, this.v3.Z - this.v1.Z);
400
401 this.n1 = this.n2 = this.n3 = Coord.Cross(edge1, edge2).Normalize();
402 }
403 }
404
405 internal struct Angle
406 {
407 internal float angle;
408 internal float X;
409 internal float Y;
410
411 internal Angle(float angle, float x, float y)
412 {
413 this.angle = angle;
414 this.X = x;
415 this.Y = y;
416 }
417 }
418
419 internal class AngleList
420 {
421 private float iX, iY; // intersection point
422
423 private static Angle[] angles3 =
424 {
425 new Angle(0.0f, 1.0f, 0.0f),
426 new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
427 new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
428 new Angle(1.0f, 1.0f, 0.0f)
429 };
430
431 private static Coord[] normals3 =
432 {
433 new Coord(0.25f, 0.4330127019f, 0.0f).Normalize(),
434 new Coord(-0.5f, 0.0f, 0.0f).Normalize(),
435 new Coord(0.25f, -0.4330127019f, 0.0f).Normalize(),
436 new Coord(0.25f, 0.4330127019f, 0.0f).Normalize()
437 };
438
439 private static Angle[] angles4 =
440 {
441 new Angle(0.0f, 1.0f, 0.0f),
442 new Angle(0.25f, 0.0f, 1.0f),
443 new Angle(0.5f, -1.0f, 0.0f),
444 new Angle(0.75f, 0.0f, -1.0f),
445 new Angle(1.0f, 1.0f, 0.0f)
446 };
447
448 private static Coord[] normals4 =
449 {
450 new Coord(0.5f, 0.5f, 0.0f).Normalize(),
451 new Coord(-0.5f, 0.5f, 0.0f).Normalize(),
452 new Coord(-0.5f, -0.5f, 0.0f).Normalize(),
453 new Coord(0.5f, -0.5f, 0.0f).Normalize(),
454 new Coord(0.5f, 0.5f, 0.0f).Normalize()
455 };
456
457 private static Angle[] angles24 =
458 {
459 new Angle(0.0f, 1.0f, 0.0f),
460 new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f),
461 new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
462 new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f),
463 new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
464 new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f),
465 new Angle(0.25f, 0.0f, 1.0f),
466 new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f),
467 new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
468 new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f),
469 new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
470 new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f),
471 new Angle(0.5f, -1.0f, 0.0f),
472 new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f),
473 new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
474 new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f),
475 new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
476 new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f),
477 new Angle(0.75f, 0.0f, -1.0f),
478 new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f),
479 new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
480 new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f),
481 new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
482 new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f),
483 new Angle(1.0f, 1.0f, 0.0f)
484 };
485
486 private Angle interpolatePoints(float newPoint, Angle p1, Angle p2)
487 {
488 float m = (newPoint - p1.angle) / (p2.angle - p1.angle);
489 return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y));
490 }
491
492 private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
493 { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
494 double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
495 double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
496
497 if (denom != 0.0)
498 {
499 double ua = uaNumerator / denom;
500 iX = (float)(x1 + ua * (x2 - x1));
501 iY = (float)(y1 + ua * (y2 - y1));
502 }
503 }
504
505 internal List<Angle> angles;
506 internal List<Coord> normals;
507
508 internal void makeAngles(int sides, float startAngle, float stopAngle)
509 {
510 angles = new List<Angle>();
511 normals = new List<Coord>();
512
513 double twoPi = System.Math.PI * 2.0;
514 float twoPiInv = 1.0f / (float)twoPi;
515
516 if (sides < 1)
517 throw new Exception("number of sides not greater than zero");
518 if (stopAngle <= startAngle)
519 throw new Exception("stopAngle not greater than startAngle");
520
521 if ((sides == 3 || sides == 4 || sides == 24))
522 {
523 startAngle *= twoPiInv;
524 stopAngle *= twoPiInv;
525
526 Angle[] sourceAngles;
527 if (sides == 3)
528 sourceAngles = angles3;
529 else if (sides == 4)
530 sourceAngles = angles4;
531 else sourceAngles = angles24;
532
533 int startAngleIndex = (int)(startAngle * sides);
534 int endAngleIndex = sourceAngles.Length - 1;
535 if (stopAngle < 1.0f)
536 endAngleIndex = (int)(stopAngle * sides) + 1;
537 if (endAngleIndex == startAngleIndex)
538 endAngleIndex++;
539
540 for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++)
541 {
542 angles.Add(sourceAngles[angleIndex]);
543 if (sides == 3)
544 normals.Add(normals3[angleIndex]);
545 else if (sides == 4)
546 normals.Add(normals4[angleIndex]);
547 }
548
549 if (startAngle > 0.0f)
550 angles[0] = interpolatePoints(startAngle, angles[0], angles[1]);
551
552 if (stopAngle < 1.0f)
553 {
554 int lastAngleIndex = angles.Count - 1;
555 angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]);
556 }
557 }
558 else
559 {
560 double stepSize = twoPi / sides;
561
562 int startStep = (int)(startAngle / stepSize);
563 double angle = stepSize * startStep;
564 int step = startStep;
565 double stopAngleTest = stopAngle;
566 if (stopAngle < twoPi)
567 {
568 stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1);
569 if (stopAngleTest < stopAngle)
570 stopAngleTest += stepSize;
571 if (stopAngleTest > twoPi)
572 stopAngleTest = twoPi;
573 }
574
575 while (angle <= stopAngleTest)
576 {
577 Angle newAngle;
578 newAngle.angle = (float)angle;
579 newAngle.X = (float)System.Math.Cos(angle);
580 newAngle.Y = (float)System.Math.Sin(angle);
581 angles.Add(newAngle);
582 step += 1;
583 angle = stepSize * step;
584 }
585
586 if (startAngle > angles[0].angle)
587 {
588 Angle newAngle;
589 intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle));
590 newAngle.angle = startAngle;
591 newAngle.X = iX;
592 newAngle.Y = iY;
593 angles[0] = newAngle;
594 }
595
596 int index = angles.Count - 1;
597 if (stopAngle < angles[index].angle)
598 {
599 Angle newAngle;
600 intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle));
601 newAngle.angle = stopAngle;
602 newAngle.X = iX;
603 newAngle.Y = iY;
604 angles[index] = newAngle;
605 }
606 }
607 }
608 }
609
610 /// <summary>
611 /// generates a profile for extrusion
612 /// </summary>
613 public class Profile
614 {
615 private const float twoPi = 2.0f * (float)Math.PI;
616
617 public string errorMessage = null;
618
619 public List<Coord> coords;
620 public List<Face> faces;
621 public List<Coord> vertexNormals;
622 public List<float> us;
623 public List<UVCoord> faceUVs;
624 public List<int> faceNumbers;
625
626 // use these for making individual meshes for each prim face
627 public List<int> outerCoordIndices = null;
628 public List<int> hollowCoordIndices = null;
629 public List<int> cut1CoordIndices = null;
630 public List<int> cut2CoordIndices = null;
631
632 public Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f);
633 public Coord cutNormal1 = new Coord();
634 public Coord cutNormal2 = new Coord();
635
636 public int numOuterVerts = 0;
637 public int numHollowVerts = 0;
638
639 public int outerFaceNumber = -1;
640 public int hollowFaceNumber = -1;
641
642 public bool calcVertexNormals = false;
643 public int bottomFaceNumber = 0;
644 public int numPrimFaces = 0;
645
646 public Profile()
647 {
648 this.coords = new List<Coord>();
649 this.faces = new List<Face>();
650 this.vertexNormals = new List<Coord>();
651 this.us = new List<float>();
652 this.faceUVs = new List<UVCoord>();
653 this.faceNumbers = new List<int>();
654 }
655
656 public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals)
657 {
658 this.calcVertexNormals = calcVertexNormals;
659 this.coords = new List<Coord>();
660 this.faces = new List<Face>();
661 this.vertexNormals = new List<Coord>();
662 this.us = new List<float>();
663 this.faceUVs = new List<UVCoord>();
664 this.faceNumbers = new List<int>();
665
666 Coord center = new Coord(0.0f, 0.0f, 0.0f);
667
668 List<Coord> hollowCoords = new List<Coord>();
669 List<Coord> hollowNormals = new List<Coord>();
670 List<float> hollowUs = new List<float>();
671
672 if (calcVertexNormals)
673 {
674 this.outerCoordIndices = new List<int>();
675 this.hollowCoordIndices = new List<int>();
676 this.cut1CoordIndices = new List<int>();
677 this.cut2CoordIndices = new List<int>();
678 }
679
680 bool hasHollow = (hollow > 0.0f);
681
682 bool hasProfileCut = (profileStart > 0.0f || profileEnd < 1.0f);
683
684 AngleList angles = new AngleList();
685 AngleList hollowAngles = new AngleList();
686
687 float xScale = 0.5f;
688 float yScale = 0.5f;
689 if (sides == 4) // corners of a square are sqrt(2) from center
690 {
691 xScale = 0.707107f;
692 yScale = 0.707107f;
693 }
694
695 float startAngle = profileStart * twoPi;
696 float stopAngle = profileEnd * twoPi;
697
698 try { angles.makeAngles(sides, startAngle, stopAngle); }
699 catch (Exception ex)
700 {
701
702 errorMessage = "makeAngles failed: Exception: " + ex.ToString()
703 + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
704
705 return;
706 }
707
708 this.numOuterVerts = angles.angles.Count;
709
710 // flag to create as few triangles as possible for 3 or 4 side profile
711 bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut);
712
713 if (hasHollow)
714 {
715 if (sides == hollowSides)
716 hollowAngles = angles;
717 else
718 {
719 try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle); }
720 catch (Exception ex)
721 {
722 errorMessage = "makeAngles failed: Exception: " + ex.ToString()
723 + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
724
725 return;
726 }
727 }
728 this.numHollowVerts = hollowAngles.angles.Count;
729 }
730 else if (!simpleFace)
731 {
732 this.coords.Add(center);
733 if (this.calcVertexNormals)
734 this.vertexNormals.Add(new Coord(0.0f, 0.0f, 1.0f));
735 this.us.Add(0.0f);
736 }
737
738 float z = 0.0f;
739
740 Angle angle;
741 Coord newVert = new Coord();
742 if (hasHollow && hollowSides != sides)
743 {
744 int numHollowAngles = hollowAngles.angles.Count;
745 for (int i = 0; i < numHollowAngles; i++)
746 {
747 angle = hollowAngles.angles[i];
748 newVert.X = hollow * xScale * angle.X;
749 newVert.Y = hollow * yScale * angle.Y;
750 newVert.Z = z;
751
752 hollowCoords.Add(newVert);
753 if (this.calcVertexNormals)
754 {
755 if (hollowSides < 5)
756 hollowNormals.Add(hollowAngles.normals[i].Invert());
757 else
758 hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f));
759
760 if (hollowSides == 4)
761 hollowUs.Add(angle.angle * hollow * 0.707107f);
762 else
763 hollowUs.Add(angle.angle * hollow);
764 }
765 }
766 }
767
768 int index = 0;
769 int numAngles = angles.angles.Count;
770
771 for (int i = 0; i < numAngles; i++)
772 {
773 angle = angles.angles[i];
774 newVert.X = angle.X * xScale;
775 newVert.Y = angle.Y * yScale;
776 newVert.Z = z;
777 this.coords.Add(newVert);
778 if (this.calcVertexNormals)
779 {
780 this.outerCoordIndices.Add(this.coords.Count - 1);
781
782 if (sides < 5)
783 {
784 this.vertexNormals.Add(angles.normals[i]);
785 float u = angle.angle;
786 this.us.Add(u);
787 }
788 else
789 {
790 this.vertexNormals.Add(new Coord(angle.X, angle.Y, 0.0f));
791 this.us.Add(angle.angle);
792 }
793 }
794
795 if (hasHollow)
796 {
797 if (hollowSides == sides)
798 {
799 newVert.X *= hollow;
800 newVert.Y *= hollow;
801 newVert.Z = z;
802 hollowCoords.Add(newVert);
803 if (this.calcVertexNormals)
804 {
805 if (sides < 5)
806 {
807 hollowNormals.Add(angles.normals[i].Invert());
808 }
809
810 else
811 hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f));
812
813 hollowUs.Add(angle.angle * hollow);
814 }
815 }
816 }
817 else if (!simpleFace && createFaces && angle.angle > 0.0001f)
818 {
819 Face newFace = new Face();
820 newFace.v1 = 0;
821 newFace.v2 = index;
822 newFace.v3 = index + 1;
823
824 this.faces.Add(newFace);
825 }
826 index += 1;
827 }
828
829 if (hasHollow)
830 {
831 hollowCoords.Reverse();
832 if (this.calcVertexNormals)
833 {
834 hollowNormals.Reverse();
835 hollowUs.Reverse();
836 }
837
838 if (createFaces)
839 {
840 int numTotalVerts = this.numOuterVerts + this.numHollowVerts;
841
842 if (this.numOuterVerts == this.numHollowVerts)
843 {
844 Face newFace = new Face();
845
846 for (int coordIndex = 0; coordIndex < this.numOuterVerts - 1; coordIndex++)
847 {
848 newFace.v1 = coordIndex;
849 newFace.v2 = coordIndex + 1;
850 newFace.v3 = numTotalVerts - coordIndex - 1;
851 this.faces.Add(newFace);
852
853 newFace.v1 = coordIndex + 1;
854 newFace.v2 = numTotalVerts - coordIndex - 2;
855 newFace.v3 = numTotalVerts - coordIndex - 1;
856 this.faces.Add(newFace);
857 }
858 }
859 else
860 {
861 if (this.numOuterVerts < this.numHollowVerts)
862 {
863 Face newFace = new Face();
864 int j = 0; // j is the index for outer vertices
865 int maxJ = this.numOuterVerts - 1;
866 for (int i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices
867 {
868 if (j < maxJ)
869 if (angles.angles[j + 1].angle - hollowAngles.angles[i].angle < hollowAngles.angles[i].angle - angles.angles[j].angle + 0.000001f)
870 {
871 newFace.v1 = numTotalVerts - i - 1;
872 newFace.v2 = j;
873 newFace.v3 = j + 1;
874
875 this.faces.Add(newFace);
876 j += 1;
877 }
878
879 newFace.v1 = j;
880 newFace.v2 = numTotalVerts - i - 2;
881 newFace.v3 = numTotalVerts - i - 1;
882
883 this.faces.Add(newFace);
884 }
885 }
886 else // numHollowVerts < numOuterVerts
887 {
888 Face newFace = new Face();
889 int j = 0; // j is the index for inner vertices
890 int maxJ = this.numHollowVerts - 1;
891 for (int i = 0; i < this.numOuterVerts; i++)
892 {
893 if (j < maxJ)
894 if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f)
895 {
896 newFace.v1 = i;
897 newFace.v2 = numTotalVerts - j - 2;
898 newFace.v3 = numTotalVerts - j - 1;
899
900 this.faces.Add(newFace);
901 j += 1;
902 }
903
904 newFace.v1 = numTotalVerts - j - 1;
905 newFace.v2 = i;
906 newFace.v3 = i + 1;
907
908 this.faces.Add(newFace);
909 }
910 }
911 }
912 }
913
914 if (calcVertexNormals)
915 {
916 foreach (Coord hc in hollowCoords)
917 {
918 this.coords.Add(hc);
919 hollowCoordIndices.Add(this.coords.Count - 1);
920 }
921 }
922 else
923 this.coords.AddRange(hollowCoords);
924
925 if (this.calcVertexNormals)
926 {
927 this.vertexNormals.AddRange(hollowNormals);
928 this.us.AddRange(hollowUs);
929
930 }
931 }
932
933 if (simpleFace && createFaces)
934 {
935 if (sides == 3)
936 this.faces.Add(new Face(0, 1, 2));
937 else if (sides == 4)
938 {
939 this.faces.Add(new Face(0, 1, 2));
940 this.faces.Add(new Face(0, 2, 3));
941 }
942 }
943
944 if (calcVertexNormals && hasProfileCut)
945 {
946 int lastOuterVertIndex = this.numOuterVerts - 1;
947
948 if (hasHollow)
949 {
950 this.cut1CoordIndices.Add(0);
951 this.cut1CoordIndices.Add(this.coords.Count - 1);
952
953 this.cut2CoordIndices.Add(lastOuterVertIndex + 1);
954 this.cut2CoordIndices.Add(lastOuterVertIndex);
955
956 this.cutNormal1.X = this.coords[0].Y - this.coords[this.coords.Count - 1].Y;
957 this.cutNormal1.Y = -(this.coords[0].X - this.coords[this.coords.Count - 1].X);
958
959 this.cutNormal2.X = this.coords[lastOuterVertIndex + 1].Y - this.coords[lastOuterVertIndex].Y;
960 this.cutNormal2.Y = -(this.coords[lastOuterVertIndex + 1].X - this.coords[lastOuterVertIndex].X);
961 }
962
963 else
964 {
965 this.cut1CoordIndices.Add(0);
966 this.cut1CoordIndices.Add(1);
967
968 this.cut2CoordIndices.Add(lastOuterVertIndex);
969 this.cut2CoordIndices.Add(0);
970
971 this.cutNormal1.X = this.vertexNormals[1].Y;
972 this.cutNormal1.Y = -this.vertexNormals[1].X;
973
974 this.cutNormal2.X = -this.vertexNormals[this.vertexNormals.Count - 2].Y;
975 this.cutNormal2.Y = this.vertexNormals[this.vertexNormals.Count - 2].X;
976
977 }
978 this.cutNormal1.Normalize();
979 this.cutNormal2.Normalize();
980 }
981
982 this.MakeFaceUVs();
983
984 hollowCoords = null;
985 hollowNormals = null;
986 hollowUs = null;
987
988 if (calcVertexNormals)
989 { // calculate prim face numbers
990
991 // face number order is top, outer, hollow, bottom, start cut, end cut
992 // I know it's ugly but so is the whole concept of prim face numbers
993
994 int faceNum = 1; // start with outer faces
995 this.outerFaceNumber = faceNum;
996
997 int startVert = hasProfileCut && !hasHollow ? 1 : 0;
998 if (startVert > 0)
999 this.faceNumbers.Add(-1);
1000 for (int i = 0; i < this.numOuterVerts - 1; i++)
1001 this.faceNumbers.Add(sides < 5 && i <= sides ? faceNum++ : faceNum);
1002
1003 this.faceNumbers.Add(hasProfileCut ? -1 : faceNum++);
1004
1005 if (sides > 4 && (hasHollow || hasProfileCut))
1006 faceNum++;
1007
1008 if (sides < 5 && (hasHollow || hasProfileCut) && this.numOuterVerts < sides)
1009 faceNum++;
1010
1011 if (hasHollow)
1012 {
1013 for (int i = 0; i < this.numHollowVerts; i++)
1014 this.faceNumbers.Add(faceNum);
1015
1016 this.hollowFaceNumber = faceNum++;
1017 }
1018
1019 this.bottomFaceNumber = faceNum++;
1020
1021 if (hasHollow && hasProfileCut)
1022 this.faceNumbers.Add(faceNum++);
1023
1024 for (int i = 0; i < this.faceNumbers.Count; i++)
1025 if (this.faceNumbers[i] == -1)
1026 this.faceNumbers[i] = faceNum++;
1027
1028 this.numPrimFaces = faceNum;
1029 }
1030
1031 }
1032
1033 public void MakeFaceUVs()
1034 {
1035 this.faceUVs = new List<UVCoord>();
1036 foreach (Coord c in this.coords)
1037 this.faceUVs.Add(new UVCoord(1.0f - (0.5f + c.X), 1.0f - (0.5f - c.Y)));
1038 }
1039
1040 public Profile Copy()
1041 {
1042 return this.Copy(true);
1043 }
1044
1045 public Profile Copy(bool needFaces)
1046 {
1047 Profile copy = new Profile();
1048
1049 copy.coords.AddRange(this.coords);
1050 copy.faceUVs.AddRange(this.faceUVs);
1051
1052 if (needFaces)
1053 copy.faces.AddRange(this.faces);
1054 if ((copy.calcVertexNormals = this.calcVertexNormals) == true)
1055 {
1056 copy.vertexNormals.AddRange(this.vertexNormals);
1057 copy.faceNormal = this.faceNormal;
1058 copy.cutNormal1 = this.cutNormal1;
1059 copy.cutNormal2 = this.cutNormal2;
1060 copy.us.AddRange(this.us);
1061 copy.faceNumbers.AddRange(this.faceNumbers);
1062
1063 copy.cut1CoordIndices = new List<int>(this.cut1CoordIndices);
1064 copy.cut2CoordIndices = new List<int>(this.cut2CoordIndices);
1065 copy.hollowCoordIndices = new List<int>(this.hollowCoordIndices);
1066 copy.outerCoordIndices = new List<int>(this.outerCoordIndices);
1067 }
1068 copy.numOuterVerts = this.numOuterVerts;
1069 copy.numHollowVerts = this.numHollowVerts;
1070
1071 return copy;
1072 }
1073
1074 public void AddPos(Coord v)
1075 {
1076 this.AddPos(v.X, v.Y, v.Z);
1077 }
1078
1079 public void AddPos(float x, float y, float z)
1080 {
1081 int i;
1082 int numVerts = this.coords.Count;
1083 Coord vert;
1084
1085 for (i = 0; i < numVerts; i++)
1086 {
1087 vert = this.coords[i];
1088 vert.X += x;
1089 vert.Y += y;
1090 vert.Z += z;
1091 this.coords[i] = vert;
1092 }
1093 }
1094
1095 public void AddRot(Quat q)
1096 {
1097 int i;
1098 int numVerts = this.coords.Count;
1099
1100 for (i = 0; i < numVerts; i++)
1101 this.coords[i] *= q;
1102
1103 if (this.calcVertexNormals)
1104 {
1105 int numNormals = this.vertexNormals.Count;
1106 for (i = 0; i < numNormals; i++)
1107 this.vertexNormals[i] *= q;
1108
1109 this.faceNormal *= q;
1110 this.cutNormal1 *= q;
1111 this.cutNormal2 *= q;
1112
1113 }
1114 }
1115
1116 public void Scale(float x, float y)
1117 {
1118 int i;
1119 int numVerts = this.coords.Count;
1120 Coord vert;
1121
1122 for (i = 0; i < numVerts; i++)
1123 {
1124 vert = this.coords[i];
1125 vert.X *= x;
1126 vert.Y *= y;
1127 this.coords[i] = vert;
1128 }
1129 }
1130
1131 /// <summary>
1132 /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices
1133 /// </summary>
1134 public void FlipNormals()
1135 {
1136 int i;
1137 int numFaces = this.faces.Count;
1138 Face tmpFace;
1139 int tmp;
1140
1141 for (i = 0; i < numFaces; i++)
1142 {
1143 tmpFace = this.faces[i];
1144 tmp = tmpFace.v3;
1145 tmpFace.v3 = tmpFace.v1;
1146 tmpFace.v1 = tmp;
1147 this.faces[i] = tmpFace;
1148 }
1149
1150 if (this.calcVertexNormals)
1151 {
1152 int normalCount = this.vertexNormals.Count;
1153 if (normalCount > 0)
1154 {
1155 Coord n = this.vertexNormals[normalCount - 1];
1156 n.Z = -n.Z;
1157 this.vertexNormals[normalCount - 1] = n;
1158 }
1159 }
1160
1161 this.faceNormal.X = -this.faceNormal.X;
1162 this.faceNormal.Y = -this.faceNormal.Y;
1163 this.faceNormal.Z = -this.faceNormal.Z;
1164
1165 int numfaceUVs = this.faceUVs.Count;
1166 for (i = 0; i < numfaceUVs; i++)
1167 {
1168 UVCoord uv = this.faceUVs[i];
1169 uv.V = 1.0f - uv.V;
1170 this.faceUVs[i] = uv;
1171 }
1172 }
1173
1174 public void AddValue2FaceVertexIndices(int num)
1175 {
1176 int numFaces = this.faces.Count;
1177 Face tmpFace;
1178 for (int i = 0; i < numFaces; i++)
1179 {
1180 tmpFace = this.faces[i];
1181 tmpFace.v1 += num;
1182 tmpFace.v2 += num;
1183 tmpFace.v3 += num;
1184
1185 this.faces[i] = tmpFace;
1186 }
1187 }
1188
1189 public void AddValue2FaceNormalIndices(int num)
1190 {
1191 if (this.calcVertexNormals)
1192 {
1193 int numFaces = this.faces.Count;
1194 Face tmpFace;
1195 for (int i = 0; i < numFaces; i++)
1196 {
1197 tmpFace = this.faces[i];
1198 tmpFace.n1 += num;
1199 tmpFace.n2 += num;
1200 tmpFace.n3 += num;
1201
1202 this.faces[i] = tmpFace;
1203 }
1204 }
1205 }
1206
1207 public void DumpRaw(String path, String name, String title)
1208 {
1209 if (path == null)
1210 return;
1211 String fileName = name + "_" + title + ".raw";
1212 String completePath = System.IO.Path.Combine(path, fileName);
1213 StreamWriter sw = new StreamWriter(completePath);
1214
1215 for (int i = 0; i < this.faces.Count; i++)
1216 {
1217 string s = this.coords[this.faces[i].v1].ToString();
1218 s += " " + this.coords[this.faces[i].v2].ToString();
1219 s += " " + this.coords[this.faces[i].v3].ToString();
1220
1221 sw.WriteLine(s);
1222 }
1223
1224 sw.Close();
1225 }
1226 }
1227
1228 public struct PathNode
1229 {
1230 public Coord position;
1231 public Quat rotation;
1232 public float xScale;
1233 public float yScale;
1234 public float percentOfPath;
1235 }
1236
1237 public enum PathType { Linear = 0, Circular = 1, Flexible = 2 }
1238
1239 public class Path
1240 {
1241 public List<PathNode> pathNodes = new List<PathNode>();
1242
1243 public float twistBegin = 0.0f;
1244 public float twistEnd = 0.0f;
1245 public float topShearX = 0.0f;
1246 public float topShearY = 0.0f;
1247 public float pathCutBegin = 0.0f;
1248 public float pathCutEnd = 1.0f;
1249 public float dimpleBegin = 0.0f;
1250 public float dimpleEnd = 1.0f;
1251 public float skew = 0.0f;
1252 public float holeSizeX = 1.0f; // called pathScaleX in pbs
1253 public float holeSizeY = 0.25f;
1254 public float taperX = 0.0f;
1255 public float taperY = 0.0f;
1256 public float radius = 0.0f;
1257 public float revolutions = 1.0f;
1258 public int stepsPerRevolution = 24;
1259
1260 private const float twoPi = 2.0f * (float)Math.PI;
1261
1262 public void Create(PathType pathType, int steps)
1263 {
1264 if (this.taperX > 0.999f)
1265 this.taperX = 0.999f;
1266 if (this.taperX < -0.999f)
1267 this.taperX = -0.999f;
1268 if (this.taperY > 0.999f)
1269 this.taperY = 0.999f;
1270 if (this.taperY < -0.999f)
1271 this.taperY = -0.999f;
1272
1273 if (pathType == PathType.Linear || pathType == PathType.Flexible)
1274 {
1275 int step = 0;
1276
1277 float length = this.pathCutEnd - this.pathCutBegin;
1278 float twistTotal = twistEnd - twistBegin;
1279 float twistTotalAbs = Math.Abs(twistTotal);
1280 if (twistTotalAbs > 0.01f)
1281 steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number
1282
1283 float start = -0.5f;
1284 float stepSize = length / (float)steps;
1285 float percentOfPathMultiplier = stepSize * 0.999999f;
1286 float xOffset = this.topShearX * this.pathCutBegin;
1287 float yOffset = this.topShearY * this.pathCutBegin;
1288 float zOffset = start;
1289 float xOffsetStepIncrement = this.topShearX * length / steps;
1290 float yOffsetStepIncrement = this.topShearY * length / steps;
1291
1292 float percentOfPath = this.pathCutBegin;
1293 zOffset += percentOfPath;
1294
1295 // sanity checks
1296
1297 bool done = false;
1298
1299 while (!done)
1300 {
1301 PathNode newNode = new PathNode();
1302
1303 newNode.xScale = 1.0f;
1304 if (this.taperX == 0.0f)
1305 newNode.xScale = 1.0f;
1306 else if (this.taperX > 0.0f)
1307 newNode.xScale = 1.0f - percentOfPath * this.taperX;
1308 else newNode.xScale = 1.0f + (1.0f - percentOfPath) * this.taperX;
1309
1310 newNode.yScale = 1.0f;
1311 if (this.taperY == 0.0f)
1312 newNode.yScale = 1.0f;
1313 else if (this.taperY > 0.0f)
1314 newNode.yScale = 1.0f - percentOfPath * this.taperY;
1315 else newNode.yScale = 1.0f + (1.0f - percentOfPath) * this.taperY;
1316
1317 float twist = twistBegin + twistTotal * percentOfPath;
1318
1319 newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
1320 newNode.position = new Coord(xOffset, yOffset, zOffset);
1321 newNode.percentOfPath = percentOfPath;
1322
1323 pathNodes.Add(newNode);
1324
1325 if (step < steps)
1326 {
1327 step += 1;
1328 percentOfPath += percentOfPathMultiplier;
1329 xOffset += xOffsetStepIncrement;
1330 yOffset += yOffsetStepIncrement;
1331 zOffset += stepSize;
1332 if (percentOfPath > this.pathCutEnd)
1333 done = true;
1334 }
1335 else done = true;
1336 }
1337 } // end of linear path code
1338
1339 else // pathType == Circular
1340 {
1341 float twistTotal = twistEnd - twistBegin;
1342
1343 // if the profile has a lot of twist, add more layers otherwise the layers may overlap
1344 // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't
1345 // accurately match the viewer
1346 float twistTotalAbs = Math.Abs(twistTotal);
1347 if (twistTotalAbs > 0.01f)
1348 {
1349 if (twistTotalAbs > Math.PI * 1.5f)
1350 steps *= 2;
1351 if (twistTotalAbs > Math.PI * 3.0f)
1352 steps *= 2;
1353 }
1354
1355 float yPathScale = this.holeSizeY * 0.5f;
1356 float pathLength = this.pathCutEnd - this.pathCutBegin;
1357 float totalSkew = this.skew * 2.0f * pathLength;
1358 float skewStart = this.pathCutBegin * 2.0f * this.skew - this.skew;
1359 float xOffsetTopShearXFactor = this.topShearX * (0.25f + 0.5f * (0.5f - this.holeSizeY));
1360 float yShearCompensation = 1.0f + Math.Abs(this.topShearY) * 0.25f;
1361
1362 // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end
1363 // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used
1364 // to calculate the sine for generating the path radius appears to approximate it's effects there
1365 // too, but there are some subtle differences in the radius which are noticeable as the prim size
1366 // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on
1367 // the meshes generated with this technique appear nearly identical in shape to the same prims when
1368 // displayed by the viewer.
1369
1370 float startAngle = (twoPi * this.pathCutBegin * this.revolutions) - this.topShearY * 0.9f;
1371 float endAngle = (twoPi * this.pathCutEnd * this.revolutions) - this.topShearY * 0.9f;
1372 float stepSize = twoPi / this.stepsPerRevolution;
1373
1374 int step = (int)(startAngle / stepSize);
1375 float angle = startAngle;
1376
1377 bool done = false;
1378 while (!done) // loop through the length of the path and add the layers
1379 {
1380 PathNode newNode = new PathNode();
1381
1382 float xProfileScale = (1.0f - Math.Abs(this.skew)) * this.holeSizeX;
1383 float yProfileScale = this.holeSizeY;
1384
1385 float percentOfPath = angle / (twoPi * this.revolutions);
1386 float percentOfAngles = (angle - startAngle) / (endAngle - startAngle);
1387
1388 if (this.taperX > 0.01f)
1389 xProfileScale *= 1.0f - percentOfPath * this.taperX;
1390 else if (this.taperX < -0.01f)
1391 xProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperX;
1392
1393 if (this.taperY > 0.01f)
1394 yProfileScale *= 1.0f - percentOfPath * this.taperY;
1395 else if (this.taperY < -0.01f)
1396 yProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperY;
1397
1398 newNode.xScale = xProfileScale;
1399 newNode.yScale = yProfileScale;
1400
1401 float radiusScale = 1.0f;
1402 if (this.radius > 0.001f)
1403 radiusScale = 1.0f - this.radius * percentOfPath;
1404 else if (this.radius < 0.001f)
1405 radiusScale = 1.0f + this.radius * (1.0f - percentOfPath);
1406
1407 float twist = twistBegin + twistTotal * percentOfPath;
1408
1409 float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles);
1410 xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor;
1411
1412 float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale;
1413
1414 float zOffset = (float)Math.Sin(angle + this.topShearY) * (0.5f - yPathScale) * radiusScale;
1415
1416 newNode.position = new Coord(xOffset, yOffset, zOffset);
1417
1418 // now orient the rotation of the profile layer relative to it's position on the path
1419 // adding taperY to the angle used to generate the quat appears to approximate the viewer
1420
1421 newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + this.topShearY);
1422
1423 // next apply twist rotation to the profile layer
1424 if (twistTotal != 0.0f || twistBegin != 0.0f)
1425 newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
1426
1427 newNode.percentOfPath = percentOfPath;
1428
1429 pathNodes.Add(newNode);
1430
1431 // calculate terms for next iteration
1432 // calculate the angle for the next iteration of the loop
1433
1434 if (angle >= endAngle - 0.01)
1435 done = true;
1436 else
1437 {
1438 step += 1;
1439 angle = stepSize * step;
1440 if (angle > endAngle)
1441 angle = endAngle;
1442 }
1443 }
1444 }
1445 }
1446 }
1447
1448 public class PrimMesh
1449 {
1450 public string errorMessage = "";
1451 private const float twoPi = 2.0f * (float)Math.PI;
1452
1453 public List<Coord> coords;
1454 public List<Coord> normals;
1455 public List<Face> faces;
1456
1457 public List<ViewerFace> viewerFaces;
1458
1459 private int sides = 4;
1460 private int hollowSides = 4;
1461 private float profileStart = 0.0f;
1462 private float profileEnd = 1.0f;
1463 private float hollow = 0.0f;
1464 public int twistBegin = 0;
1465 public int twistEnd = 0;
1466 public float topShearX = 0.0f;
1467 public float topShearY = 0.0f;
1468 public float pathCutBegin = 0.0f;
1469 public float pathCutEnd = 1.0f;
1470 public float dimpleBegin = 0.0f;
1471 public float dimpleEnd = 1.0f;
1472 public float skew = 0.0f;
1473 public float holeSizeX = 1.0f; // called pathScaleX in pbs
1474 public float holeSizeY = 0.25f;
1475 public float taperX = 0.0f;
1476 public float taperY = 0.0f;
1477 public float radius = 0.0f;
1478 public float revolutions = 1.0f;
1479 public int stepsPerRevolution = 24;
1480
1481 private int profileOuterFaceNumber = -1;
1482 private int profileHollowFaceNumber = -1;
1483
1484 private bool hasProfileCut = false;
1485 private bool hasHollow = false;
1486 public bool calcVertexNormals = false;
1487 private bool normalsProcessed = false;
1488 public bool viewerMode = false;
1489 public bool sphereMode = false;
1490
1491 public int numPrimFaces = 0;
1492
1493 /// <summary>
1494 /// Human readable string representation of the parameters used to create a mesh.
1495 /// </summary>
1496 /// <returns></returns>
1497 public string ParamsToDisplayString()
1498 {
1499 string s = "";
1500 s += "sides..................: " + this.sides.ToString();
1501 s += "\nhollowSides..........: " + this.hollowSides.ToString();
1502 s += "\nprofileStart.........: " + this.profileStart.ToString();
1503 s += "\nprofileEnd...........: " + this.profileEnd.ToString();
1504 s += "\nhollow...............: " + this.hollow.ToString();
1505 s += "\ntwistBegin...........: " + this.twistBegin.ToString();
1506 s += "\ntwistEnd.............: " + this.twistEnd.ToString();
1507 s += "\ntopShearX............: " + this.topShearX.ToString();
1508 s += "\ntopShearY............: " + this.topShearY.ToString();
1509 s += "\npathCutBegin.........: " + this.pathCutBegin.ToString();
1510 s += "\npathCutEnd...........: " + this.pathCutEnd.ToString();
1511 s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString();
1512 s += "\ndimpleEnd............: " + this.dimpleEnd.ToString();
1513 s += "\nskew.................: " + this.skew.ToString();
1514 s += "\nholeSizeX............: " + this.holeSizeX.ToString();
1515 s += "\nholeSizeY............: " + this.holeSizeY.ToString();
1516 s += "\ntaperX...............: " + this.taperX.ToString();
1517 s += "\ntaperY...............: " + this.taperY.ToString();
1518 s += "\nradius...............: " + this.radius.ToString();
1519 s += "\nrevolutions..........: " + this.revolutions.ToString();
1520 s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString();
1521 s += "\nsphereMode...........: " + this.sphereMode.ToString();
1522 s += "\nhasProfileCut........: " + this.hasProfileCut.ToString();
1523 s += "\nhasHollow............: " + this.hasHollow.ToString();
1524 s += "\nviewerMode...........: " + this.viewerMode.ToString();
1525
1526 return s;
1527 }
1528
1529 public int ProfileOuterFaceNumber
1530 {
1531 get { return profileOuterFaceNumber; }
1532 }
1533
1534 public int ProfileHollowFaceNumber
1535 {
1536 get { return profileHollowFaceNumber; }
1537 }
1538
1539 public bool HasProfileCut
1540 {
1541 get { return hasProfileCut; }
1542 }
1543
1544 public bool HasHollow
1545 {
1546 get { return hasHollow; }
1547 }
1548
1549
1550 /// <summary>
1551 /// Constructs a PrimMesh object and creates the profile for extrusion.
1552 /// </summary>
1553 /// <param name="sides"></param>
1554 /// <param name="profileStart"></param>
1555 /// <param name="profileEnd"></param>
1556 /// <param name="hollow"></param>
1557 /// <param name="hollowSides"></param>
1558 public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides)
1559 {
1560 this.coords = new List<Coord>();
1561 this.faces = new List<Face>();
1562
1563 this.sides = sides;
1564 this.profileStart = profileStart;
1565 this.profileEnd = profileEnd;
1566 this.hollow = hollow;
1567 this.hollowSides = hollowSides;
1568
1569 if (sides < 3)
1570 this.sides = 3;
1571 if (hollowSides < 3)
1572 this.hollowSides = 3;
1573 if (profileStart < 0.0f)
1574 this.profileStart = 0.0f;
1575 if (profileEnd > 1.0f)
1576 this.profileEnd = 1.0f;
1577 if (profileEnd < 0.02f)
1578 this.profileEnd = 0.02f;
1579 if (profileStart >= profileEnd)
1580 this.profileStart = profileEnd - 0.02f;
1581 if (hollow > 0.99f)
1582 this.hollow = 0.99f;
1583 if (hollow < 0.0f)
1584 this.hollow = 0.0f;
1585 }
1586
1587 /// <summary>
1588 /// Extrudes a profile along a path.
1589 /// </summary>
1590 public void Extrude(PathType pathType)
1591 {
1592 bool needEndFaces = false;
1593
1594 this.coords = new List<Coord>();
1595 this.faces = new List<Face>();
1596
1597 if (this.viewerMode)
1598 {
1599 this.viewerFaces = new List<ViewerFace>();
1600 this.calcVertexNormals = true;
1601 }
1602
1603 if (this.calcVertexNormals)
1604 this.normals = new List<Coord>();
1605
1606 int steps = 1;
1607
1608 float length = this.pathCutEnd - this.pathCutBegin;
1609 normalsProcessed = false;
1610
1611 if (this.viewerMode && this.sides == 3)
1612 {
1613 // prisms don't taper well so add some vertical resolution
1614 // other prims may benefit from this but just do prisms for now
1615 if (Math.Abs(this.taperX) > 0.01 || Math.Abs(this.taperY) > 0.01)
1616 steps = (int)(steps * 4.5 * length);
1617 }
1618
1619 if (this.sphereMode)
1620 this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f;
1621 else
1622 this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f;
1623 this.hasHollow = (this.hollow > 0.001f);
1624
1625 float twistBegin = this.twistBegin / 360.0f * twoPi;
1626 float twistEnd = this.twistEnd / 360.0f * twoPi;
1627 float twistTotal = twistEnd - twistBegin;
1628 float twistTotalAbs = Math.Abs(twistTotal);
1629 if (twistTotalAbs > 0.01f)
1630 steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number
1631
1632 float hollow = this.hollow;
1633
1634 if (pathType == PathType.Circular)
1635 {
1636 needEndFaces = false;
1637 if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f)
1638 needEndFaces = true;
1639 else if (this.taperX != 0.0f || this.taperY != 0.0f)
1640 needEndFaces = true;
1641 else if (this.skew != 0.0f)
1642 needEndFaces = true;
1643 else if (twistTotal != 0.0f)
1644 needEndFaces = true;
1645 else if (this.radius != 0.0f)
1646 needEndFaces = true;
1647 }
1648 else needEndFaces = true;
1649
1650 // sanity checks
1651 float initialProfileRot = 0.0f;
1652 if (pathType == PathType.Circular)
1653 {
1654 if (this.sides == 3)
1655 {
1656 initialProfileRot = (float)Math.PI;
1657 if (this.hollowSides == 4)
1658 {
1659 if (hollow > 0.7f)
1660 hollow = 0.7f;
1661 hollow *= 0.707f;
1662 }
1663 else hollow *= 0.5f;
1664 }
1665 else if (this.sides == 4)
1666 {
1667 initialProfileRot = 0.25f * (float)Math.PI;
1668 if (this.hollowSides != 4)
1669 hollow *= 0.707f;
1670 }
1671 else if (this.sides > 4)
1672 {
1673 initialProfileRot = (float)Math.PI;
1674 if (this.hollowSides == 4)
1675 {
1676 if (hollow > 0.7f)
1677 hollow = 0.7f;
1678 hollow /= 0.7f;
1679 }
1680 }
1681 }
1682 else
1683 {
1684 if (this.sides == 3)
1685 {
1686 if (this.hollowSides == 4)
1687 {
1688 if (hollow > 0.7f)
1689 hollow = 0.7f;
1690 hollow *= 0.707f;
1691 }
1692 else hollow *= 0.5f;
1693 }
1694 else if (this.sides == 4)
1695 {
1696 initialProfileRot = 1.25f * (float)Math.PI;
1697 if (this.hollowSides != 4)
1698 hollow *= 0.707f;
1699 }
1700 else if (this.sides == 24 && this.hollowSides == 4)
1701 hollow *= 1.414f;
1702 }
1703
1704 Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, true, calcVertexNormals);
1705 this.errorMessage = profile.errorMessage;
1706
1707 this.numPrimFaces = profile.numPrimFaces;
1708
1709 int cut1FaceNumber = profile.bottomFaceNumber + 1;
1710 int cut2FaceNumber = cut1FaceNumber + 1;
1711 if (!needEndFaces)
1712 {
1713 cut1FaceNumber -= 2;
1714 cut2FaceNumber -= 2;
1715 }
1716
1717 profileOuterFaceNumber = profile.outerFaceNumber;
1718 if (!needEndFaces)
1719 profileOuterFaceNumber--;
1720
1721 if (hasHollow)
1722 {
1723 profileHollowFaceNumber = profile.hollowFaceNumber;
1724 if (!needEndFaces)
1725 profileHollowFaceNumber--;
1726 }
1727
1728 int cut1Vert = -1;
1729 int cut2Vert = -1;
1730 if (hasProfileCut)
1731 {
1732 cut1Vert = hasHollow ? profile.coords.Count - 1 : 0;
1733 cut2Vert = hasHollow ? profile.numOuterVerts - 1 : profile.numOuterVerts;
1734 }
1735
1736 if (initialProfileRot != 0.0f)
1737 {
1738 profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot));
1739 if (viewerMode)
1740 profile.MakeFaceUVs();
1741 }
1742
1743 Coord lastCutNormal1 = new Coord();
1744 Coord lastCutNormal2 = new Coord();
1745 float thisV = 0.0f;
1746 float lastV = 0.0f;
1747
1748 Path path = new Path();
1749 path.twistBegin = twistBegin;
1750 path.twistEnd = twistEnd;
1751 path.topShearX = topShearX;
1752 path.topShearY = topShearY;
1753 path.pathCutBegin = pathCutBegin;
1754 path.pathCutEnd = pathCutEnd;
1755 path.dimpleBegin = dimpleBegin;
1756 path.dimpleEnd = dimpleEnd;
1757 path.skew = skew;
1758 path.holeSizeX = holeSizeX;
1759 path.holeSizeY = holeSizeY;
1760 path.taperX = taperX;
1761 path.taperY = taperY;
1762 path.radius = radius;
1763 path.revolutions = revolutions;
1764 path.stepsPerRevolution = stepsPerRevolution;
1765
1766 path.Create(pathType, steps);
1767
1768 for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
1769 {
1770 PathNode node = path.pathNodes[nodeIndex];
1771 Profile newLayer = profile.Copy();
1772 newLayer.Scale(node.xScale, node.yScale);
1773
1774 newLayer.AddRot(node.rotation);
1775 newLayer.AddPos(node.position);
1776
1777 if (needEndFaces && nodeIndex == 0)
1778 {
1779 newLayer.FlipNormals();
1780
1781 // add the bottom faces to the viewerFaces list
1782 if (this.viewerMode)
1783 {
1784 Coord faceNormal = newLayer.faceNormal;
1785 ViewerFace newViewerFace = new ViewerFace(profile.bottomFaceNumber);
1786 int numFaces = newLayer.faces.Count;
1787 List<Face> faces = newLayer.faces;
1788
1789 for (int i = 0; i < numFaces; i++)
1790 {
1791 Face face = faces[i];
1792 newViewerFace.v1 = newLayer.coords[face.v1];
1793 newViewerFace.v2 = newLayer.coords[face.v2];
1794 newViewerFace.v3 = newLayer.coords[face.v3];
1795
1796 newViewerFace.coordIndex1 = face.v1;
1797 newViewerFace.coordIndex2 = face.v2;
1798 newViewerFace.coordIndex3 = face.v3;
1799
1800 newViewerFace.n1 = faceNormal;
1801 newViewerFace.n2 = faceNormal;
1802 newViewerFace.n3 = faceNormal;
1803
1804 newViewerFace.uv1 = newLayer.faceUVs[face.v1];
1805 newViewerFace.uv2 = newLayer.faceUVs[face.v2];
1806 newViewerFace.uv3 = newLayer.faceUVs[face.v3];
1807
1808 if (pathType == PathType.Linear)
1809 {
1810 newViewerFace.uv1.Flip();
1811 newViewerFace.uv2.Flip();
1812 newViewerFace.uv3.Flip();
1813 }
1814
1815 this.viewerFaces.Add(newViewerFace);
1816 }
1817 }
1818 } // if (nodeIndex == 0)
1819
1820 // append this layer
1821
1822 int coordsLen = this.coords.Count;
1823 newLayer.AddValue2FaceVertexIndices(coordsLen);
1824
1825 this.coords.AddRange(newLayer.coords);
1826
1827 if (this.calcVertexNormals)
1828 {
1829 newLayer.AddValue2FaceNormalIndices(this.normals.Count);
1830 this.normals.AddRange(newLayer.vertexNormals);
1831 }
1832
1833 if (node.percentOfPath < this.pathCutBegin + 0.01f || node.percentOfPath > this.pathCutEnd - 0.01f)
1834 this.faces.AddRange(newLayer.faces);
1835
1836 // fill faces between layers
1837
1838 int numVerts = newLayer.coords.Count;
1839 Face newFace1 = new Face();
1840 Face newFace2 = new Face();
1841
1842 thisV = 1.0f - node.percentOfPath;
1843
1844 if (nodeIndex > 0)
1845 {
1846 int startVert = coordsLen + 1;
1847 int endVert = this.coords.Count;
1848
1849 if (sides < 5 || this.hasProfileCut || this.hasHollow)
1850 startVert--;
1851
1852 for (int i = startVert; i < endVert; i++)
1853 {
1854 int iNext = i + 1;
1855 if (i == endVert - 1)
1856 iNext = startVert;
1857
1858 int whichVert = i - startVert;
1859
1860 newFace1.v1 = i;
1861 newFace1.v2 = i - numVerts;
1862 newFace1.v3 = iNext;
1863
1864 newFace1.n1 = newFace1.v1;
1865 newFace1.n2 = newFace1.v2;
1866 newFace1.n3 = newFace1.v3;
1867 this.faces.Add(newFace1);
1868
1869 newFace2.v1 = iNext;
1870 newFace2.v2 = i - numVerts;
1871 newFace2.v3 = iNext - numVerts;
1872
1873 newFace2.n1 = newFace2.v1;
1874 newFace2.n2 = newFace2.v2;
1875 newFace2.n3 = newFace2.v3;
1876 this.faces.Add(newFace2);
1877
1878 if (this.viewerMode)
1879 {
1880 // add the side faces to the list of viewerFaces here
1881
1882 int primFaceNum = profile.faceNumbers[whichVert];
1883 if (!needEndFaces)
1884 primFaceNum -= 1;
1885
1886 ViewerFace newViewerFace1 = new ViewerFace(primFaceNum);
1887 ViewerFace newViewerFace2 = new ViewerFace(primFaceNum);
1888
1889 int uIndex = whichVert;
1890 if (!hasHollow && sides > 4 && uIndex < newLayer.us.Count - 1)
1891 {
1892 uIndex++;
1893 }
1894
1895 float u1 = newLayer.us[uIndex];
1896 float u2 = 1.0f;
1897 if (uIndex < (int)newLayer.us.Count - 1)
1898 u2 = newLayer.us[uIndex + 1];
1899
1900 if (whichVert == cut1Vert || whichVert == cut2Vert)
1901 {
1902 u1 = 0.0f;
1903 u2 = 1.0f;
1904 }
1905 else if (sides < 5)
1906 {
1907 if (whichVert < profile.numOuterVerts)
1908 { // boxes and prisms have one texture face per side of the prim, so the U values have to be scaled
1909 // to reflect the entire texture width
1910 u1 *= sides;
1911 u2 *= sides;
1912 u2 -= (int)u1;
1913 u1 -= (int)u1;
1914 if (u2 < 0.1f)
1915 u2 = 1.0f;
1916 }
1917 }
1918
1919 if (this.sphereMode)
1920 {
1921 if (whichVert != cut1Vert && whichVert != cut2Vert)
1922 {
1923 u1 = u1 * 2.0f - 1.0f;
1924 u2 = u2 * 2.0f - 1.0f;
1925
1926 if (whichVert >= newLayer.numOuterVerts)
1927 {
1928 u1 -= hollow;
1929 u2 -= hollow;
1930 }
1931
1932 }
1933 }
1934
1935 newViewerFace1.uv1.U = u1;
1936 newViewerFace1.uv2.U = u1;
1937 newViewerFace1.uv3.U = u2;
1938
1939 newViewerFace1.uv1.V = thisV;
1940 newViewerFace1.uv2.V = lastV;
1941 newViewerFace1.uv3.V = thisV;
1942
1943 newViewerFace2.uv1.U = u2;
1944 newViewerFace2.uv2.U = u1;
1945 newViewerFace2.uv3.U = u2;
1946
1947 newViewerFace2.uv1.V = thisV;
1948 newViewerFace2.uv2.V = lastV;
1949 newViewerFace2.uv3.V = lastV;
1950
1951 newViewerFace1.v1 = this.coords[newFace1.v1];
1952 newViewerFace1.v2 = this.coords[newFace1.v2];
1953 newViewerFace1.v3 = this.coords[newFace1.v3];
1954
1955 newViewerFace2.v1 = this.coords[newFace2.v1];
1956 newViewerFace2.v2 = this.coords[newFace2.v2];
1957 newViewerFace2.v3 = this.coords[newFace2.v3];
1958
1959 newViewerFace1.coordIndex1 = newFace1.v1;
1960 newViewerFace1.coordIndex2 = newFace1.v2;
1961 newViewerFace1.coordIndex3 = newFace1.v3;
1962
1963 newViewerFace2.coordIndex1 = newFace2.v1;
1964 newViewerFace2.coordIndex2 = newFace2.v2;
1965 newViewerFace2.coordIndex3 = newFace2.v3;
1966
1967 // profile cut faces
1968 if (whichVert == cut1Vert)
1969 {
1970 newViewerFace1.primFaceNumber = cut1FaceNumber;
1971 newViewerFace2.primFaceNumber = cut1FaceNumber;
1972 newViewerFace1.n1 = newLayer.cutNormal1;
1973 newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal1;
1974
1975 newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal1;
1976 newViewerFace2.n2 = lastCutNormal1;
1977 }
1978 else if (whichVert == cut2Vert)
1979 {
1980 newViewerFace1.primFaceNumber = cut2FaceNumber;
1981 newViewerFace2.primFaceNumber = cut2FaceNumber;
1982 newViewerFace1.n1 = newLayer.cutNormal2;
1983 newViewerFace1.n2 = lastCutNormal2;
1984 newViewerFace1.n3 = lastCutNormal2;
1985
1986 newViewerFace2.n1 = newLayer.cutNormal2;
1987 newViewerFace2.n3 = newLayer.cutNormal2;
1988 newViewerFace2.n2 = lastCutNormal2;
1989 }
1990
1991 else // outer and hollow faces
1992 {
1993 if ((sides < 5 && whichVert < newLayer.numOuterVerts) || (hollowSides < 5 && whichVert >= newLayer.numOuterVerts))
1994 { // looks terrible when path is twisted... need vertex normals here
1995 newViewerFace1.CalcSurfaceNormal();
1996 newViewerFace2.CalcSurfaceNormal();
1997 }
1998 else
1999 {
2000 newViewerFace1.n1 = this.normals[newFace1.n1];
2001 newViewerFace1.n2 = this.normals[newFace1.n2];
2002 newViewerFace1.n3 = this.normals[newFace1.n3];
2003
2004 newViewerFace2.n1 = this.normals[newFace2.n1];
2005 newViewerFace2.n2 = this.normals[newFace2.n2];
2006 newViewerFace2.n3 = this.normals[newFace2.n3];
2007 }
2008 }
2009
2010 this.viewerFaces.Add(newViewerFace1);
2011 this.viewerFaces.Add(newViewerFace2);
2012
2013 }
2014 }
2015 }
2016
2017 lastCutNormal1 = newLayer.cutNormal1;
2018 lastCutNormal2 = newLayer.cutNormal2;
2019 lastV = thisV;
2020
2021 if (needEndFaces && nodeIndex == path.pathNodes.Count - 1 && viewerMode)
2022 {
2023 // add the top faces to the viewerFaces list here
2024 Coord faceNormal = newLayer.faceNormal;
2025 ViewerFace newViewerFace = new ViewerFace(0);
2026 int numFaces = newLayer.faces.Count;
2027 List<Face> faces = newLayer.faces;
2028
2029 for (int i = 0; i < numFaces; i++)
2030 {
2031 Face face = faces[i];
2032 newViewerFace.v1 = newLayer.coords[face.v1 - coordsLen];
2033 newViewerFace.v2 = newLayer.coords[face.v2 - coordsLen];
2034 newViewerFace.v3 = newLayer.coords[face.v3 - coordsLen];
2035
2036 newViewerFace.coordIndex1 = face.v1 - coordsLen;
2037 newViewerFace.coordIndex2 = face.v2 - coordsLen;
2038 newViewerFace.coordIndex3 = face.v3 - coordsLen;
2039
2040 newViewerFace.n1 = faceNormal;
2041 newViewerFace.n2 = faceNormal;
2042 newViewerFace.n3 = faceNormal;
2043
2044 newViewerFace.uv1 = newLayer.faceUVs[face.v1 - coordsLen];
2045 newViewerFace.uv2 = newLayer.faceUVs[face.v2 - coordsLen];
2046 newViewerFace.uv3 = newLayer.faceUVs[face.v3 - coordsLen];
2047
2048 if (pathType == PathType.Linear)
2049 {
2050 newViewerFace.uv1.Flip();
2051 newViewerFace.uv2.Flip();
2052 newViewerFace.uv3.Flip();
2053 }
2054
2055 this.viewerFaces.Add(newViewerFace);
2056 }
2057 }
2058
2059
2060 } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
2061
2062 }
2063
2064
2065 /// <summary>
2066 /// DEPRICATED - use Extrude(PathType.Linear) instead
2067 /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism.
2068 /// </summary>
2069 ///
2070 public void ExtrudeLinear()
2071 {
2072 this.Extrude(PathType.Linear);
2073 }
2074
2075
2076 /// <summary>
2077 /// DEPRICATED - use Extrude(PathType.Circular) instead
2078 /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring.
2079 /// </summary>
2080 ///
2081 public void ExtrudeCircular()
2082 {
2083 this.Extrude(PathType.Circular);
2084 }
2085
2086
2087 private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3)
2088 {
2089 Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
2090 Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
2091
2092 Coord normal = Coord.Cross(edge1, edge2);
2093
2094 normal.Normalize();
2095
2096 return normal;
2097 }
2098
2099 private Coord SurfaceNormal(Face face)
2100 {
2101 return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]);
2102 }
2103
2104 /// <summary>
2105 /// Calculate the surface normal for a face in the list of faces
2106 /// </summary>
2107 /// <param name="faceIndex"></param>
2108 /// <returns></returns>
2109 public Coord SurfaceNormal(int faceIndex)
2110 {
2111 int numFaces = this.faces.Count;
2112 if (faceIndex < 0 || faceIndex >= numFaces)
2113 throw new Exception("faceIndex out of range");
2114
2115 return SurfaceNormal(this.faces[faceIndex]);
2116 }
2117
2118 /// <summary>
2119 /// Duplicates a PrimMesh object. All object properties are copied by value, including lists.
2120 /// </summary>
2121 /// <returns></returns>
2122 public PrimMesh Copy()
2123 {
2124 PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides);
2125 copy.twistBegin = this.twistBegin;
2126 copy.twistEnd = this.twistEnd;
2127 copy.topShearX = this.topShearX;
2128 copy.topShearY = this.topShearY;
2129 copy.pathCutBegin = this.pathCutBegin;
2130 copy.pathCutEnd = this.pathCutEnd;
2131 copy.dimpleBegin = this.dimpleBegin;
2132 copy.dimpleEnd = this.dimpleEnd;
2133 copy.skew = this.skew;
2134 copy.holeSizeX = this.holeSizeX;
2135 copy.holeSizeY = this.holeSizeY;
2136 copy.taperX = this.taperX;
2137 copy.taperY = this.taperY;
2138 copy.radius = this.radius;
2139 copy.revolutions = this.revolutions;
2140 copy.stepsPerRevolution = this.stepsPerRevolution;
2141 copy.calcVertexNormals = this.calcVertexNormals;
2142 copy.normalsProcessed = this.normalsProcessed;
2143 copy.viewerMode = this.viewerMode;
2144 copy.numPrimFaces = this.numPrimFaces;
2145 copy.errorMessage = this.errorMessage;
2146
2147 copy.coords = new List<Coord>(this.coords);
2148 copy.faces = new List<Face>(this.faces);
2149 copy.viewerFaces = new List<ViewerFace>(this.viewerFaces);
2150 copy.normals = new List<Coord>(this.normals);
2151
2152 return copy;
2153 }
2154
2155 /// <summary>
2156 /// Calculate surface normals for all of the faces in the list of faces in this mesh
2157 /// </summary>
2158 public void CalcNormals()
2159 {
2160 if (normalsProcessed)
2161 return;
2162
2163 normalsProcessed = true;
2164
2165 int numFaces = faces.Count;
2166
2167 if (!this.calcVertexNormals)
2168 this.normals = new List<Coord>();
2169
2170 for (int i = 0; i < numFaces; i++)
2171 {
2172 Face face = faces[i];
2173
2174 this.normals.Add(SurfaceNormal(i).Normalize());
2175
2176 int normIndex = normals.Count - 1;
2177 face.n1 = normIndex;
2178 face.n2 = normIndex;
2179 face.n3 = normIndex;
2180
2181 this.faces[i] = face;
2182 }
2183 }
2184
2185 /// <summary>
2186 /// Adds a value to each XYZ vertex coordinate in the mesh
2187 /// </summary>
2188 /// <param name="x"></param>
2189 /// <param name="y"></param>
2190 /// <param name="z"></param>
2191 public void AddPos(float x, float y, float z)
2192 {
2193 int i;
2194 int numVerts = this.coords.Count;
2195 Coord vert;
2196
2197 for (i = 0; i < numVerts; i++)
2198 {
2199 vert = this.coords[i];
2200 vert.X += x;
2201 vert.Y += y;
2202 vert.Z += z;
2203 this.coords[i] = vert;
2204 }
2205
2206 if (this.viewerFaces != null)
2207 {
2208 int numViewerFaces = this.viewerFaces.Count;
2209
2210 for (i = 0; i < numViewerFaces; i++)
2211 {
2212 ViewerFace v = this.viewerFaces[i];
2213 v.AddPos(x, y, z);
2214 this.viewerFaces[i] = v;
2215 }
2216 }
2217 }
2218
2219 /// <summary>
2220 /// Rotates the mesh
2221 /// </summary>
2222 /// <param name="q"></param>
2223 public void AddRot(Quat q)
2224 {
2225 int i;
2226 int numVerts = this.coords.Count;
2227
2228 for (i = 0; i < numVerts; i++)
2229 this.coords[i] *= q;
2230
2231 if (this.normals != null)
2232 {
2233 int numNormals = this.normals.Count;
2234 for (i = 0; i < numNormals; i++)
2235 this.normals[i] *= q;
2236 }
2237
2238 if (this.viewerFaces != null)
2239 {
2240 int numViewerFaces = this.viewerFaces.Count;
2241
2242 for (i = 0; i < numViewerFaces; i++)
2243 {
2244 ViewerFace v = this.viewerFaces[i];
2245 v.v1 *= q;
2246 v.v2 *= q;
2247 v.v3 *= q;
2248
2249 v.n1 *= q;
2250 v.n2 *= q;
2251 v.n3 *= q;
2252 this.viewerFaces[i] = v;
2253 }
2254 }
2255 }
2256
2257#if VERTEX_INDEXER
2258 public VertexIndexer GetVertexIndexer()
2259 {
2260 if (this.viewerMode && this.viewerFaces.Count > 0)
2261 return new VertexIndexer(this);
2262 return null;
2263 }
2264#endif
2265
2266 /// <summary>
2267 /// Scales the mesh
2268 /// </summary>
2269 /// <param name="x"></param>
2270 /// <param name="y"></param>
2271 /// <param name="z"></param>
2272 public void Scale(float x, float y, float z)
2273 {
2274 int i;
2275 int numVerts = this.coords.Count;
2276 //Coord vert;
2277
2278 Coord m = new Coord(x, y, z);
2279 for (i = 0; i < numVerts; i++)
2280 this.coords[i] *= m;
2281
2282 if (this.viewerFaces != null)
2283 {
2284 int numViewerFaces = this.viewerFaces.Count;
2285 for (i = 0; i < numViewerFaces; i++)
2286 {
2287 ViewerFace v = this.viewerFaces[i];
2288 v.v1 *= m;
2289 v.v2 *= m;
2290 v.v3 *= m;
2291 this.viewerFaces[i] = v;
2292 }
2293
2294 }
2295
2296 }
2297
2298 /// <summary>
2299 /// Dumps the mesh to a Blender compatible "Raw" format file
2300 /// </summary>
2301 /// <param name="path"></param>
2302 /// <param name="name"></param>
2303 /// <param name="title"></param>
2304 public void DumpRaw(String path, String name, String title)
2305 {
2306 if (path == null)
2307 return;
2308 String fileName = name + "_" + title + ".raw";
2309 String completePath = System.IO.Path.Combine(path, fileName);
2310 StreamWriter sw = new StreamWriter(completePath);
2311
2312 for (int i = 0; i < this.faces.Count; i++)
2313 {
2314 string s = this.coords[this.faces[i].v1].ToString();
2315 s += " " + this.coords[this.faces[i].v2].ToString();
2316 s += " " + this.coords[this.faces[i].v3].ToString();
2317
2318 sw.WriteLine(s);
2319 }
2320
2321 sw.Close();
2322 }
2323 }
2324}
diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs
new file mode 100644
index 0000000..1c75db6
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs
@@ -0,0 +1,244 @@
1/*
2 * Copyright (c) Contributors
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.Text;
31
32using System.Drawing;
33using System.Drawing.Imaging;
34
35namespace PrimMesher
36{
37 public class SculptMap
38 {
39 public int width;
40 public int height;
41 public byte[] redBytes;
42 public byte[] greenBytes;
43 public byte[] blueBytes;
44
45 public SculptMap()
46 {
47 }
48
49 public SculptMap(Bitmap bm, int lod)
50 {
51 int bmW = bm.Width;
52 int bmH = bm.Height;
53
54 if (bmW == 0 || bmH == 0)
55 throw new Exception("SculptMap: bitmap has no data");
56
57 int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image
58
59 bool needsScaling = false;
60 bool smallMap = false;
61
62 width = bmW;
63 height = bmH;
64
65 while (width * height > numLodPixels * 4)
66 {
67 width >>= 1;
68 height >>= 1;
69 needsScaling = true;
70 }
71
72 try
73 {
74 if (needsScaling)
75 bm = ScaleImage(bm, width, height);
76 }
77
78 catch (Exception e)
79 {
80 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
81 }
82
83 if (width * height > numLodPixels)
84 {
85 smallMap = false;
86 width >>= 1;
87 height >>= 1;
88 }
89 else
90 smallMap = true;
91
92 int numBytes = (width + 1) * (height + 1);
93 redBytes = new byte[numBytes];
94 greenBytes = new byte[numBytes];
95 blueBytes = new byte[numBytes];
96
97 int byteNdx = 0;
98 Color c;
99
100 try
101 {
102 for (int y = 0; y <= height; y++)
103 {
104 for (int x = 0; x < width; x++)
105 {
106 if (smallMap)
107 c = bm.GetPixel(x, y < height ? y : y - 1);
108 else
109 c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1);
110
111 redBytes[byteNdx] = c.R;
112 greenBytes[byteNdx] = c.G;
113 blueBytes[byteNdx] = c.B;
114
115 ++byteNdx;
116 }
117
118 if (smallMap)
119 c = bm.GetPixel(width - 1, y < height ? y : y - 1);
120 else
121 c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1);
122
123 redBytes[byteNdx] = c.R;
124 greenBytes[byteNdx] = c.G;
125 blueBytes[byteNdx] = c.B;
126
127 ++byteNdx;
128 }
129 }
130 catch (Exception e)
131 {
132 throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString());
133 }
134
135 width++;
136 height++;
137 }
138
139 public List<List<Coord>> ToRows(bool mirror)
140 {
141 int numRows = height;
142 int numCols = width;
143
144 List<List<Coord>> rows = new List<List<Coord>>(numRows);
145
146 float pixScale = 1.0f / 255;
147
148 int rowNdx, colNdx;
149 int smNdx = 0;
150
151 for (rowNdx = 0; rowNdx < numRows; rowNdx++)
152 {
153 List<Coord> row = new List<Coord>(numCols);
154 for (colNdx = 0; colNdx < numCols; colNdx++)
155 {
156
157 if (mirror)
158 row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
159 else
160 row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
161
162 ++smNdx;
163 }
164 rows.Add(row);
165 }
166 return rows;
167 }
168
169 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
170 {
171
172 Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
173
174 Color c;
175
176
177 // will let last step to be eventually diferent, as seems to be in sl
178
179 float xscale = (float)srcImage.Width / (float)destWidth;
180 float yscale = (float)srcImage.Height / (float)destHeight;
181
182 int lastsx = srcImage.Width - 1;
183 int lastsy = srcImage.Height - 1;
184 int lastdx = destWidth - 1;
185 int lastdy = destHeight - 1;
186
187 float sy = 0.5f;
188 float sx;
189
190 for (int y = 0; y < lastdy; y++)
191 {
192 sx = 0.5f;
193 for (int x = 0; x < lastdx; x++)
194 {
195 try
196 {
197 c = srcImage.GetPixel((int)(sx), (int)(sy));
198 scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
199 }
200 catch (IndexOutOfRangeException)
201 {
202 }
203 sx += xscale;
204 }
205 try
206 {
207 c = srcImage.GetPixel(lastsx, (int)(sy));
208 scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B));
209 }
210 catch (IndexOutOfRangeException)
211 {
212 }
213
214 sy += yscale;
215 }
216
217 sx = 0.5f;
218 for (int x = 0; x < lastdx; x++)
219 {
220 try
221 {
222 c = srcImage.GetPixel((int)(sx), lastsy);
223 scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B));
224 }
225 catch (IndexOutOfRangeException)
226 {
227 }
228
229 sx += xscale;
230 }
231 try
232 {
233 c = srcImage.GetPixel(lastsx, lastsy);
234 scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B));
235 }
236 catch (IndexOutOfRangeException)
237 {
238 }
239
240 srcImage.Dispose();
241 return scaledImage;
242 }
243 }
244} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs
new file mode 100644
index 0000000..bc1375b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs
@@ -0,0 +1,220 @@
1/*
2 * Copyright (c) Contributors
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.Text;
31using System.IO;
32
33using System.Drawing;
34using System.Drawing.Imaging;
35
36namespace PrimMesher
37{
38
39 public class SculptMesh
40 {
41 public List<Coord> coords;
42 public List<Face> faces;
43
44 public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 };
45
46
47 public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert)
48 {
49 if (mirror)
50 invert = !invert;
51
52 SculptMap smap = new SculptMap(sculptBitmap, lod);
53
54 List<List<Coord>> rows = smap.ToRows(mirror);
55
56 _SculptMesh(rows, sculptType, invert);
57 }
58
59 private void _SculptMesh(List<List<Coord>> rows, SculptType sculptType, bool invert)
60 {
61 coords = new List<Coord>();
62 faces = new List<Face>();
63
64 sculptType = (SculptType)(((int)sculptType) & 0x07);
65
66 int width = rows[0].Count;
67
68 int p1, p2, p3, p4;
69
70 int imageX, imageY;
71
72 if (sculptType != SculptType.plane)
73 {
74 if (rows.Count % 2 == 0)
75 {
76 for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++)
77 rows[rowNdx].Add(rows[rowNdx][0]);
78 }
79 else
80 {
81 int lastIndex = rows[0].Count - 1;
82
83 for (int i = 0; i < rows.Count; i++)
84 rows[i][0] = rows[i][lastIndex];
85 }
86 }
87
88 Coord topPole = rows[0][width / 2];
89 Coord bottomPole = rows[rows.Count - 1][width / 2];
90
91 if (sculptType == SculptType.sphere)
92 {
93 if (rows.Count % 2 == 0)
94 {
95 int count = rows[0].Count;
96 List<Coord> topPoleRow = new List<Coord>(count);
97 List<Coord> bottomPoleRow = new List<Coord>(count);
98
99 for (int i = 0; i < count; i++)
100 {
101 topPoleRow.Add(topPole);
102 bottomPoleRow.Add(bottomPole);
103 }
104 rows.Insert(0, topPoleRow);
105 rows.Add(bottomPoleRow);
106 }
107 else
108 {
109 int count = rows[0].Count;
110
111 List<Coord> topPoleRow = rows[0];
112 List<Coord> bottomPoleRow = rows[rows.Count - 1];
113
114 for (int i = 0; i < count; i++)
115 {
116 topPoleRow[i] = topPole;
117 bottomPoleRow[i] = bottomPole;
118 }
119 }
120 }
121
122 if (sculptType == SculptType.torus)
123 rows.Add(rows[0]);
124
125 int coordsDown = rows.Count;
126 int coordsAcross = rows[0].Count;
127
128 float widthUnit = 1.0f / (coordsAcross - 1);
129 float heightUnit = 1.0f / (coordsDown - 1);
130
131 for (imageY = 0; imageY < coordsDown; imageY++)
132 {
133 int rowOffset = imageY * coordsAcross;
134
135 for (imageX = 0; imageX < coordsAcross; imageX++)
136 {
137 /*
138 * p1-----p2
139 * | \ f2 |
140 * | \ |
141 * | f1 \|
142 * p3-----p4
143 */
144
145 p4 = rowOffset + imageX;
146 p3 = p4 - 1;
147
148 p2 = p4 - coordsAcross;
149 p1 = p3 - coordsAcross;
150
151 this.coords.Add(rows[imageY][imageX]);
152
153 if (imageY > 0 && imageX > 0)
154 {
155 Face f1, f2;
156
157 if (invert)
158 {
159 f1 = new Face(p1, p4, p3);
160 f2 = new Face(p1, p2, p4);
161 }
162 else
163 {
164 f1 = new Face(p1, p3, p4);
165 f2 = new Face(p1, p4, p2);
166 }
167
168 this.faces.Add(f1);
169 this.faces.Add(f2);
170 }
171 }
172 }
173 }
174
175 /// <summary>
176 /// Duplicates a SculptMesh object. All object properties are copied by value, including lists.
177 /// </summary>
178 /// <returns></returns>
179 public SculptMesh Copy()
180 {
181 return new SculptMesh(this);
182 }
183
184 public SculptMesh(SculptMesh sm)
185 {
186 coords = new List<Coord>(sm.coords);
187 faces = new List<Face>(sm.faces);
188 }
189
190 public void Scale(float x, float y, float z)
191 {
192 int i;
193 int numVerts = this.coords.Count;
194
195 Coord m = new Coord(x, y, z);
196 for (i = 0; i < numVerts; i++)
197 this.coords[i] *= m;
198 }
199
200 public void DumpRaw(String path, String name, String title)
201 {
202 if (path == null)
203 return;
204 String fileName = name + "_" + title + ".raw";
205 String completePath = System.IO.Path.Combine(path, fileName);
206 StreamWriter sw = new StreamWriter(completePath);
207
208 for (int i = 0; i < this.faces.Count; i++)
209 {
210 string s = this.coords[this.faces[i].v1].ToString();
211 s += " " + this.coords[this.faces[i].v2].ToString();
212 s += " " + this.coords[this.faces[i].v3].ToString();
213
214 sw.WriteLine(s);
215 }
216
217 sw.Close();
218 }
219 }
220}
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..bea34d4
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1950 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28
29// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private Vector3 _velocity;
78 private Vector3 _target_velocity;
79 private Vector3 _acceleration;
80 private Vector3 m_rotationalVelocity;
81 private Vector3 m_size;
82 private Quaternion m_orientation;
83 private Quaternion m_orientation2D;
84 private float m_mass = 80f;
85 public float m_density = 60f;
86 private bool m_pidControllerActive = true;
87
88 const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
89 const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
90 public float PID_D;
91 public float PID_P;
92
93 private float timeStep;
94 private float invtimeStep;
95
96 private float m_feetOffset = 0;
97 private float feetOff = 0;
98 private float feetSZ = 0.5f;
99 const float feetScale = 0.8f;
100 private float boneOff = 0;
101 private float m_lastVelocitySqr = 0;
102
103 public float walkDivisor = 1.3f;
104 public float runDivisor = 0.8f;
105 private bool flying = false;
106 private bool m_iscolliding = false;
107 private bool m_iscollidingGround = false;
108 private bool m_iscollidingObj = false;
109 private bool m_alwaysRun = false;
110
111 private bool _zeroFlag = false;
112
113 private int m_requestedUpdateFrequency = 0;
114 private uint m_localID = 0;
115 public bool m_returnCollisions = false;
116 // taints and their non-tainted counterparts
117 public bool m_isPhysical = false; // the current physical status
118 public float MinimumGroundFlightOffset = 3f;
119
120 private float m_buoyancy = 0f;
121
122 private bool m_freemove = false;
123 // private CollisionLocker ode;
124
125// private string m_name = String.Empty;
126 // other filter control
127 int m_colliderfilter = 0;
128 int m_colliderGroundfilter = 0;
129 int m_colliderObjectfilter = 0;
130 bool m_collisionException = false;
131
132 // Default we're a Character
133 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
134
135 // Default, Collide with Other Geometries, spaces, bodies and characters.
136 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
137 | CollisionCategories.Geom
138 | CollisionCategories.VolumeDtc
139 );
140 // we do land collisions not ode | CollisionCategories.Land);
141 public IntPtr Body = IntPtr.Zero;
142 private OdeScene _parent_scene;
143 private IntPtr topbox = IntPtr.Zero;
144 private IntPtr midbox = IntPtr.Zero;
145 private IntPtr feetbox = IntPtr.Zero;
146 private IntPtr bbox = IntPtr.Zero;
147 public IntPtr collider = IntPtr.Zero;
148
149 public IntPtr Amotor = IntPtr.Zero;
150
151 public d.Mass ShellMass;
152
153
154
155
156 public int m_eventsubscription = 0;
157 private int m_cureventsubscription = 0;
158 private CollisionEventUpdate CollisionEventsThisFrame = null;
159 private bool SentEmptyCollisionsEvent;
160
161 // unique UUID of this character object
162 public UUID m_uuid;
163 public bool bad = false;
164
165 float mu;
166
167
168
169 public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
170 {
171 m_uuid = UUID.Random();
172 m_localID = localID;
173
174 timeStep = parent_scene.ODE_STEPSIZE;
175 invtimeStep = 1 / timeStep;
176
177 if (pos.IsFinite())
178 {
179 if (pos.Z > 99999f)
180 {
181 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
182 }
183 if (pos.Z < -100f) // shouldn't this be 0 ?
184 {
185 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
186 }
187 _position = pos;
188 }
189 else
190 {
191 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
192 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
193 }
194
195 _parent_scene = parent_scene;
196
197
198 m_size.X = pSize.X;
199 m_size.Y = pSize.Y;
200 m_size.Z = pSize.Z;
201
202 if(m_size.X <0.01f)
203 m_size.X = 0.01f;
204 if(m_size.Y <0.01f)
205 m_size.Y = 0.01f;
206 if(m_size.Z <0.01f)
207 m_size.Z = 0.01f;
208
209 m_feetOffset = pfeetOffset;
210 m_orientation = Quaternion.Identity;
211 m_orientation2D = Quaternion.Identity;
212 m_density = density;
213
214 // force lower density for testing
215 m_density = 3.0f;
216
217 m_density *= 1.4f; // scale to have mass similar to capsule
218
219 mu = parent_scene.AvatarFriction;
220
221 walkDivisor = walk_divisor;
222 runDivisor = rundivisor;
223
224 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
225
226 PID_D = basePID_D * m_mass * invtimeStep;
227 PID_P = basePID_P * m_mass * invtimeStep;
228
229 m_isPhysical = false; // current status: no ODE information exists
230
231 Name = avName;
232
233 AddChange(changes.Add, null);
234 }
235
236 public override int PhysicsActorType
237 {
238 get { return (int)ActorTypes.Agent; }
239 set { return; }
240 }
241
242 public override void getContactData(ref ContactData cdata)
243 {
244 cdata.mu = mu;
245 cdata.bounce = 0;
246 cdata.softcolide = false;
247 }
248
249 public override bool Building { get; set; }
250
251 /// <summary>
252 /// If this is set, the avatar will move faster
253 /// </summary>
254 public override bool SetAlwaysRun
255 {
256 get { return m_alwaysRun; }
257 set { m_alwaysRun = value; }
258 }
259
260 public override uint LocalID
261 {
262 get { return m_localID; }
263 set { m_localID = value; }
264 }
265
266 public override PhysicsActor ParentActor
267 {
268 get { return (PhysicsActor)this; }
269 }
270
271 public override bool Grabbed
272 {
273 set { return; }
274 }
275
276 public override bool Selected
277 {
278 set { return; }
279 }
280
281 public override float Buoyancy
282 {
283 get { return m_buoyancy; }
284 set { m_buoyancy = value; }
285 }
286
287 public override bool FloatOnWater
288 {
289 set { return; }
290 }
291
292 public override bool IsPhysical
293 {
294 get { return m_isPhysical; }
295 set { return; }
296 }
297
298 public override bool ThrottleUpdates
299 {
300 get { return false; }
301 set { return; }
302 }
303
304 public override bool Flying
305 {
306 get { return flying; }
307 set
308 {
309 flying = value;
310// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
311 }
312 }
313
314 /// <summary>
315 /// Returns if the avatar is colliding in general.
316 /// This includes the ground and objects and avatar.
317 /// </summary>
318 public override bool IsColliding
319 {
320 get { return (m_iscolliding || m_iscollidingGround); }
321 set
322 {
323 if (value)
324 {
325 m_colliderfilter += 2;
326 if (m_colliderfilter > 2)
327 m_colliderfilter = 2;
328 }
329 else
330 {
331 m_colliderfilter--;
332 if (m_colliderfilter < 0)
333 m_colliderfilter = 0;
334 }
335
336 if (m_colliderfilter == 0)
337 m_iscolliding = false;
338 else
339 {
340 m_pidControllerActive = true;
341 m_iscolliding = true;
342 }
343 }
344 }
345
346 /// <summary>
347 /// Returns if an avatar is colliding with the ground
348 /// </summary>
349 public override bool CollidingGround
350 {
351 get { return m_iscollidingGround; }
352 set
353 {
354/* we now control this
355 if (value)
356 {
357 m_colliderGroundfilter += 2;
358 if (m_colliderGroundfilter > 2)
359 m_colliderGroundfilter = 2;
360 }
361 else
362 {
363 m_colliderGroundfilter--;
364 if (m_colliderGroundfilter < 0)
365 m_colliderGroundfilter = 0;
366 }
367
368 if (m_colliderGroundfilter == 0)
369 m_iscollidingGround = false;
370 else
371 m_iscollidingGround = true;
372 */
373 }
374
375 }
376
377 /// <summary>
378 /// Returns if the avatar is colliding with an object
379 /// </summary>
380 public override bool CollidingObj
381 {
382 get { return m_iscollidingObj; }
383 set
384 {
385 // Ubit filter this also
386 if (value)
387 {
388 m_colliderObjectfilter += 2;
389 if (m_colliderObjectfilter > 2)
390 m_colliderObjectfilter = 2;
391 }
392 else
393 {
394 m_colliderObjectfilter--;
395 if (m_colliderObjectfilter < 0)
396 m_colliderObjectfilter = 0;
397 }
398
399 if (m_colliderObjectfilter == 0)
400 m_iscollidingObj = false;
401 else
402 m_iscollidingObj = true;
403
404// m_iscollidingObj = value;
405
406 if (m_iscollidingObj)
407 m_pidControllerActive = false;
408 else
409 m_pidControllerActive = true;
410 }
411 }
412
413 /// <summary>
414 /// turn the PID controller on or off.
415 /// The PID Controller will turn on all by itself in many situations
416 /// </summary>
417 /// <param name="status"></param>
418 public void SetPidStatus(bool status)
419 {
420 m_pidControllerActive = status;
421 }
422
423 public override bool Stopped
424 {
425 get { return _zeroFlag; }
426 }
427
428 /// <summary>
429 /// This 'puts' an avatar somewhere in the physics space.
430 /// Not really a good choice unless you 'know' it's a good
431 /// spot otherwise you're likely to orbit the avatar.
432 /// </summary>
433 public override Vector3 Position
434 {
435 get { return _position; }
436 set
437 {
438 if (value.IsFinite())
439 {
440 if (value.Z > 9999999f)
441 {
442 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
443 }
444 if (value.Z < -100f)
445 {
446 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
447 }
448 AddChange(changes.Position, value);
449 }
450 else
451 {
452 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
453 }
454 }
455 }
456
457 public override Vector3 RotationalVelocity
458 {
459 get { return m_rotationalVelocity; }
460 set { m_rotationalVelocity = value; }
461 }
462
463 /// <summary>
464 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
465 /// and use it to offset landings properly
466 /// </summary>
467 public override Vector3 Size
468 {
469 get
470 {
471 return m_size;
472 }
473 set
474 {
475 if (value.IsFinite())
476 {
477 if(value.X <0.01f)
478 value.X = 0.01f;
479 if(value.Y <0.01f)
480 value.Y = 0.01f;
481 if(value.Z <0.01f)
482 value.Z = 0.01f;
483
484 AddChange(changes.Size, value);
485 }
486 else
487 {
488 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
489 }
490 }
491 }
492
493 public override void setAvatarSize(Vector3 size, float feetOffset)
494 {
495 if (size.IsFinite())
496 {
497 if (size.X < 0.01f)
498 size.X = 0.01f;
499 if (size.Y < 0.01f)
500 size.Y = 0.01f;
501 if (size.Z < 0.01f)
502 size.Z = 0.01f;
503
504 strAvatarSize st = new strAvatarSize();
505 st.size = size;
506 st.offset = feetOffset;
507 AddChange(changes.AvatarSize, st);
508 }
509 else
510 {
511 m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
512 }
513
514 }
515 /// <summary>
516 /// This creates the Avatar's physical Surrogate at the position supplied
517 /// </summary>
518 /// <param name="npositionX"></param>
519 /// <param name="npositionY"></param>
520 /// <param name="npositionZ"></param>
521
522 //
523 /// <summary>
524 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
525 /// This may be used in calculations in the scene/scenepresence
526 /// </summary>
527 public override float Mass
528 {
529 get
530 {
531 return m_mass;
532 }
533 }
534 public override void link(PhysicsActor obj)
535 {
536
537 }
538
539 public override void delink()
540 {
541
542 }
543
544 public override void LockAngularMotion(Vector3 axis)
545 {
546
547 }
548
549
550 public override Vector3 Force
551 {
552 get { return _target_velocity; }
553 set { return; }
554 }
555
556 public override int VehicleType
557 {
558 get { return 0; }
559 set { return; }
560 }
561
562 public override void VehicleFloatParam(int param, float value)
563 {
564
565 }
566
567 public override void VehicleVectorParam(int param, Vector3 value)
568 {
569
570 }
571
572 public override void VehicleRotationParam(int param, Quaternion rotation)
573 {
574
575 }
576
577 public override void VehicleFlags(int param, bool remove)
578 {
579
580 }
581
582 public override void SetVolumeDetect(int param)
583 {
584
585 }
586
587 public override Vector3 CenterOfMass
588 {
589 get
590 {
591 Vector3 pos = _position;
592 return pos;
593 }
594 }
595
596 public override Vector3 GeometricCenter
597 {
598 get
599 {
600 Vector3 pos = _position;
601 return pos;
602 }
603 }
604
605 public override PrimitiveBaseShape Shape
606 {
607 set { return; }
608 }
609
610 public override Vector3 Velocity
611 {
612 get
613 {
614 return _velocity;
615 }
616 set
617 {
618 if (value.IsFinite())
619 {
620 AddChange(changes.Velocity, value);
621 }
622 else
623 {
624 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
625 }
626 }
627 }
628
629 public override Vector3 Torque
630 {
631 get { return Vector3.Zero; }
632 set { return; }
633 }
634
635 public override float CollisionScore
636 {
637 get { return 0f; }
638 set { }
639 }
640
641 public override bool Kinematic
642 {
643 get { return false; }
644 set { }
645 }
646
647 public override Quaternion Orientation
648 {
649 get { return m_orientation; }
650 set
651 {
652// fakeori = value;
653// givefakeori++;
654 value.Normalize();
655 AddChange(changes.Orientation, value);
656 }
657 }
658
659 public override Vector3 Acceleration
660 {
661 get { return _acceleration; }
662 set { }
663 }
664
665 public void SetAcceleration(Vector3 accel)
666 {
667 m_pidControllerActive = true;
668 _acceleration = accel;
669 }
670
671 /// <summary>
672 /// Adds the force supplied to the Target Velocity
673 /// The PID controller takes this target velocity and tries to make it a reality
674 /// </summary>
675 /// <param name="force"></param>
676 public override void AddForce(Vector3 force, bool pushforce)
677 {
678 if (force.IsFinite())
679 {
680 if (pushforce)
681 {
682 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
683 }
684 else
685 {
686 AddChange(changes.Velocity, force);
687 }
688 }
689 else
690 {
691 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
692 }
693 //m_lastUpdateSent = false;
694 }
695
696 public override void AddAngularForce(Vector3 force, bool pushforce)
697 {
698
699 }
700
701 public override void SetMomentum(Vector3 momentum)
702 {
703 if (momentum.IsFinite())
704 AddChange(changes.Momentum, momentum);
705 }
706
707 private void ajustCollider()
708 {
709 float vq = _velocity.LengthSquared();
710 if (m_lastVelocitySqr != vq)
711 {
712 m_lastVelocitySqr = vq;
713 if (vq > 100.0f)
714 {
715 Vector3 off = _velocity;
716 float t = 0.5f * timeStep;
717 off = off * t;
718 d.Quaternion qtmp;
719 d.GeomCopyQuaternion(bbox, out qtmp);
720 Quaternion q;
721 q.X = qtmp.X;
722 q.Y = qtmp.Y;
723 q.Z = qtmp.Z;
724 q.W = qtmp.W;
725 off *= Quaternion.Conjugate(q);
726
727 d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
728
729 off.X = 2.0f * (m_size.X + Math.Abs(off.X));
730 off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
731 off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
732 d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
733
734 d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
735 d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
736 d.GeomSetCategoryBits(topbox, 0);
737 d.GeomSetCollideBits(topbox, 0);
738 d.GeomSetCategoryBits(midbox, 0);
739 d.GeomSetCollideBits(midbox, 0);
740 d.GeomSetCategoryBits(feetbox, 0);
741 d.GeomSetCollideBits(feetbox, 0);
742 }
743 else
744 {
745 d.GeomSetCategoryBits(bbox, 0);
746 d.GeomSetCollideBits(bbox, 0);
747 d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
748 d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
749 d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
750 d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
751 d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
752 d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
753 }
754 uint cat1 = d.GeomGetCategoryBits(bbox);
755 uint col1 = d.GeomGetCollideBits(bbox);
756
757 }
758 }
759
760 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
761 {
762 // sizes one day should came from visual parameters
763 float sx = m_size.X;
764 float sy = m_size.Y;
765 float sz = m_size.Z;
766
767 float topsx = sx * 0.9f;
768 float midsx = sx;
769 float feetsx = sx * feetScale;
770 float bonesx = sx * 0.2f;
771
772 float topsy = sy * 0.4f;
773 float midsy = sy;
774 float feetsy = sy * feetScale * 0.8f;
775 float bonesy = feetsy * 0.2f;
776
777 float topsz = sz * 0.15f;
778 float feetsz = sz * 0.45f;
779 if (feetsz > 0.6f)
780 feetsz = 0.6f;
781
782 float midsz = sz - topsz - feetsz;
783 float bonesz = sz;
784
785 float bot = -sz * 0.5f + m_feetOffset;
786
787 boneOff = bot + 0.3f;
788
789 float feetz = bot + feetsz * 0.5f;
790 bot += feetsz;
791
792 feetOff = bot;
793 feetSZ = feetsz;
794
795 float midz = bot + midsz * 0.5f;
796 bot += midsz;
797 float topz = bot + topsz * 0.5f;
798
799 _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
800
801 collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
802 d.HashSpaceSetLevels(collider, -4, 3);
803 d.SpaceSetSublevel(collider, 3);
804 d.SpaceSetCleanup(collider, false);
805 d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
806 d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
807
808 feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
809 midbox = d.CreateBox(collider, midsx, midsy, midsz);
810 topbox = d.CreateBox(collider, topsx, topsy, topsz);
811 bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
812
813 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
814
815 d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
816
817 PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
818 PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
819
820 Body = d.BodyCreate(_parent_scene.world);
821
822 _zeroFlag = false;
823 m_collisionException = false;
824 m_pidControllerActive = true;
825 m_freemove = false;
826
827 _velocity = Vector3.Zero;
828 m_lastVelocitySqr = 0;
829
830 d.BodySetAutoDisableFlag(Body, false);
831 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
832
833 _position.X = npositionX;
834 _position.Y = npositionY;
835 _position.Z = npositionZ;
836
837 d.BodySetMass(Body, ref ShellMass);
838 d.GeomSetBody(feetbox, Body);
839 d.GeomSetBody(midbox, Body);
840 d.GeomSetBody(topbox, Body);
841 d.GeomSetBody(bbox, Body);
842
843 d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
844 d.GeomSetOffsetPosition(midbox, 0, 0, midz);
845 d.GeomSetOffsetPosition(topbox, 0, 0, topz);
846
847 ajustCollider();
848
849
850 // The purpose of the AMotor here is to keep the avatar's physical
851 // surrogate from rotating while moving
852 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
853 d.JointAttach(Amotor, Body, IntPtr.Zero);
854
855 d.JointSetAMotorMode(Amotor, 0);
856 d.JointSetAMotorNumAxes(Amotor, 3);
857 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
858 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
859 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
860
861 d.JointSetAMotorAngle(Amotor, 0, 0);
862 d.JointSetAMotorAngle(Amotor, 1, 0);
863 d.JointSetAMotorAngle(Amotor, 2, 0);
864
865 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
866 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
867 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
868 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
869 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
870 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
871
872 // These lowstops and high stops are effectively (no wiggle room)
873 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
874 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
875 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
876 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
877 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
878 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
879
880 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
881 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
882 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
883
884 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
885 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
886 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
887 }
888
889 /// <summary>
890 /// Destroys the avatar body and geom
891
892 private void AvatarGeomAndBodyDestroy()
893 {
894 // Kill the Amotor
895 if (Amotor != IntPtr.Zero)
896 {
897 d.JointDestroy(Amotor);
898 Amotor = IntPtr.Zero;
899 }
900
901 if (Body != IntPtr.Zero)
902 {
903 //kill the body
904 d.BodyDestroy(Body);
905 Body = IntPtr.Zero;
906 }
907
908 //kill the Geoms
909 if (topbox != IntPtr.Zero)
910 {
911 _parent_scene.actor_name_map.Remove(topbox);
912 _parent_scene.waitForSpaceUnlock(collider);
913 d.GeomDestroy(topbox);
914 topbox = IntPtr.Zero;
915 }
916 if (midbox != IntPtr.Zero)
917 {
918 _parent_scene.actor_name_map.Remove(midbox);
919 _parent_scene.waitForSpaceUnlock(collider);
920 d.GeomDestroy(midbox);
921 midbox = IntPtr.Zero;
922 }
923 if (feetbox != IntPtr.Zero)
924 {
925 _parent_scene.actor_name_map.Remove(feetbox);
926 _parent_scene.waitForSpaceUnlock(collider);
927 d.GeomDestroy(feetbox);
928 feetbox = IntPtr.Zero;
929 }
930
931 if (bbox != IntPtr.Zero)
932 {
933 _parent_scene.actor_name_map.Remove(bbox);
934 _parent_scene.waitForSpaceUnlock(collider);
935 d.GeomDestroy(bbox);
936 bbox = IntPtr.Zero;
937 }
938
939 if (collider != IntPtr.Zero)
940 {
941 d.SpaceDestroy(collider);
942 collider = IntPtr.Zero;
943 }
944
945 }
946
947 //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
948 public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
949 {
950 float sin = 2.0f * rot.Z * rot.W;
951 float cos = rot.W * rot.W - rot.Z * rot.Z;
952 float tx = x;
953
954 x = tx * cos - y * sin;
955 y = tx * sin + y * cos;
956 }
957 public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
958 {
959 float tx = x;
960 x = tx * cos - y * sin;
961 y = tx * sin + y * cos;
962 }
963 public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
964 {
965 float tx = x;
966 x = tx * cos + y * sin;
967 y = -tx * sin + y * cos;
968 }
969
970 public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
971 {
972 float sin = - 2.0f * rot.Z * rot.W;
973 float cos = rot.W * rot.W - rot.Z * rot.Z;
974 float tx = x;
975
976 x = tx * cos - y * sin;
977 y = tx * sin + y * cos;
978 }
979
980
981 public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
982 {
983 feetcollision = false;
984 if (m_collisionException)
985 return false;
986
987 Vector3 offset;
988
989 if (me == bbox) // if moving fast
990 {
991 // force a full inelastic collision
992 m_collisionException = true;
993
994 offset = m_size * m_orientation2D;
995
996 offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
997 offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
998 offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
999
1000 if (reverse)
1001 {
1002 offset.X *= -contact.normal.X;
1003 offset.Y *= -contact.normal.Y;
1004 offset.Z *= -contact.normal.Z;
1005 }
1006 else
1007 {
1008 offset.X *= contact.normal.X;
1009 offset.Y *= contact.normal.Y;
1010 offset.Z *= contact.normal.Z;
1011 }
1012
1013 offset.X += contact.pos.X;
1014 offset.Y += contact.pos.Y;
1015 offset.Z += contact.pos.Z;
1016
1017 //_position = offset;
1018 //return false;
1019 }
1020
1021 offset.X = contact.pos.X - _position.X;
1022 offset.Y = contact.pos.Y - _position.Y;
1023
1024 if (me == topbox)
1025 {
1026 offset.Z = contact.pos.Z - _position.Z;
1027
1028 offset.Normalize();
1029
1030 if (reverse)
1031 {
1032 contact.normal.X = offset.X;
1033 contact.normal.Y = offset.Y;
1034 contact.normal.Z = offset.Z;
1035 }
1036 else
1037 {
1038 contact.normal.X = -offset.X;
1039 contact.normal.Y = -offset.Y;
1040 contact.normal.Z = -offset.Z;
1041 }
1042 return true;
1043 }
1044
1045 if (me == midbox)
1046 {
1047 if (Math.Abs(contact.normal.Z) > 0.95f)
1048 offset.Z = contact.pos.Z - _position.Z;
1049 else
1050 offset.Z = contact.normal.Z;
1051
1052 offset.Normalize();
1053
1054 if (reverse)
1055 {
1056 contact.normal.X = offset.X;
1057 contact.normal.Y = offset.Y;
1058 contact.normal.Z = offset.Z;
1059 }
1060 else
1061 {
1062 contact.normal.X = -offset.X;
1063 contact.normal.Y = -offset.Y;
1064 contact.normal.Z = -offset.Z;
1065 }
1066
1067 return true;
1068 }
1069
1070 else if (me == feetbox)
1071 {
1072 float h = contact.pos.Z - _position.Z;
1073
1074 if (Math.Abs(contact.normal.Z) > 0.95f)
1075 {
1076 if (contact.normal.Z > 0)
1077 contact.normal.Z = 1.0f;
1078 else
1079 contact.normal.Z = -1.0f;
1080 contact.normal.X = 0.0f;
1081 contact.normal.Y = 0.0f;
1082 feetcollision = true;
1083 if (h < boneOff)
1084 IsColliding = true;
1085 return true;
1086 }
1087
1088 offset.Z = h - feetOff; // distance from top of feetbox
1089
1090 if (offset.Z > 0)
1091 return false;
1092
1093 if (offset.Z > -0.01)
1094 {
1095 offset.X = 0;
1096 offset.Y = 0;
1097 offset.Z = -1.0f;
1098 }
1099 else
1100 {
1101 offset.Normalize();
1102 }
1103
1104 if (reverse)
1105 {
1106 contact.normal.X = offset.X;
1107 contact.normal.Y = offset.Y;
1108 contact.normal.Z = offset.Z;
1109 }
1110 else
1111 {
1112 contact.normal.X = -offset.X;
1113 contact.normal.Y = -offset.Y;
1114 contact.normal.Z = -offset.Z;
1115 }
1116 feetcollision = true;
1117 if (h < boneOff)
1118 IsColliding = true;
1119 }
1120 else
1121 return false;
1122
1123 return true;
1124 }
1125
1126 /// <summary>
1127 /// Called from Simulate
1128 /// This is the avatar's movement control + PID Controller
1129 /// </summary>
1130 /// <param name="timeStep"></param>
1131 public void Move(List<OdeCharacter> defects)
1132 {
1133 if (Body == IntPtr.Zero)
1134 return;
1135
1136 if (m_collisionException)
1137 {
1138 d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
1139 d.BodySetLinearVel(Body, 0, 0, 0);
1140
1141 float v = _velocity.Length();
1142 if (v != 0)
1143 {
1144 v = 5.0f / v;
1145 _velocity = _velocity * v;
1146 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1147 }
1148 ajustCollider();
1149 m_collisionException = false;
1150 return;
1151 }
1152
1153 d.Vector3 dtmp = d.BodyGetPosition(Body);
1154 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1155
1156 // the Amotor still lets avatar rotation to drift during colisions
1157 // so force it back to identity
1158
1159 d.Quaternion qtmp;
1160 qtmp.W = m_orientation2D.W;
1161 qtmp.X = m_orientation2D.X;
1162 qtmp.Y = m_orientation2D.Y;
1163 qtmp.Z = m_orientation2D.Z;
1164 d.BodySetQuaternion(Body, ref qtmp);
1165
1166 if (m_pidControllerActive == false)
1167 {
1168 _zeroPosition = localpos;
1169 }
1170
1171 if (!localpos.IsFinite())
1172 {
1173 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1174 defects.Add(this);
1175 // _parent_scene.RemoveCharacter(this);
1176
1177 // destroy avatar capsule and related ODE data
1178 AvatarGeomAndBodyDestroy();
1179 return;
1180 }
1181
1182 // check outbounds forcing to be in world
1183 bool fixbody = false;
1184 if (localpos.X < 0.0f)
1185 {
1186 fixbody = true;
1187 localpos.X = 0.1f;
1188 }
1189 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
1190 {
1191 fixbody = true;
1192 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
1193 }
1194 if (localpos.Y < 0.0f)
1195 {
1196 fixbody = true;
1197 localpos.Y = 0.1f;
1198 }
1199 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
1200 {
1201 fixbody = true;
1202 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
1203 }
1204 if (fixbody)
1205 {
1206 m_freemove = false;
1207 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
1208 }
1209
1210 float breakfactor;
1211
1212 Vector3 vec = Vector3.Zero;
1213 dtmp = d.BodyGetLinearVel(Body);
1214 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1215 float velLengthSquared = vel.LengthSquared();
1216
1217 float movementdivisor = 1f;
1218 //Ubit change divisions into multiplications below
1219 if (!m_alwaysRun)
1220 movementdivisor = 1 / walkDivisor;
1221 else
1222 movementdivisor = 1 / runDivisor;
1223
1224 //******************************************
1225 // colide with land
1226
1227 d.AABB aabb;
1228 d.GeomGetAABB(feetbox, out aabb);
1229 float chrminZ = aabb.MinZ; ; // move up a bit
1230 Vector3 posch = localpos;
1231
1232 float ftmp;
1233
1234 if (flying)
1235 {
1236 ftmp = timeStep;
1237 posch.X += vel.X * ftmp;
1238 posch.Y += vel.Y * ftmp;
1239 }
1240
1241 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
1242 if (chrminZ < terrainheight)
1243 {
1244 float depth = terrainheight - chrminZ;
1245 if (!flying)
1246 {
1247 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
1248 }
1249 else
1250 vec.Z = depth * PID_P * 50;
1251
1252 if (depth < 0.1f)
1253 {
1254 m_colliderGroundfilter++;
1255 if (m_colliderGroundfilter > 2)
1256 {
1257 m_iscolliding = true;
1258 m_colliderfilter = 2;
1259
1260 if (m_colliderGroundfilter > 10)
1261 {
1262 m_colliderGroundfilter = 10;
1263 m_freemove = false;
1264 }
1265
1266 m_iscollidingGround = true;
1267
1268 ContactPoint contact = new ContactPoint();
1269 contact.PenetrationDepth = depth;
1270 contact.Position.X = localpos.X;
1271 contact.Position.Y = localpos.Y;
1272 contact.Position.Z = terrainheight;
1273 contact.SurfaceNormal.X = 0.0f;
1274 contact.SurfaceNormal.Y = 0.0f;
1275 contact.SurfaceNormal.Z = -1f;
1276 contact.RelativeSpeed = -vel.Z;
1277 contact.CharacterFeet = true;
1278 AddCollisionEvent(0, contact);
1279
1280 vec.Z *= 0.5f;
1281 }
1282 }
1283
1284 else
1285 {
1286 m_colliderGroundfilter = 0;
1287 m_iscollidingGround = false;
1288 }
1289 }
1290 else
1291 {
1292 m_colliderGroundfilter = 0;
1293 m_iscollidingGround = false;
1294 }
1295
1296
1297 //******************************************
1298
1299 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
1300
1301 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
1302 if (!tviszero)
1303 m_freemove = false;
1304
1305 if (!m_freemove)
1306 {
1307
1308 // if velocity is zero, use position control; otherwise, velocity control
1309 if (tviszero && m_iscolliding)
1310 {
1311 // keep track of where we stopped. No more slippin' & slidin'
1312 if (!_zeroFlag)
1313 {
1314 _zeroFlag = true;
1315 _zeroPosition = localpos;
1316 }
1317 if (m_pidControllerActive)
1318 {
1319 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1320 // react to the physics scene by moving it's position.
1321 // Avatar to Avatar collisions
1322 // Prim to avatar collisions
1323
1324 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
1325 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
1326 if (flying)
1327 {
1328 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1329 }
1330 }
1331 //PidStatus = true;
1332 }
1333 else
1334 {
1335 m_pidControllerActive = true;
1336 _zeroFlag = false;
1337
1338 if (m_iscolliding)
1339 {
1340 if (!flying)
1341 {
1342 if (_target_velocity.Z > 0.0f)
1343 {
1344 // We're colliding with something and we're not flying but we're moving
1345 // This means we're walking or running. JUMPING
1346 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
1347 }
1348 // We're standing on something
1349 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
1350 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
1351 }
1352 else
1353 {
1354 // We're flying and colliding with something
1355 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
1356 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
1357 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1358 }
1359 }
1360 else // ie not colliding
1361 {
1362 if (flying) //(!m_iscolliding && flying)
1363 {
1364 // we're in mid air suspended
1365 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
1366 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
1367 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1368 }
1369
1370 else
1371 {
1372 // we're not colliding and we're not flying so that means we're falling!
1373 // m_iscolliding includes collisions with the ground.
1374
1375 // d.Vector3 pos = d.BodyGetPosition(Body);
1376 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
1377 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
1378 }
1379 }
1380 }
1381
1382 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
1383 {
1384 breakfactor = 0.16f * m_mass;
1385 vec.X -= breakfactor * vel.X;
1386 vec.Y -= breakfactor * vel.Y;
1387 vec.Z -= breakfactor * vel.Z;
1388 }
1389 }
1390 else
1391 {
1392 breakfactor = m_mass;
1393 vec.X -= breakfactor * vel.X;
1394 vec.Y -= breakfactor * vel.Y;
1395 if (flying)
1396 vec.Z -= breakfactor * vel.Z;
1397 else
1398 vec.Z -= .5f* m_mass * vel.Z;
1399 }
1400
1401 if (flying)
1402 {
1403 vec.Z -= _parent_scene.gravityz * m_mass;
1404
1405 //Added for auto fly height. Kitto Flora
1406 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1407
1408 if (localpos.Z < target_altitude)
1409 {
1410 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1411 }
1412 // end add Kitto Flora
1413 }
1414
1415 if (vec.IsFinite())
1416 {
1417 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1418 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1419 }
1420 else
1421 {
1422 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1423 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1424 defects.Add(this);
1425 // _parent_scene.RemoveCharacter(this);
1426 // destroy avatar capsule and related ODE data
1427 AvatarGeomAndBodyDestroy();
1428 return;
1429 }
1430
1431 // update our local ideia of position velocity and aceleration
1432 // _position = localpos;
1433 _position = localpos;
1434
1435 if (_zeroFlag)
1436 {
1437 _velocity = Vector3.Zero;
1438 _acceleration = Vector3.Zero;
1439 m_rotationalVelocity = Vector3.Zero;
1440 }
1441 else
1442 {
1443 Vector3 a =_velocity; // previus velocity
1444 SetSmooth(ref _velocity, ref vel, 2);
1445 a = (_velocity - a) * invtimeStep;
1446 SetSmooth(ref _acceleration, ref a, 2);
1447
1448 dtmp = d.BodyGetAngularVel(Body);
1449 m_rotationalVelocity.X = 0f;
1450 m_rotationalVelocity.Y = 0f;
1451 m_rotationalVelocity.Z = dtmp.Z;
1452 Math.Round(m_rotationalVelocity.Z,3);
1453 }
1454 ajustCollider();
1455 }
1456
1457 public void round(ref Vector3 v, int digits)
1458 {
1459 v.X = (float)Math.Round(v.X, digits);
1460 v.Y = (float)Math.Round(v.Y, digits);
1461 v.Z = (float)Math.Round(v.Z, digits);
1462 }
1463
1464 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1465 {
1466 dst.X = 0.1f * dst.X + 0.9f * value.X;
1467 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1468 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1469 }
1470
1471 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1472 {
1473 dst.X = 0.4f * dst.X + 0.6f * value.X;
1474 dst.X = (float)Math.Round(dst.X, rounddigits);
1475
1476 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1477 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1478
1479 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1480 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1481 }
1482
1483
1484 /// <summary>
1485 /// Updates the reported position and velocity.
1486 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1487 /// also outbounds checking
1488 /// copy and outbounds now done in move(..) at ode rate
1489 ///
1490 /// </summary>
1491 public void UpdatePositionAndVelocity()
1492 {
1493 return;
1494
1495// if (Body == IntPtr.Zero)
1496// return;
1497
1498 }
1499
1500 /// <summary>
1501 /// Cleanup the things we use in the scene.
1502 /// </summary>
1503 public void Destroy()
1504 {
1505 AddChange(changes.Remove, null);
1506 }
1507
1508 public override void CrossingFailure()
1509 {
1510 }
1511
1512 public override Vector3 PIDTarget { set { return; } }
1513 public override bool PIDActive { set { return; } }
1514 public override float PIDTau { set { return; } }
1515
1516 public override float PIDHoverHeight { set { return; } }
1517 public override bool PIDHoverActive { set { return; } }
1518 public override PIDHoverType PIDHoverType { set { return; } }
1519 public override float PIDHoverTau { set { return; } }
1520
1521 public override Quaternion APIDTarget { set { return; } }
1522
1523 public override bool APIDActive { set { return; } }
1524
1525 public override float APIDStrength { set { return; } }
1526
1527 public override float APIDDamping { set { return; } }
1528
1529
1530 public override void SubscribeEvents(int ms)
1531 {
1532 m_eventsubscription = ms;
1533 m_cureventsubscription = 0;
1534 if (CollisionEventsThisFrame == null)
1535 CollisionEventsThisFrame = new CollisionEventUpdate();
1536 SentEmptyCollisionsEvent = false;
1537 }
1538
1539 public override void UnSubscribeEvents()
1540 {
1541 if (CollisionEventsThisFrame != null)
1542 {
1543 lock (CollisionEventsThisFrame)
1544 {
1545 CollisionEventsThisFrame.Clear();
1546 CollisionEventsThisFrame = null;
1547 }
1548 }
1549 m_eventsubscription = 0;
1550 }
1551
1552 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1553 {
1554 if (CollisionEventsThisFrame == null)
1555 CollisionEventsThisFrame = new CollisionEventUpdate();
1556 lock (CollisionEventsThisFrame)
1557 {
1558 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1559 _parent_scene.AddCollisionEventReporting(this);
1560 }
1561 }
1562
1563 public void SendCollisions()
1564 {
1565 if (CollisionEventsThisFrame == null)
1566 return;
1567
1568 lock (CollisionEventsThisFrame)
1569 {
1570 if (m_cureventsubscription < m_eventsubscription)
1571 return;
1572
1573 m_cureventsubscription = 0;
1574
1575 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1576
1577 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1578 {
1579 base.SendCollisionUpdate(CollisionEventsThisFrame);
1580
1581 if (ncolisions == 0)
1582 {
1583 SentEmptyCollisionsEvent = true;
1584 _parent_scene.RemoveCollisionEventReporting(this);
1585 }
1586 else
1587 {
1588 SentEmptyCollisionsEvent = false;
1589 CollisionEventsThisFrame.Clear();
1590 }
1591 }
1592 }
1593 }
1594
1595 internal void AddCollisionFrameTime(int t)
1596 {
1597 // protect it from overflow crashing
1598 if (m_cureventsubscription < 50000)
1599 m_cureventsubscription += t;
1600 }
1601
1602 public override bool SubscribedEvents()
1603 {
1604 if (m_eventsubscription > 0)
1605 return true;
1606 return false;
1607 }
1608
1609 private void changePhysicsStatus(bool NewStatus)
1610 {
1611 if (NewStatus != m_isPhysical)
1612 {
1613 if (NewStatus)
1614 {
1615 AvatarGeomAndBodyDestroy();
1616
1617 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1618
1619 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1620 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1621 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1622 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1623 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1624 _parent_scene.AddCharacter(this);
1625 }
1626 else
1627 {
1628 _parent_scene.RemoveCollisionEventReporting(this);
1629 _parent_scene.RemoveCharacter(this);
1630 // destroy avatar capsule and related ODE data
1631 AvatarGeomAndBodyDestroy();
1632 }
1633 m_freemove = false;
1634 m_isPhysical = NewStatus;
1635 }
1636 }
1637
1638 private void changeAdd()
1639 {
1640 changePhysicsStatus(true);
1641 }
1642
1643 private void changeRemove()
1644 {
1645 changePhysicsStatus(false);
1646 }
1647
1648 private void changeShape(PrimitiveBaseShape arg)
1649 {
1650 }
1651
1652 private void changeAvatarSize(strAvatarSize st)
1653 {
1654 m_feetOffset = st.offset;
1655 changeSize(st.size);
1656 }
1657
1658 private void changeSize(Vector3 pSize)
1659 {
1660 if (pSize.IsFinite())
1661 {
1662 // for now only look to Z changes since viewers also don't change X and Y
1663 if (pSize.Z != m_size.Z)
1664 {
1665 AvatarGeomAndBodyDestroy();
1666
1667
1668 float oldsz = m_size.Z;
1669 m_size = pSize;
1670
1671
1672 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1673 _position.Z + (m_size.Z - oldsz) * 0.5f);
1674
1675 Velocity = Vector3.Zero;
1676
1677
1678 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1679 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1680 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1681 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1682 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1683 }
1684 m_freemove = false;
1685 m_collisionException = false;
1686 m_pidControllerActive = true;
1687 }
1688 else
1689 {
1690 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1691 }
1692 }
1693
1694 private void changePosition( Vector3 newPos)
1695 {
1696 if (Body != IntPtr.Zero)
1697 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1698 _position = newPos;
1699 m_freemove = false;
1700 m_pidControllerActive = true;
1701 }
1702
1703 private void changeOrientation(Quaternion newOri)
1704 {
1705 if (m_orientation != newOri)
1706 {
1707 m_orientation = newOri; // keep a copy for core use
1708 // but only use rotations around Z
1709
1710 m_orientation2D.W = newOri.W;
1711 m_orientation2D.Z = newOri.Z;
1712
1713 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1714 if (t > 0)
1715 {
1716 t = 1.0f / (float)Math.Sqrt(t);
1717 m_orientation2D.W *= t;
1718 m_orientation2D.Z *= t;
1719 }
1720 else
1721 {
1722 m_orientation2D.W = 1.0f;
1723 m_orientation2D.Z = 0f;
1724 }
1725 m_orientation2D.Y = 0f;
1726 m_orientation2D.X = 0f;
1727
1728 d.Quaternion myrot = new d.Quaternion();
1729 myrot.X = m_orientation2D.X;
1730 myrot.Y = m_orientation2D.Y;
1731 myrot.Z = m_orientation2D.Z;
1732 myrot.W = m_orientation2D.W;
1733 d.BodySetQuaternion(Body, ref myrot);
1734 }
1735 }
1736
1737 private void changeVelocity(Vector3 newVel)
1738 {
1739 m_pidControllerActive = true;
1740 m_freemove = false;
1741 _target_velocity = newVel;
1742 }
1743
1744 private void changeSetTorque(Vector3 newTorque)
1745 {
1746 }
1747
1748 private void changeAddForce(Vector3 newForce)
1749 {
1750 }
1751
1752 private void changeAddAngularForce(Vector3 arg)
1753 {
1754 }
1755
1756 private void changeAngularLock(Vector3 arg)
1757 {
1758 }
1759
1760 private void changeFloatOnWater(bool arg)
1761 {
1762 }
1763
1764 private void changeVolumedetetion(bool arg)
1765 {
1766 }
1767
1768 private void changeSelectedStatus(bool arg)
1769 {
1770 }
1771
1772 private void changeDisable(bool arg)
1773 {
1774 }
1775
1776 private void changeBuilding(bool arg)
1777 {
1778 }
1779
1780 private void setFreeMove()
1781 {
1782 m_pidControllerActive = true;
1783 _zeroFlag = false;
1784 _target_velocity = Vector3.Zero;
1785 m_freemove = true;
1786 m_colliderfilter = -2;
1787 m_colliderObjectfilter = -2;
1788 m_colliderGroundfilter = -2;
1789
1790 m_iscolliding = false;
1791 m_iscollidingGround = false;
1792 m_iscollidingObj = false;
1793
1794 CollisionEventsThisFrame.Clear();
1795 }
1796
1797 private void changeForce(Vector3 newForce)
1798 {
1799 setFreeMove();
1800
1801 if (Body != IntPtr.Zero)
1802 {
1803 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1804 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1805 }
1806 }
1807
1808 // for now momentum is actually velocity
1809 private void changeMomentum(Vector3 newmomentum)
1810 {
1811 _velocity = newmomentum;
1812 setFreeMove();
1813
1814 if (Body != IntPtr.Zero)
1815 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1816 ajustCollider();
1817 }
1818
1819 private void donullchange()
1820 {
1821 }
1822
1823 public bool DoAChange(changes what, object arg)
1824 {
1825 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1826 {
1827 return false;
1828 }
1829
1830 // nasty switch
1831 switch (what)
1832 {
1833 case changes.Add:
1834 changeAdd();
1835 break;
1836 case changes.Remove:
1837 changeRemove();
1838 break;
1839
1840 case changes.Position:
1841 changePosition((Vector3)arg);
1842 break;
1843
1844 case changes.Orientation:
1845 changeOrientation((Quaternion)arg);
1846 break;
1847
1848 case changes.PosOffset:
1849 donullchange();
1850 break;
1851
1852 case changes.OriOffset:
1853 donullchange();
1854 break;
1855
1856 case changes.Velocity:
1857 changeVelocity((Vector3)arg);
1858 break;
1859
1860 // case changes.Acceleration:
1861 // changeacceleration((Vector3)arg);
1862 // break;
1863 // case changes.AngVelocity:
1864 // changeangvelocity((Vector3)arg);
1865 // break;
1866
1867 case changes.Force:
1868 changeForce((Vector3)arg);
1869 break;
1870
1871 case changes.Torque:
1872 changeSetTorque((Vector3)arg);
1873 break;
1874
1875 case changes.AddForce:
1876 changeAddForce((Vector3)arg);
1877 break;
1878
1879 case changes.AddAngForce:
1880 changeAddAngularForce((Vector3)arg);
1881 break;
1882
1883 case changes.AngLock:
1884 changeAngularLock((Vector3)arg);
1885 break;
1886
1887 case changes.Size:
1888 changeSize((Vector3)arg);
1889 break;
1890
1891 case changes.AvatarSize:
1892 changeAvatarSize((strAvatarSize)arg);
1893 break;
1894
1895 case changes.Momentum:
1896 changeMomentum((Vector3)arg);
1897 break;
1898/* not in use for now
1899 case changes.Shape:
1900 changeShape((PrimitiveBaseShape)arg);
1901 break;
1902
1903 case changes.CollidesWater:
1904 changeFloatOnWater((bool)arg);
1905 break;
1906
1907 case changes.VolumeDtc:
1908 changeVolumedetetion((bool)arg);
1909 break;
1910
1911 case changes.Physical:
1912 changePhysicsStatus((bool)arg);
1913 break;
1914
1915 case changes.Selected:
1916 changeSelectedStatus((bool)arg);
1917 break;
1918
1919 case changes.disabled:
1920 changeDisable((bool)arg);
1921 break;
1922
1923 case changes.building:
1924 changeBuilding((bool)arg);
1925 break;
1926*/
1927 case changes.Null:
1928 donullchange();
1929 break;
1930
1931 default:
1932 donullchange();
1933 break;
1934 }
1935 return false;
1936 }
1937
1938 public void AddChange(changes what, object arg)
1939 {
1940 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1941 }
1942
1943 private struct strAvatarSize
1944 {
1945 public Vector3 size;
1946 public float offset;
1947 }
1948
1949 }
1950}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..a7dda7a
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1087 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30 * ODEPrim.cs contains methods dealing with Prim editing, Prim
31 * characteristics and Kinetic motion.
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use.
39 */
40
41// Extensive change Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager;
52
53namespace OpenSim.Region.Physics.OdePlugin
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private OdeScene _pParentScene;
64
65 // Vehicle properties
66 // WARNING this are working copies for internel use
67 // their values may not be the corresponding parameter
68
69 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71
72 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73
74 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75 // HOVER_TERRAIN_ONLY
76 // HOVER_GLOBAL_HEIGHT
77 // NO_DEFLECTION_UP
78 // HOVER_WATER_ONLY
79 // HOVER_UP_ONLY
80 // LIMIT_MOTOR_UP
81 // LIMIT_ROLL_ONLY
82 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83
84 // Linear properties
85 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87 private float m_linearMotorDecayTimescale = 120;
88 private float m_linearMotorTimescale = 1000;
89 private Vector3 m_linearMotorOffset = Vector3.Zero;
90
91 //Angular properties
92 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96
97 //Deflection properties
98 private float m_angularDeflectionEfficiency = 0;
99 private float m_angularDeflectionTimescale = 1000;
100 private float m_linearDeflectionEfficiency = 0;
101 private float m_linearDeflectionTimescale = 1000;
102
103 //Banking properties
104 private float m_bankingEfficiency = 0;
105 private float m_bankingMix = 0;
106 private float m_bankingTimescale = 1000;
107
108 //Hover and Buoyancy properties
109 private float m_VhoverHeight = 0f;
110 private float m_VhoverEfficiency = 0f;
111 private float m_VhoverTimescale = 1000f;
112 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116
117 //Attractor properties
118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120
121
122 // auxiliar
123 private float m_lmEfect = 0f; // current linear motor eficiency
124 private float m_lmDecay = 0f; // current linear decay
125
126 private float m_amEfect = 0; // current angular motor eficiency
127 private float m_amDecay = 0f; // current linear decay
128
129 private float m_ffactor = 1.0f;
130
131 private float m_timestep = 0.02f;
132 private float m_invtimestep = 50;
133
134
135 float m_ampwr;
136 float m_amdampX;
137 float m_amdampY;
138 float m_amdampZ;
139
140
141 public float FrictionFactor
142 {
143 get
144 {
145 return m_ffactor;
146 }
147 }
148
149
150 public ODEDynamics(OdePrim rootp)
151 {
152 rootPrim = rootp;
153 _pParentScene = rootPrim._parent_scene;
154 m_timestep = _pParentScene.ODE_STEPSIZE;
155 m_invtimestep = 1.0f / m_timestep;
156 }
157
158 public void DoSetVehicle(VehicleData vd)
159 {
160 m_type = vd.m_type;
161 m_flags = vd.m_flags;
162
163
164 // Linear properties
165 m_linearMotorDirection = vd.m_linearMotorDirection;
166
167 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
168 if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
169 if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
170 if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
171
172 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
173 if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
174 m_linearMotorDecayTimescale += 0.2f;
175 m_linearMotorDecayTimescale *= m_invtimestep;
176
177 m_linearMotorTimescale = vd.m_linearMotorTimescale;
178 if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
179
180 m_linearMotorOffset = vd.m_linearMotorOffset;
181
182 //Angular properties
183 m_angularMotorDirection = vd.m_angularMotorDirection;
184 m_angularMotorTimescale = vd.m_angularMotorTimescale;
185 if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
186
187 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
188 if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
189 m_angularMotorDecayTimescale *= m_invtimestep;
190
191 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
192 if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
193 if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
194 if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
195
196 //Deflection properties
197 m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
198 m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
199 if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
200
201 m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
202 m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
203 if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
204
205 //Banking properties
206 m_bankingEfficiency = vd.m_bankingEfficiency;
207 m_bankingMix = vd.m_bankingMix;
208 m_bankingTimescale = vd.m_bankingTimescale;
209 if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
210
211 //Hover and Buoyancy properties
212 m_VhoverHeight = vd.m_VhoverHeight;
213 m_VhoverEfficiency = vd.m_VhoverEfficiency;
214 m_VhoverTimescale = vd.m_VhoverTimescale;
215 if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
216
217 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
218
219 //Attractor properties
220 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
221 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
222 if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
223
224 // Axis
225 m_referenceFrame = vd.m_referenceFrame;
226
227 m_lmEfect = 0;
228 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
229 m_amEfect = 0;
230 m_ffactor = 1.0f;
231 }
232
233 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
234 {
235 float len;
236
237 switch (pParam)
238 {
239 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
240 if (pValue < 0f) pValue = 0f;
241 if (pValue > 1f) pValue = 1f;
242 m_angularDeflectionEfficiency = pValue;
243 break;
244 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
245 if (pValue < m_timestep) pValue = m_timestep;
246 m_angularDeflectionTimescale = pValue;
247 break;
248 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
249 if (pValue < m_timestep) pValue = m_timestep;
250 else if (pValue > 120) pValue = 120;
251 m_angularMotorDecayTimescale = pValue * m_invtimestep;
252 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
253 break;
254 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
255 if (pValue < m_timestep) pValue = m_timestep;
256 m_angularMotorTimescale = pValue;
257 break;
258 case Vehicle.BANKING_EFFICIENCY:
259 if (pValue < -1f) pValue = -1f;
260 if (pValue > 1f) pValue = 1f;
261 m_bankingEfficiency = pValue;
262 break;
263 case Vehicle.BANKING_MIX:
264 if (pValue < 0f) pValue = 0f;
265 if (pValue > 1f) pValue = 1f;
266 m_bankingMix = pValue;
267 break;
268 case Vehicle.BANKING_TIMESCALE:
269 if (pValue < m_timestep) pValue = m_timestep;
270 m_bankingTimescale = pValue;
271 break;
272 case Vehicle.BUOYANCY:
273 if (pValue < -1f) pValue = -1f;
274 if (pValue > 1f) pValue = 1f;
275 m_VehicleBuoyancy = pValue;
276 break;
277 case Vehicle.HOVER_EFFICIENCY:
278 if (pValue < 0f) pValue = 0f;
279 if (pValue > 1f) pValue = 1f;
280 m_VhoverEfficiency = pValue;
281 break;
282 case Vehicle.HOVER_HEIGHT:
283 m_VhoverHeight = pValue;
284 break;
285 case Vehicle.HOVER_TIMESCALE:
286 if (pValue < m_timestep) pValue = m_timestep;
287 m_VhoverTimescale = pValue;
288 break;
289 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
290 if (pValue < 0f) pValue = 0f;
291 if (pValue > 1f) pValue = 1f;
292 m_linearDeflectionEfficiency = pValue;
293 break;
294 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
295 if (pValue < m_timestep) pValue = m_timestep;
296 m_linearDeflectionTimescale = pValue;
297 break;
298 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
299 if (pValue < m_timestep) pValue = m_timestep;
300 else if (pValue > 120) pValue = 120;
301 m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
302 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
303 break;
304 case Vehicle.LINEAR_MOTOR_TIMESCALE:
305 if (pValue < m_timestep) pValue = m_timestep;
306 m_linearMotorTimescale = pValue;
307 break;
308 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
309 if (pValue < 0f) pValue = 0f;
310 if (pValue > 1f) pValue = 1f;
311 m_verticalAttractionEfficiency = pValue;
312 break;
313 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
314 if (pValue < m_timestep) pValue = m_timestep;
315 m_verticalAttractionTimescale = pValue;
316 break;
317
318 // These are vector properties but the engine lets you use a single float value to
319 // set all of the components to the same value
320 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
321 if (pValue < m_timestep) pValue = m_timestep;
322 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
323 break;
324 case Vehicle.ANGULAR_MOTOR_DIRECTION:
325 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
326 len = m_angularMotorDirection.Length();
327 if (len > 12.566f)
328 m_angularMotorDirection *= (12.566f / len);
329
330 m_amEfect = 1.0f ; // turn it on
331 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
332
333 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
334 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
335 d.BodyEnable(rootPrim.Body);
336 break;
337 case Vehicle.LINEAR_FRICTION_TIMESCALE:
338 if (pValue < m_timestep) pValue = m_timestep;
339 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
340 break;
341 case Vehicle.LINEAR_MOTOR_DIRECTION:
342 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
343 len = m_linearMotorDirection.Length();
344 if (len > 100.0f)
345 m_linearMotorDirection *= (100.0f / len);
346
347 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
348 m_lmEfect = 1.0f; // turn it on
349
350 m_ffactor = 0.0f;
351 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
352 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
353 d.BodyEnable(rootPrim.Body);
354 break;
355 case Vehicle.LINEAR_MOTOR_OFFSET:
356 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
357 len = m_linearMotorOffset.Length();
358 if (len > 100.0f)
359 m_linearMotorOffset *= (100.0f / len);
360 break;
361 }
362 }//end ProcessFloatVehicleParam
363
364 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
365 {
366 float len;
367
368 switch (pParam)
369 {
370 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
371 if (pValue.X < m_timestep) pValue.X = m_timestep;
372 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
373 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
374
375 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
376 break;
377 case Vehicle.ANGULAR_MOTOR_DIRECTION:
378 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
379 // Limit requested angular speed to 2 rps= 4 pi rads/sec
380 len = m_angularMotorDirection.Length();
381 if (len > 12.566f)
382 m_angularMotorDirection *= (12.566f / len);
383
384 m_amEfect = 1.0f; // turn it on
385 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
386
387 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
388 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
389 d.BodyEnable(rootPrim.Body);
390 break;
391 case Vehicle.LINEAR_FRICTION_TIMESCALE:
392 if (pValue.X < m_timestep) pValue.X = m_timestep;
393 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
394 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
395 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
396 break;
397 case Vehicle.LINEAR_MOTOR_DIRECTION:
398 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
399 len = m_linearMotorDirection.Length();
400 if (len > 100.0f)
401 m_linearMotorDirection *= (100.0f / len);
402
403 m_lmEfect = 1.0f; // turn it on
404 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
405
406 m_ffactor = 0.0f;
407 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
408 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
409 d.BodyEnable(rootPrim.Body);
410 break;
411 case Vehicle.LINEAR_MOTOR_OFFSET:
412 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
413 len = m_linearMotorOffset.Length();
414 if (len > 100.0f)
415 m_linearMotorOffset *= (100.0f / len);
416 break;
417 case Vehicle.BLOCK_EXIT:
418 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
419 break;
420 }
421 }//end ProcessVectorVehicleParam
422
423 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
424 {
425 switch (pParam)
426 {
427 case Vehicle.REFERENCE_FRAME:
428 // m_referenceFrame = Quaternion.Inverse(pValue);
429 m_referenceFrame = pValue;
430 break;
431 case Vehicle.ROLL_FRAME:
432 m_RollreferenceFrame = pValue;
433 break;
434 }
435 }//end ProcessRotationVehicleParam
436
437 internal void ProcessVehicleFlags(int pParam, bool remove)
438 {
439 if (remove)
440 {
441 m_flags &= ~((VehicleFlag)pParam);
442 }
443 else
444 {
445 m_flags |= (VehicleFlag)pParam;
446 }
447 }//end ProcessVehicleFlags
448
449 internal void ProcessTypeChange(Vehicle pType)
450 {
451 m_lmEfect = 0;
452
453 m_amEfect = 0;
454 m_ffactor = 1f;
455
456 m_linearMotorDirection = Vector3.Zero;
457 m_angularMotorDirection = Vector3.Zero;
458
459 m_BlockingEndPoint = Vector3.Zero;
460 m_RollreferenceFrame = Quaternion.Identity;
461 m_linearMotorOffset = Vector3.Zero;
462
463 m_referenceFrame = Quaternion.Identity;
464
465 // Set Defaults For Type
466 m_type = pType;
467 switch (pType)
468 {
469 case Vehicle.TYPE_NONE:
470 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
471 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
472 m_linearMotorTimescale = 1000;
473 m_linearMotorDecayTimescale = 120 * m_invtimestep;
474 m_angularMotorTimescale = 1000;
475 m_angularMotorDecayTimescale = 1000 * m_invtimestep;
476 m_VhoverHeight = 0;
477 m_VhoverEfficiency = 1;
478 m_VhoverTimescale = 1000;
479 m_VehicleBuoyancy = 0;
480 m_linearDeflectionEfficiency = 0;
481 m_linearDeflectionTimescale = 1000;
482 m_angularDeflectionEfficiency = 0;
483 m_angularDeflectionTimescale = 1000;
484 m_bankingEfficiency = 0;
485 m_bankingMix = 1;
486 m_bankingTimescale = 1000;
487 m_verticalAttractionEfficiency = 0;
488 m_verticalAttractionTimescale = 1000;
489
490 m_flags = (VehicleFlag)0;
491 break;
492
493 case Vehicle.TYPE_SLED:
494 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
495 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
496 m_linearMotorTimescale = 1000;
497 m_linearMotorDecayTimescale = 120 * m_invtimestep;
498 m_angularMotorTimescale = 1000;
499 m_angularMotorDecayTimescale = 120 * m_invtimestep;
500 m_VhoverHeight = 0;
501 m_VhoverEfficiency = 1;
502 m_VhoverTimescale = 10;
503 m_VehicleBuoyancy = 0;
504 m_linearDeflectionEfficiency = 1;
505 m_linearDeflectionTimescale = 1;
506 m_angularDeflectionEfficiency = 0;
507 m_angularDeflectionTimescale = 10;
508 m_verticalAttractionEfficiency = 1;
509 m_verticalAttractionTimescale = 1000;
510 m_bankingEfficiency = 0;
511 m_bankingMix = 1;
512 m_bankingTimescale = 10;
513 m_flags &=
514 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
515 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
516 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
517 VehicleFlag.LIMIT_ROLL_ONLY |
518 VehicleFlag.LIMIT_MOTOR_UP);
519 break;
520
521 case Vehicle.TYPE_CAR:
522 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
523 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
524 m_linearMotorTimescale = 1;
525 m_linearMotorDecayTimescale = 60 * m_invtimestep;
526 m_angularMotorTimescale = 1;
527 m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
528 m_VhoverHeight = 0;
529 m_VhoverEfficiency = 0;
530 m_VhoverTimescale = 1000;
531 m_VehicleBuoyancy = 0;
532 m_linearDeflectionEfficiency = 1;
533 m_linearDeflectionTimescale = 2;
534 m_angularDeflectionEfficiency = 0;
535 m_angularDeflectionTimescale = 10;
536 m_verticalAttractionEfficiency = 1f;
537 m_verticalAttractionTimescale = 10f;
538 m_bankingEfficiency = -0.2f;
539 m_bankingMix = 1;
540 m_bankingTimescale = 1;
541 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
542 VehicleFlag.HOVER_TERRAIN_ONLY |
543 VehicleFlag.HOVER_GLOBAL_HEIGHT);
544 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
545 VehicleFlag.LIMIT_ROLL_ONLY |
546 VehicleFlag.LIMIT_MOTOR_UP |
547 VehicleFlag.HOVER_UP_ONLY);
548 break;
549 case Vehicle.TYPE_BOAT:
550 m_linearFrictionTimescale = new Vector3(10, 3, 2);
551 m_angularFrictionTimescale = new Vector3(10, 10, 10);
552 m_linearMotorTimescale = 5;
553 m_linearMotorDecayTimescale = 60 * m_invtimestep;
554 m_angularMotorTimescale = 4;
555 m_angularMotorDecayTimescale = 4 * m_invtimestep;
556 m_VhoverHeight = 0;
557 m_VhoverEfficiency = 0.5f;
558 m_VhoverTimescale = 2;
559 m_VehicleBuoyancy = 1;
560 m_linearDeflectionEfficiency = 0.5f;
561 m_linearDeflectionTimescale = 3;
562 m_angularDeflectionEfficiency = 0.5f;
563 m_angularDeflectionTimescale = 5;
564 m_verticalAttractionEfficiency = 0.5f;
565 m_verticalAttractionTimescale = 5f;
566 m_bankingEfficiency = -0.3f;
567 m_bankingMix = 0.8f;
568 m_bankingTimescale = 1;
569 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
570 VehicleFlag.HOVER_GLOBAL_HEIGHT |
571 VehicleFlag.HOVER_UP_ONLY); // |
572// VehicleFlag.LIMIT_ROLL_ONLY);
573 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
574 VehicleFlag.LIMIT_MOTOR_UP |
575 VehicleFlag.HOVER_UP_ONLY | // new sl
576 VehicleFlag.HOVER_WATER_ONLY);
577 break;
578
579 case Vehicle.TYPE_AIRPLANE:
580 m_linearFrictionTimescale = new Vector3(200, 10, 5);
581 m_angularFrictionTimescale = new Vector3(20, 20, 20);
582 m_linearMotorTimescale = 2;
583 m_linearMotorDecayTimescale = 60 * m_invtimestep;
584 m_angularMotorTimescale = 4;
585 m_angularMotorDecayTimescale = 8 * m_invtimestep;
586 m_VhoverHeight = 0;
587 m_VhoverEfficiency = 0.5f;
588 m_VhoverTimescale = 1000;
589 m_VehicleBuoyancy = 0;
590 m_linearDeflectionEfficiency = 0.5f;
591 m_linearDeflectionTimescale = 0.5f;
592 m_angularDeflectionEfficiency = 1;
593 m_angularDeflectionTimescale = 2;
594 m_verticalAttractionEfficiency = 0.9f;
595 m_verticalAttractionTimescale = 2f;
596 m_bankingEfficiency = 1;
597 m_bankingMix = 0.7f;
598 m_bankingTimescale = 2;
599 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
600 VehicleFlag.HOVER_TERRAIN_ONLY |
601 VehicleFlag.HOVER_GLOBAL_HEIGHT |
602 VehicleFlag.HOVER_UP_ONLY |
603 VehicleFlag.NO_DEFLECTION_UP |
604 VehicleFlag.LIMIT_MOTOR_UP);
605 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
606 break;
607
608 case Vehicle.TYPE_BALLOON:
609 m_linearFrictionTimescale = new Vector3(5, 5, 5);
610 m_angularFrictionTimescale = new Vector3(10, 10, 10);
611 m_linearMotorTimescale = 5;
612 m_linearMotorDecayTimescale = 60 * m_invtimestep;
613 m_angularMotorTimescale = 6;
614 m_angularMotorDecayTimescale = 10 * m_invtimestep;
615 m_VhoverHeight = 5;
616 m_VhoverEfficiency = 0.8f;
617 m_VhoverTimescale = 10;
618 m_VehicleBuoyancy = 1;
619 m_linearDeflectionEfficiency = 0;
620 m_linearDeflectionTimescale = 5 * m_invtimestep;
621 m_angularDeflectionEfficiency = 0;
622 m_angularDeflectionTimescale = 5;
623 m_verticalAttractionEfficiency = 1f;
624 m_verticalAttractionTimescale = 1000f;
625 m_bankingEfficiency = 0;
626 m_bankingMix = 0.7f;
627 m_bankingTimescale = 5;
628 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
629 VehicleFlag.HOVER_TERRAIN_ONLY |
630 VehicleFlag.HOVER_UP_ONLY |
631 VehicleFlag.NO_DEFLECTION_UP |
632 VehicleFlag.LIMIT_MOTOR_UP | //);
633 VehicleFlag.LIMIT_ROLL_ONLY | // new sl
634 VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
635
636// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
637// VehicleFlag.HOVER_GLOBAL_HEIGHT);
638 break;
639
640 }
641
642 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
643 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
644
645 }//end SetDefaultsForType
646
647 internal void Stop()
648 {
649 m_lmEfect = 0;
650 m_lmDecay = 0f;
651 m_amEfect = 0;
652 m_amDecay = 0;
653 m_ffactor = 1f;
654 }
655
656 public static Vector3 Xrot(Quaternion rot)
657 {
658 Vector3 vec;
659 rot.Normalize(); // just in case
660 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
661 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
662 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
663 return vec;
664 }
665
666 public static Vector3 Zrot(Quaternion rot)
667 {
668 Vector3 vec;
669 rot.Normalize(); // just in case
670 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
671 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
672 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
673
674 return vec;
675 }
676
677 private const float pi = (float)Math.PI;
678 private const float halfpi = 0.5f * (float)Math.PI;
679 private const float twopi = 2.0f * pi;
680
681 public static Vector3 ubitRot2Euler(Quaternion rot)
682 {
683 // returns roll in X
684 // pitch in Y
685 // yaw in Z
686 Vector3 vec;
687
688 // assuming rot is normalised
689 // rot.Normalize();
690
691 float zX = rot.X * rot.Z + rot.Y * rot.W;
692
693 if (zX < -0.49999f)
694 {
695 vec.X = 0;
696 vec.Y = -halfpi;
697 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
698 }
699 else if (zX > 0.49999f)
700 {
701 vec.X = 0;
702 vec.Y = halfpi;
703 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
704 }
705 else
706 {
707 vec.Y = (float)Math.Asin(2 * zX);
708
709 float sqw = rot.W * rot.W;
710
711 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
712 float zZ = rot.Z * rot.Z + sqw - 0.5f;
713
714 vec.X = (float)Math.Atan2(minuszY, zZ);
715
716 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
717 float yY = rot.X * rot.X + sqw - 0.5f;
718 vec.Z = (float)Math.Atan2(yX, yY);
719 }
720 return vec;
721 }
722
723 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
724 {
725 // assuming rot is normalised
726 // rot.Normalize();
727
728 float zX = rot.X * rot.Z + rot.Y * rot.W;
729
730 if (zX < -0.49999f)
731 {
732 roll = 0;
733 pitch = -halfpi;
734 }
735 else if (zX > 0.49999f)
736 {
737 roll = 0;
738 pitch = halfpi;
739 }
740 else
741 {
742 pitch = (float)Math.Asin(2 * zX);
743
744 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
745 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
746
747 roll = (float)Math.Atan2(minuszY, zZ);
748 }
749 return ;
750 }
751
752 internal void Step()
753 {
754 IntPtr Body = rootPrim.Body;
755
756 d.Mass dmass;
757 d.BodyGetMass(Body, out dmass);
758
759 d.Quaternion rot = d.BodyGetQuaternion(Body);
760 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
761 Quaternion rotq = objrotq; // rotq = rotation of object
762 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
763 Quaternion irotq = Quaternion.Inverse(rotq);
764
765 d.Vector3 dvtmp;
766 Vector3 tmpV;
767 Vector3 curVel; // velocity in world
768 Vector3 curAngVel; // angular velocity in world
769 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
770 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
771 d.Vector3 dtorque = new d.Vector3();
772
773 dvtmp = d.BodyGetLinearVel(Body);
774 curVel.X = dvtmp.X;
775 curVel.Y = dvtmp.Y;
776 curVel.Z = dvtmp.Z;
777 Vector3 curLocalVel = curVel * irotq; // current velocity in local
778
779 dvtmp = d.BodyGetAngularVel(Body);
780 curAngVel.X = dvtmp.X;
781 curAngVel.Y = dvtmp.Y;
782 curAngVel.Z = dvtmp.Z;
783 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
784
785 float ldampZ = 0;
786
787 // linear motor
788 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
789 {
790 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
791 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
792 tmpV *= rotq; // to world
793
794 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
795 tmpV.Z = 0;
796
797 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
798 {
799 // have offset, do it now
800 tmpV *= dmass.mass;
801 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
802 }
803 else
804 {
805 force.X += tmpV.X;
806 force.Y += tmpV.Y;
807 force.Z += tmpV.Z;
808 }
809
810 m_lmEfect *= m_lmDecay;
811// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
812 m_ffactor = 0.0f;
813 }
814 else
815 {
816 m_lmEfect = 0;
817 m_ffactor = 1f;
818 }
819
820 // hover
821 if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
822 {
823 // d.Vector3 pos = d.BodyGetPosition(Body);
824 d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
825 pos.Z -= 0.21f; // minor offset that seems to be always there in sl
826
827 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
828 float perr;
829
830 // default to global but don't go underground
831 perr = m_VhoverHeight - pos.Z;
832
833 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
834 {
835 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
836 {
837 perr += _pParentScene.GetWaterLevel();
838 }
839 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
840 {
841 perr += t;
842 }
843 else
844 {
845 float w = _pParentScene.GetWaterLevel();
846 if (t > w)
847 perr += t;
848 else
849 perr += w;
850 }
851 }
852 else if (t > m_VhoverHeight)
853 perr = t - pos.Z; ;
854
855 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
856 {
857 ldampZ = m_VhoverEfficiency * m_invtimestep;
858
859 perr *= (1.0f + ldampZ) / m_VhoverTimescale;
860
861 // force.Z += perr - curVel.Z * tmp;
862 force.Z += perr;
863 ldampZ *= -curVel.Z;
864
865 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
866 }
867 else // no buoyancy
868 force.Z += _pParentScene.gravityz;
869 }
870 else
871 {
872 // default gravity and Buoyancy
873 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
874 }
875
876 // linear deflection
877 if (m_linearDeflectionEfficiency > 0)
878 {
879 float len = curVel.Length();
880 if (len > 0.01) // if moving
881 {
882 Vector3 atAxis;
883 atAxis = Xrot(rotq); // where are we pointing to
884 atAxis *= len; // make it same size as world velocity vector
885
886 tmpV = -atAxis; // oposite direction
887 atAxis -= curVel; // error to one direction
888 len = atAxis.LengthSquared();
889
890 tmpV -= curVel; // error to oposite
891 float lens = tmpV.LengthSquared();
892
893 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
894 {
895 if (len < lens)
896 tmpV = atAxis;
897
898 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
899 force.X += tmpV.X;
900 force.Y += tmpV.Y;
901 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
902 force.Z += tmpV.Z;
903 }
904 }
905 }
906
907 // linear friction/damping
908 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
909 {
910 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
911 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
912 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
913 tmpV *= rotq; // to world
914
915 if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
916 tmpV.Z = ldampZ;
917 force.X += tmpV.X;
918 force.Y += tmpV.Y;
919 force.Z += tmpV.Z;
920 }
921
922 // vertical atractor
923 if (m_verticalAttractionTimescale < 300)
924 {
925 float roll;
926 float pitch;
927
928
929
930 float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
931
932 float ftmp2;
933 ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
934 m_amdampX = ftmp2;
935
936 m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
937
938 GetRollPitch(irotq, out roll, out pitch);
939
940 if (roll > halfpi)
941 roll = pi - roll;
942 else if (roll < -halfpi)
943 roll = -pi - roll;
944
945 float effroll = pitch / halfpi;
946 effroll *= effroll;
947 effroll = 1 - effroll;
948 effroll *= roll;
949
950
951 torque.X += effroll * ftmp;
952
953 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
954 {
955 float effpitch = roll / halfpi;
956 effpitch *= effpitch;
957 effpitch = 1 - effpitch;
958 effpitch *= pitch;
959
960 torque.Y += effpitch * ftmp;
961 }
962
963 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
964 {
965
966 float broll = effroll;
967 /*
968 if (broll > halfpi)
969 broll = pi - broll;
970 else if (broll < -halfpi)
971 broll = -pi - broll;
972 */
973 broll *= m_bankingEfficiency;
974 if (m_bankingMix != 0)
975 {
976 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
977 if (vfact > 1.0f) vfact = 1.0f;
978
979 if (curLocalVel.X >= 0)
980 broll *= (1 + (vfact - 1) * m_bankingMix);
981 else
982 broll *= -(1 + (vfact - 1) * m_bankingMix);
983 }
984 // make z rot be in world Z not local as seems to be in sl
985
986 broll = broll / m_bankingTimescale;
987
988
989 tmpV = Zrot(irotq);
990 tmpV *= broll;
991
992 torque.X += tmpV.X;
993 torque.Y += tmpV.Y;
994 torque.Z += tmpV.Z;
995
996 m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
997 m_amdampY = m_amdampZ;
998
999 }
1000 else
1001 {
1002 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1003 m_amdampY = m_amdampX;
1004 }
1005 }
1006 else
1007 {
1008 m_ampwr = 1.0f;
1009 m_amdampX = 1 / m_angularFrictionTimescale.X;
1010 m_amdampY = 1 / m_angularFrictionTimescale.Y;
1011 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1012 }
1013
1014 // angular motor
1015 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
1016 {
1017 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
1018 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
1019 torque.X += tmpV.X * m_ampwr;
1020 torque.Y += tmpV.Y * m_ampwr;
1021 torque.Z += tmpV.Z;
1022
1023 m_amEfect *= m_amDecay;
1024 }
1025 else
1026 m_amEfect = 0;
1027
1028 // angular deflection
1029 if (m_angularDeflectionEfficiency > 0)
1030 {
1031 Vector3 dirv;
1032
1033 if (curLocalVel.X > 0.01f)
1034 dirv = curLocalVel;
1035 else if (curLocalVel.X < -0.01f)
1036 // use oposite
1037 dirv = -curLocalVel;
1038 else
1039 {
1040 // make it fall into small positive x case
1041 dirv.X = 0.01f;
1042 dirv.Y = curLocalVel.Y;
1043 dirv.Z = curLocalVel.Z;
1044 }
1045
1046 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1047
1048 if (Math.Abs(dirv.Z) > 0.01)
1049 {
1050 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
1051 }
1052
1053 if (Math.Abs(dirv.Y) > 0.01)
1054 {
1055 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
1056 }
1057 }
1058
1059 // angular friction
1060 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1061 {
1062 torque.X -= curLocalAngVel.X * m_amdampX;
1063 torque.Y -= curLocalAngVel.Y * m_amdampY;
1064 torque.Z -= curLocalAngVel.Z * m_amdampZ;
1065 }
1066
1067
1068
1069 if (force.X != 0 || force.Y != 0 || force.Z != 0)
1070 {
1071 force *= dmass.mass;
1072 d.BodyAddForce(Body, force.X, force.Y, force.Z);
1073 }
1074
1075 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1076 {
1077 torque *= m_referenceFrame; // to object frame
1078 dtorque.X = torque.X ;
1079 dtorque.Y = torque.Y;
1080 dtorque.Z = torque.Z;
1081
1082 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
1083 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1084 }
1085 }
1086 }
1087}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
new file mode 100644
index 0000000..0df71eb
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEMeshWorker.cs
@@ -0,0 +1,932 @@
1/*
2 * AJLDuarte 2012
3 */
4
5using System;
6using System.Threading;
7using System.Collections.Generic;
8using System.IO;
9using System.Reflection;
10using System.Runtime.InteropServices;
11using System.Text;
12using OpenSim.Framework;
13using OpenSim.Region.Physics.Manager;
14using OdeAPI;
15using log4net;
16using Nini.Config;
17using OpenMetaverse;
18
19namespace OpenSim.Region.Physics.OdePlugin
20{
21 public enum MeshState : byte
22 {
23 noNeed = 0,
24
25 loadingAsset = 1,
26
27 AssetOK = 0x0f, // 00001111
28
29 NeedMask = 0x30, // 00110000
30 needMesh = 0x10, // 00010000
31 needAsset = 0x20, // 00100000
32
33 FailMask = 0xC0, // 11000000
34 AssetFailed = 0x40, // 01000000
35 MeshFailed = 0x80, // 10000000
36
37 MeshNoColide = FailMask | needAsset
38 }
39
40 public enum meshWorkerCmnds : byte
41 {
42 nop = 0,
43 addnew,
44 changefull,
45 changesize,
46 changeshapetype,
47 getmesh,
48 }
49
50 public class ODEPhysRepData
51 {
52 public PhysicsActor actor;
53 public PrimitiveBaseShape pbs;
54 public IMesh mesh;
55
56 public Vector3 size;
57 public Vector3 OBB;
58 public Vector3 OBBOffset;
59
60 public float volume;
61
62 public byte shapetype;
63 public bool hasOBB;
64 public bool hasMeshVolume;
65 public MeshState meshState;
66 public UUID? assetID;
67 public meshWorkerCmnds comand;
68 }
69
70 public class ODEMeshWorker
71 {
72
73 private ILog m_log;
74 private OdeScene m_scene;
75 private IMesher m_mesher;
76
77 public bool meshSculptedPrim = true;
78 public bool forceSimplePrimMeshing = false;
79 public float meshSculptLOD = 32;
80 public float MeshSculptphysicalLOD = 32;
81
82
83 private OpenSim.Framework.BlockingQueue<ODEPhysRepData> createqueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>();
84 private bool m_running;
85
86 private Thread m_thread;
87
88 public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig)
89 {
90 m_scene = pScene;
91 m_log = pLog;
92 m_mesher = pMesher;
93
94 if (pConfig != null)
95 {
96 forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
97 meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
98 meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD);
99 MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
100 }
101 m_running = true;
102 m_thread = new Thread(DoWork);
103 m_thread.Start();
104 }
105
106 private void DoWork()
107 {
108 m_mesher.ExpireFileCache();
109
110 while(m_running)
111 {
112 ODEPhysRepData nextRep = createqueue.Dequeue();
113 if(!m_running)
114 return;
115 if (nextRep == null)
116 continue;
117 if (m_scene.haveActor(nextRep.actor))
118 {
119 switch (nextRep.comand)
120 {
121 case meshWorkerCmnds.changefull:
122 case meshWorkerCmnds.changeshapetype:
123 case meshWorkerCmnds.changesize:
124 GetMesh(nextRep);
125 if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor))
126 m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep);
127 break;
128 case meshWorkerCmnds.getmesh:
129 DoRepDataGetMesh(nextRep);
130 break;
131 }
132 }
133 }
134 }
135
136 public void Stop()
137 {
138 try
139 {
140 m_thread.Abort();
141 createqueue.Clear();
142 }
143 catch
144 {
145 }
146 }
147
148 public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
149 Vector3 size, byte shapetype)
150 {
151 ODEPhysRepData repData = new ODEPhysRepData();
152 repData.actor = actor;
153 repData.pbs = pbs;
154 repData.size = size;
155 repData.shapetype = shapetype;
156
157 CheckMesh(repData);
158 CalcVolumeData(repData);
159 m_scene.AddChange(actor, changes.PhysRepData, repData);
160 return;
161 }
162
163 public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs,
164 Vector3 size, byte shapetype)
165 {
166 ODEPhysRepData repData = new ODEPhysRepData();
167 repData.actor = actor;
168 repData.pbs = pbs;
169 repData.size = size;
170 repData.shapetype = shapetype;
171
172 CheckMesh(repData);
173 CalcVolumeData(repData);
174 m_scene.AddChange(actor, changes.AddPhysRep, repData);
175 return repData;
176 }
177
178 public void RequestMesh(ODEPhysRepData repData)
179 {
180 repData.mesh = null;
181
182 if (repData.meshState == MeshState.needAsset)
183 {
184 PrimitiveBaseShape pbs = repData.pbs;
185
186 // check if we got outdated
187
188 if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero)
189 {
190 repData.meshState = MeshState.noNeed;
191 return;
192 }
193
194 repData.assetID = pbs.SculptTexture;
195 repData.meshState = MeshState.loadingAsset;
196
197 repData.comand = meshWorkerCmnds.getmesh;
198 createqueue.Enqueue(repData);
199 }
200 }
201
202 // creates and prepares a mesh to use and calls parameters estimation
203 public bool CreateActorPhysRep(ODEPhysRepData repData)
204 {
205 IMesh mesh = repData.mesh;
206
207 if (mesh != null)
208 {
209 IntPtr vertices, indices;
210 int vertexCount, indexCount;
211 int vertexStride, triStride;
212
213 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
214 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
215
216 if (vertexCount == 0 || indexCount == 0)
217 {
218 m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}",
219 repData.actor.Name, repData.pbs.SculptTexture.ToString());
220 repData.meshState = MeshState.MeshFailed;
221 repData.hasOBB = false;
222 repData.mesh = null;
223 m_scene.mesher.ReleaseMesh(mesh);
224 }
225 else
226 {
227 repData.OBBOffset = mesh.GetCentroid();
228 repData.OBB = mesh.GetOBB();
229 repData.hasOBB = true;
230 mesh.releaseSourceMeshData();
231 }
232 }
233 CalcVolumeData(repData);
234 return true;
235 }
236
237 public void AssetLoaded(ODEPhysRepData repData)
238 {
239 if (m_scene.haveActor(repData.actor))
240 {
241 if (needsMeshing(repData.pbs)) // no need for pbs now?
242 {
243 repData.comand = meshWorkerCmnds.changefull;
244 createqueue.Enqueue(repData);
245 }
246 }
247 else
248 repData.pbs.SculptData = Utils.EmptyBytes;
249 }
250
251 public void DoRepDataGetMesh(ODEPhysRepData repData)
252 {
253 if (!repData.pbs.SculptEntry)
254 return;
255
256 if (repData.meshState != MeshState.loadingAsset)
257 return;
258
259 if (repData.assetID == null || repData.assetID == UUID.Zero)
260 return;
261
262 if (repData.assetID != repData.pbs.SculptTexture)
263 return;
264
265 // check if it is in cache
266 GetMesh(repData);
267 if (repData.meshState != MeshState.needAsset)
268 {
269 CreateActorPhysRep(repData);
270 m_scene.AddChange(repData.actor, changes.PhysRepData, repData);
271 return;
272 }
273
274 RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod;
275 if (assetProvider == null)
276 return;
277 ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log);
278 }
279
280
281 /// <summary>
282 /// Routine to figure out if we need to mesh this prim with our mesher
283 /// </summary>
284 /// <param name="pbs"></param>
285 /// <returns></returns>
286 public bool needsMeshing(PrimitiveBaseShape pbs)
287 {
288 // check sculpts or meshs
289 if (pbs.SculptEntry)
290 {
291 if (meshSculptedPrim)
292 return true;
293
294 if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
295 return true;
296
297 return false;
298 }
299
300 if (forceSimplePrimMeshing)
301 return true;
302
303 // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
304
305 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
306 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
307 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
308 {
309
310 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
311 && pbs.ProfileHollow == 0
312 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
313 && pbs.PathBegin == 0 && pbs.PathEnd == 0
314 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
315 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
316 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
317 {
318 return false;
319 }
320 }
321
322 // following code doesn't give meshs to boxes and spheres ever
323 // and it's odd.. so for now just return true if asked to force meshs
324 // hopefully mesher will fail if doesn't suport so things still get basic boxes
325
326 int iPropertiesNotSupportedDefault = 0;
327
328 if (pbs.ProfileHollow != 0)
329 iPropertiesNotSupportedDefault++;
330
331 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
332 iPropertiesNotSupportedDefault++;
333
334 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
335 iPropertiesNotSupportedDefault++;
336
337 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
338 iPropertiesNotSupportedDefault++;
339
340 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
341 iPropertiesNotSupportedDefault++;
342
343 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
344 iPropertiesNotSupportedDefault++;
345
346 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
347 iPropertiesNotSupportedDefault++;
348
349 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
350 iPropertiesNotSupportedDefault++;
351
352 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
353 iPropertiesNotSupportedDefault++;
354
355 // test for torus
356 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
357 {
358 if (pbs.PathCurve == (byte)Extrusion.Curve1)
359 {
360 iPropertiesNotSupportedDefault++;
361 }
362 }
363 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
364 {
365 if (pbs.PathCurve == (byte)Extrusion.Straight)
366 {
367 iPropertiesNotSupportedDefault++;
368 }
369
370 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
371 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
372 {
373 iPropertiesNotSupportedDefault++;
374 }
375 }
376 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
377 {
378 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
379 {
380 iPropertiesNotSupportedDefault++;
381 }
382 }
383 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
384 {
385 if (pbs.PathCurve == (byte)Extrusion.Straight)
386 {
387 iPropertiesNotSupportedDefault++;
388 }
389 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
390 {
391 iPropertiesNotSupportedDefault++;
392 }
393 }
394
395 if (iPropertiesNotSupportedDefault == 0)
396 {
397 return false;
398 }
399 return true;
400 }
401
402 // see if we need a mesh and if so if we have a cached one
403 // called with a new repData
404 public void CheckMesh(ODEPhysRepData repData)
405 {
406 PhysicsActor actor = repData.actor;
407 PrimitiveBaseShape pbs = repData.pbs;
408
409 if (!needsMeshing(pbs))
410 {
411 repData.meshState = MeshState.noNeed;
412 return;
413 }
414
415 IMesh mesh = null;
416
417 Vector3 size = repData.size;
418 byte shapetype = repData.shapetype;
419
420 bool convex;
421
422 int clod = (int)LevelOfDetail.High;
423 if (shapetype == 0)
424 convex = false;
425 else
426 {
427 convex = true;
428 if (pbs.SculptType != (byte)SculptType.Mesh)
429 clod = (int)LevelOfDetail.Low;
430 }
431
432 mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex);
433
434 if (mesh == null)
435 {
436 if (pbs.SculptEntry)
437 {
438 if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero)
439 {
440 repData.assetID = pbs.SculptTexture;
441 repData.meshState = MeshState.needAsset;
442 }
443 else
444 repData.meshState = MeshState.MeshFailed;
445
446 return;
447 }
448 else
449 {
450 repData.meshState = MeshState.needMesh;
451 mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
452 if (mesh == null)
453 {
454 repData.meshState = MeshState.MeshFailed;
455 return;
456 }
457 }
458 }
459
460 repData.meshState = MeshState.AssetOK;
461 repData.mesh = mesh;
462
463 if (pbs.SculptEntry)
464 {
465 repData.assetID = pbs.SculptTexture;
466 }
467
468 pbs.SculptData = Utils.EmptyBytes;
469 return ;
470 }
471
472 public void GetMesh(ODEPhysRepData repData)
473 {
474 PhysicsActor actor = repData.actor;
475
476 PrimitiveBaseShape pbs = repData.pbs;
477
478 repData.mesh = null;
479 repData.hasOBB = false;
480
481 if (!needsMeshing(pbs))
482 {
483 repData.meshState = MeshState.noNeed;
484 return;
485 }
486
487 if (repData.meshState == MeshState.MeshFailed)
488 return;
489
490 if (pbs.SculptEntry)
491 {
492 if (repData.meshState == MeshState.AssetFailed)
493 {
494 if (pbs.SculptTexture == repData.assetID)
495 return;
496 }
497 }
498
499 repData.meshState = MeshState.noNeed;
500
501 IMesh mesh = null;
502 Vector3 size = repData.size;
503 byte shapetype = repData.shapetype;
504
505 bool convex;
506 int clod = (int)LevelOfDetail.High;
507 if (shapetype == 0)
508 convex = false;
509 else
510 {
511 convex = true;
512 if (pbs.SculptType != (byte)SculptType.Mesh)
513 clod = (int)LevelOfDetail.Low;
514 }
515
516 mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, false, convex, true);
517
518 if (mesh == null)
519 {
520 if (pbs.SculptEntry)
521 {
522 if (pbs.SculptTexture == UUID.Zero)
523 return;
524
525 repData.assetID = pbs.SculptTexture;
526
527 if (pbs.SculptData == null || pbs.SculptData.Length == 0)
528 {
529 repData.meshState = MeshState.needAsset;
530 return;
531 }
532 }
533 }
534
535 repData.mesh = mesh;
536 repData.pbs.SculptData = Utils.EmptyBytes;
537
538 if (mesh == null)
539 {
540 if (pbs.SculptEntry)
541 repData.meshState = MeshState.AssetFailed;
542 else
543 repData.meshState = MeshState.MeshFailed;
544
545 return;
546 }
547
548 repData.meshState = MeshState.AssetOK;
549
550 return;
551 }
552
553 private void CalculateBasicPrimVolume(ODEPhysRepData repData)
554 {
555 PrimitiveBaseShape _pbs = repData.pbs;
556 Vector3 _size = repData.size;
557
558 float volume = _size.X * _size.Y * _size.Z; // default
559 float tmp;
560
561 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
562 float hollowVolume = hollowAmount * hollowAmount;
563
564 switch (_pbs.ProfileShape)
565 {
566 case ProfileShape.Square:
567 // default box
568
569 if (_pbs.PathCurve == (byte)Extrusion.Straight)
570 {
571 if (hollowAmount > 0.0)
572 {
573 switch (_pbs.HollowShape)
574 {
575 case HollowShape.Square:
576 case HollowShape.Same:
577 break;
578
579 case HollowShape.Circle:
580
581 hollowVolume *= 0.78539816339f;
582 break;
583
584 case HollowShape.Triangle:
585
586 hollowVolume *= (0.5f * .5f);
587 break;
588
589 default:
590 hollowVolume = 0;
591 break;
592 }
593 volume *= (1.0f - hollowVolume);
594 }
595 }
596
597 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
598 {
599 //a tube
600
601 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
602 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
603 volume -= volume * tmp * tmp;
604
605 if (hollowAmount > 0.0)
606 {
607 hollowVolume *= hollowAmount;
608
609 switch (_pbs.HollowShape)
610 {
611 case HollowShape.Square:
612 case HollowShape.Same:
613 break;
614
615 case HollowShape.Circle:
616 hollowVolume *= 0.78539816339f;
617 break;
618
619 case HollowShape.Triangle:
620 hollowVolume *= 0.5f * 0.5f;
621 break;
622 default:
623 hollowVolume = 0;
624 break;
625 }
626 volume *= (1.0f - hollowVolume);
627 }
628 }
629
630 break;
631
632 case ProfileShape.Circle:
633
634 if (_pbs.PathCurve == (byte)Extrusion.Straight)
635 {
636 volume *= 0.78539816339f; // elipse base
637
638 if (hollowAmount > 0.0)
639 {
640 switch (_pbs.HollowShape)
641 {
642 case HollowShape.Same:
643 case HollowShape.Circle:
644 break;
645
646 case HollowShape.Square:
647 hollowVolume *= 0.5f * 2.5984480504799f;
648 break;
649
650 case HollowShape.Triangle:
651 hollowVolume *= .5f * 1.27323954473516f;
652 break;
653
654 default:
655 hollowVolume = 0;
656 break;
657 }
658 volume *= (1.0f - hollowVolume);
659 }
660 }
661
662 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
663 {
664 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
665 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
666 volume *= (1.0f - tmp * tmp);
667
668 if (hollowAmount > 0.0)
669 {
670
671 // calculate the hollow volume by it's shape compared to the prim shape
672 hollowVolume *= hollowAmount;
673
674 switch (_pbs.HollowShape)
675 {
676 case HollowShape.Same:
677 case HollowShape.Circle:
678 break;
679
680 case HollowShape.Square:
681 hollowVolume *= 0.5f * 2.5984480504799f;
682 break;
683
684 case HollowShape.Triangle:
685 hollowVolume *= .5f * 1.27323954473516f;
686 break;
687
688 default:
689 hollowVolume = 0;
690 break;
691 }
692 volume *= (1.0f - hollowVolume);
693 }
694 }
695 break;
696
697 case ProfileShape.HalfCircle:
698 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
699 {
700 volume *= 0.5236f;
701
702 if (hollowAmount > 0.0)
703 {
704 hollowVolume *= hollowAmount;
705
706 switch (_pbs.HollowShape)
707 {
708 case HollowShape.Circle:
709 case HollowShape.Triangle: // diference in sl is minor and odd
710 case HollowShape.Same:
711 break;
712
713 case HollowShape.Square:
714 hollowVolume *= 0.909f;
715 break;
716
717 // case HollowShape.Triangle:
718 // hollowVolume *= .827f;
719 // break;
720 default:
721 hollowVolume = 0;
722 break;
723 }
724 volume *= (1.0f - hollowVolume);
725 }
726
727 }
728 break;
729
730 case ProfileShape.EquilateralTriangle:
731
732 if (_pbs.PathCurve == (byte)Extrusion.Straight)
733 {
734 volume *= 0.32475953f;
735
736 if (hollowAmount > 0.0)
737 {
738
739 // calculate the hollow volume by it's shape compared to the prim shape
740 switch (_pbs.HollowShape)
741 {
742 case HollowShape.Same:
743 case HollowShape.Triangle:
744 hollowVolume *= .25f;
745 break;
746
747 case HollowShape.Square:
748 hollowVolume *= 0.499849f * 3.07920140172638f;
749 break;
750
751 case HollowShape.Circle:
752 // Hollow shape is a perfect cyllinder in respect to the cube's scale
753 // Cyllinder hollow volume calculation
754
755 hollowVolume *= 0.1963495f * 3.07920140172638f;
756 break;
757
758 default:
759 hollowVolume = 0;
760 break;
761 }
762 volume *= (1.0f - hollowVolume);
763 }
764 }
765 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
766 {
767 volume *= 0.32475953f;
768 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
769 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
770 volume *= (1.0f - tmp * tmp);
771
772 if (hollowAmount > 0.0)
773 {
774
775 hollowVolume *= hollowAmount;
776
777 switch (_pbs.HollowShape)
778 {
779 case HollowShape.Same:
780 case HollowShape.Triangle:
781 hollowVolume *= .25f;
782 break;
783
784 case HollowShape.Square:
785 hollowVolume *= 0.499849f * 3.07920140172638f;
786 break;
787
788 case HollowShape.Circle:
789
790 hollowVolume *= 0.1963495f * 3.07920140172638f;
791 break;
792
793 default:
794 hollowVolume = 0;
795 break;
796 }
797 volume *= (1.0f - hollowVolume);
798 }
799 }
800 break;
801
802 default:
803 break;
804 }
805
806 float taperX1;
807 float taperY1;
808 float taperX;
809 float taperY;
810 float pathBegin;
811 float pathEnd;
812 float profileBegin;
813 float profileEnd;
814
815 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
816 {
817 taperX1 = _pbs.PathScaleX * 0.01f;
818 if (taperX1 > 1.0f)
819 taperX1 = 2.0f - taperX1;
820 taperX = 1.0f - taperX1;
821
822 taperY1 = _pbs.PathScaleY * 0.01f;
823 if (taperY1 > 1.0f)
824 taperY1 = 2.0f - taperY1;
825 taperY = 1.0f - taperY1;
826 }
827 else
828 {
829 taperX = _pbs.PathTaperX * 0.01f;
830 if (taperX < 0.0f)
831 taperX = -taperX;
832 taperX1 = 1.0f - taperX;
833
834 taperY = _pbs.PathTaperY * 0.01f;
835 if (taperY < 0.0f)
836 taperY = -taperY;
837 taperY1 = 1.0f - taperY;
838 }
839
840 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
841
842 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
843 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
844 volume *= (pathEnd - pathBegin);
845
846 // this is crude aproximation
847 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
848 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
849 volume *= (profileEnd - profileBegin);
850
851 repData.volume = volume;
852 }
853
854 private void CalcVolumeData(ODEPhysRepData repData)
855 {
856 if (repData.hasOBB)
857 {
858 Vector3 OBB = repData.OBB;
859 }
860 else
861 {
862 Vector3 OBB = repData.size;
863 OBB.X *= 0.5f;
864 OBB.Y *= 0.5f;
865 OBB.Z *= 0.5f;
866
867 repData.OBB = OBB;
868 repData.OBBOffset = Vector3.Zero;
869 }
870
871 CalculateBasicPrimVolume(repData);
872 }
873 }
874
875 public class ODEAssetRequest
876 {
877 ODEMeshWorker m_worker;
878 private ILog m_log;
879 ODEPhysRepData repData;
880
881 public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider,
882 ODEPhysRepData pRepData, ILog plog)
883 {
884 m_worker = pWorker;
885 m_log = plog;
886 repData = pRepData;
887
888 repData.meshState = MeshState.AssetFailed;
889 if (provider == null)
890 return;
891
892 if (repData.assetID == null)
893 return;
894
895 UUID assetID = (UUID) repData.assetID;
896 if (assetID == UUID.Zero)
897 return;
898
899 repData.meshState = MeshState.loadingAsset;
900 provider(assetID, ODEassetReceived);
901 }
902
903 void ODEassetReceived(AssetBase asset)
904 {
905 repData.meshState = MeshState.AssetFailed;
906 if (asset != null)
907 {
908 if (asset.Data != null && asset.Data.Length > 0)
909 {
910 repData.meshState = MeshState.noNeed;
911
912 if (!repData.pbs.SculptEntry)
913 return;
914 if (repData.pbs.SculptTexture != repData.assetID)
915 return;
916
917// repData.pbs.SculptData = new byte[asset.Data.Length];
918// asset.Data.CopyTo(repData.pbs.SculptData,0);
919 repData.pbs.SculptData = asset.Data;
920 repData.meshState = MeshState.AssetOK;
921 m_worker.AssetLoaded(repData);
922 }
923 else
924 m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.",
925 repData.actor.Name, asset.ID.ToString());
926 }
927 else
928 m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.",
929 repData.actor.Name);
930 }
931 }
932}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..faa9488
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,3844 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
33/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
36 * ODEPrim.cs contains methods dealing with Prim editing, Prim
37 * characteristics and Kinetic motion.
38 * ODEDynamics.cs contains methods dealing with Prim Physical motion
39 * (dynamics) and the associated settings. Old Linear and angular
40 * motors for dynamic motion have been replace with MoveLinear()
41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
43 * switch between 'VEHICLE' parameter use and general dynamics
44 * settings use.
45 */
46
47//#define SPAM
48
49using System;
50using System.Collections.Generic;
51using System.Reflection;
52using System.Runtime.InteropServices;
53using System.Threading;
54using log4net;
55using OpenMetaverse;
56using OdeAPI;
57using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager;
59
60namespace OpenSim.Region.Physics.OdePlugin
61{
62 public class OdePrim : PhysicsActor
63 {
64 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
65
66 private bool m_isphysical;
67 private bool m_fakeisphysical;
68 private bool m_isphantom;
69 private bool m_fakeisphantom;
70 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
71 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72
73 protected bool m_building;
74 protected bool m_forcePosOrRotation;
75 private bool m_iscolliding;
76
77 internal bool m_isSelected;
78 private bool m_delaySelect;
79 private bool m_lastdoneSelected;
80 internal bool m_outbounds;
81
82 private Quaternion m_lastorientation;
83 private Quaternion _orientation;
84
85 private Vector3 _position;
86 private Vector3 _velocity;
87 private Vector3 m_torque;
88 private Vector3 m_lastVelocity;
89 private Vector3 m_lastposition;
90 private Vector3 m_rotationalVelocity;
91 private Vector3 _size;
92 private Vector3 _acceleration;
93 private Vector3 m_angularlock = Vector3.One;
94 private IntPtr Amotor;
95
96 private Vector3 m_force;
97 private Vector3 m_forceacc;
98 private Vector3 m_angularForceacc;
99
100 private float m_invTimeStep;
101 private float m_timeStep;
102
103 private Vector3 m_PIDTarget;
104 private float m_PIDTau;
105 private bool m_usePID;
106
107 private float m_PIDHoverHeight;
108 private float m_PIDHoverTau;
109 private bool m_useHoverPID;
110 private PIDHoverType m_PIDHoverType;
111 private float m_targetHoverHeight;
112 private float m_groundHeight;
113 private float m_waterHeight;
114 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
115
116 private int body_autodisable_frames;
117 public int bodydisablecontrol;
118
119
120 // Default we're a Geometry
121 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
122 // Default colide nonphysical don't try to colide with anything
123 private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
124
125 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
126 CollisionCategories.Character |
127 CollisionCategories.Land |
128 CollisionCategories.VolumeDtc);
129
130// private bool m_collidesLand = true;
131 private bool m_collidesWater;
132// public bool m_returnCollisions;
133
134 private bool m_NoColide; // for now only for internal use for bad meshs
135
136
137 // Default, Collide with Other Geometries, spaces and Bodies
138 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
139
140 public bool m_disabled;
141
142 private uint m_localID;
143
144 private IMesh m_mesh;
145 private object m_meshlock = new object();
146 private PrimitiveBaseShape _pbs;
147
148 private UUID? m_assetID;
149 private MeshState m_meshState;
150
151 public OdeScene _parent_scene;
152
153 /// <summary>
154 /// The physics space which contains prim geometry
155 /// </summary>
156 public IntPtr m_targetSpace;
157
158 public IntPtr prim_geom;
159 public IntPtr _triMeshData;
160
161 private PhysicsActor _parent;
162
163 private List<OdePrim> childrenPrim = new List<OdePrim>();
164
165 public float m_collisionscore;
166 private int m_colliderfilter = 0;
167
168 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
169
170 private float m_density;
171 private byte m_shapetype;
172 public bool _zeroFlag;
173 private bool m_lastUpdateSent;
174
175 public IntPtr Body;
176
177 private Vector3 _target_velocity;
178
179 public Vector3 m_OBBOffset;
180 public Vector3 m_OBB;
181 public float primOOBradiusSQ;
182
183 private bool m_hasOBB = true;
184
185 private float m_physCost;
186 private float m_streamCost;
187
188 public d.Mass primdMass; // prim inertia information on it's own referencial
189 float primMass; // prim own mass
190 float primVolume; // prim own volume;
191 float _mass; // object mass acording to case
192
193 public int givefakepos;
194 private Vector3 fakepos;
195 public int givefakeori;
196 private Quaternion fakeori;
197
198 private int m_eventsubscription;
199 private int m_cureventsubscription;
200 private CollisionEventUpdate CollisionEventsThisFrame = null;
201 private bool SentEmptyCollisionsEvent;
202
203 public volatile bool childPrim;
204
205 public ODEDynamics m_vehicle;
206
207 internal int m_material = (int)Material.Wood;
208 private float mu;
209 private float bounce;
210
211 /// <summary>
212 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
213 /// </summary>
214 public override bool IsPhysical // this is not reliable for internal use
215 {
216 get { return m_fakeisphysical; }
217 set
218 {
219 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
220 // and also to stop imediatly some updates
221 // but real change will only happen in taintprocessing
222
223 if (!value) // Zero the remembered last velocity
224 m_lastVelocity = Vector3.Zero;
225 AddChange(changes.Physical, value);
226 }
227 }
228
229 public override bool IsVolumeDtc
230 {
231 get { return m_fakeisVolumeDetect; }
232 set
233 {
234 m_fakeisVolumeDetect = value;
235 AddChange(changes.VolumeDtc, value);
236 }
237 }
238
239 public override bool Phantom // this is not reliable for internal use
240 {
241 get { return m_fakeisphantom; }
242 set
243 {
244 m_fakeisphantom = value;
245 AddChange(changes.Phantom, value);
246 }
247 }
248
249 public override bool Building // this is not reliable for internal use
250 {
251 get { return m_building; }
252 set
253 {
254 if (value)
255 m_building = true;
256 AddChange(changes.building, value);
257 }
258 }
259
260 public override void getContactData(ref ContactData cdata)
261 {
262 cdata.mu = mu;
263 cdata.bounce = bounce;
264
265 // cdata.softcolide = m_softcolide;
266 cdata.softcolide = false;
267
268 if (m_isphysical)
269 {
270 ODEDynamics veh;
271 if (_parent != null)
272 veh = ((OdePrim)_parent).m_vehicle;
273 else
274 veh = m_vehicle;
275
276 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
277 cdata.mu *= veh.FrictionFactor;
278// cdata.mu *= 0;
279 }
280 }
281
282 public override float PhysicsCost
283 {
284 get
285 {
286 return m_physCost;
287 }
288 }
289
290 public override float StreamCost
291 {
292 get
293 {
294 return m_streamCost;
295 }
296 }
297
298 public override int PhysicsActorType
299 {
300 get { return (int)ActorTypes.Prim; }
301 set { return; }
302 }
303
304 public override bool SetAlwaysRun
305 {
306 get { return false; }
307 set { return; }
308 }
309
310 public override uint LocalID
311 {
312 get { return m_localID; }
313 set { m_localID = value; }
314 }
315
316 public override PhysicsActor ParentActor
317 {
318 get
319 {
320 if (childPrim)
321 return _parent;
322 else
323 return (PhysicsActor)this;
324 }
325 }
326
327 public override bool Grabbed
328 {
329 set { return; }
330 }
331
332 public override bool Selected
333 {
334 set
335 {
336 if (value)
337 m_isSelected = value; // if true set imediatly to stop moves etc
338 AddChange(changes.Selected, value);
339 }
340 }
341
342 public override bool Flying
343 {
344 // no flying prims for you
345 get { return false; }
346 set { }
347 }
348
349 public override bool IsColliding
350 {
351 get { return m_iscolliding; }
352 set
353 {
354 if (value)
355 {
356 m_colliderfilter += 2;
357 if (m_colliderfilter > 2)
358 m_colliderfilter = 2;
359 }
360 else
361 {
362 m_colliderfilter--;
363 if (m_colliderfilter < 0)
364 m_colliderfilter = 0;
365 }
366
367 if (m_colliderfilter == 0)
368 m_iscolliding = false;
369 else
370 m_iscolliding = true;
371 }
372 }
373
374 public override bool CollidingGround
375 {
376 get { return false; }
377 set { return; }
378 }
379
380 public override bool CollidingObj
381 {
382 get { return false; }
383 set { return; }
384 }
385
386
387 public override bool ThrottleUpdates {get;set;}
388
389 public override bool Stopped
390 {
391 get { return _zeroFlag; }
392 }
393
394 public override Vector3 Position
395 {
396 get
397 {
398 if (givefakepos > 0)
399 return fakepos;
400 else
401 return _position;
402 }
403
404 set
405 {
406 fakepos = value;
407 givefakepos++;
408 AddChange(changes.Position, value);
409 }
410 }
411
412 public override Vector3 Size
413 {
414 get { return _size; }
415 set
416 {
417 if (value.IsFinite())
418 {
419 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype);
420 }
421 else
422 {
423 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
424 }
425 }
426 }
427
428 public override float Mass
429 {
430 get { return primMass; }
431 }
432
433 public override Vector3 Force
434 {
435 get { return m_force; }
436 set
437 {
438 if (value.IsFinite())
439 {
440 AddChange(changes.Force, value);
441 }
442 else
443 {
444 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
445 }
446 }
447 }
448
449 public override void SetVolumeDetect(int param)
450 {
451 m_fakeisVolumeDetect = (param != 0);
452 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
453 }
454
455 public override Vector3 GeometricCenter
456 {
457 // this is not real geometric center but a average of positions relative to root prim acording to
458 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
459 // ignoring tortured prims details since sl also seems to ignore
460 // so no real use in doing it on physics
461 get
462 {
463 return Vector3.Zero;
464 }
465 }
466
467 public override Vector3 CenterOfMass
468 {
469 get
470 {
471 lock (_parent_scene.OdeLock)
472 {
473 d.Vector3 dtmp;
474 if (!childPrim && Body != IntPtr.Zero)
475 {
476 dtmp = d.BodyGetPosition(Body);
477 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
478 }
479 else if (prim_geom != IntPtr.Zero)
480 {
481 d.Quaternion dq;
482 d.GeomCopyQuaternion(prim_geom, out dq);
483 Quaternion q;
484 q.X = dq.X;
485 q.Y = dq.Y;
486 q.Z = dq.Z;
487 q.W = dq.W;
488
489 Vector3 Ptot = m_OBBOffset * q;
490 dtmp = d.GeomGetPosition(prim_geom);
491 Ptot.X += dtmp.X;
492 Ptot.Y += dtmp.Y;
493 Ptot.Z += dtmp.Z;
494
495 // if(childPrim) we only know about physical linksets
496 return Ptot;
497/*
498 float tmass = _mass;
499 Ptot *= tmass;
500
501 float m;
502
503 foreach (OdePrim prm in childrenPrim)
504 {
505 m = prm._mass;
506 Ptot += prm.CenterOfMass * m;
507 tmass += m;
508 }
509
510 if (tmass == 0)
511 tmass = 0;
512 else
513 tmass = 1.0f / tmass;
514
515 Ptot *= tmass;
516 return Ptot;
517*/
518 }
519 else
520 return _position;
521 }
522 }
523 }
524
525 public override Vector3 OOBsize
526 {
527 get
528 {
529 return m_OBB;
530 }
531 }
532
533 public override Vector3 OOBoffset
534 {
535 get
536 {
537 return m_OBBOffset;
538 }
539 }
540
541 public override float OOBRadiusSQ
542 {
543 get
544 {
545 return primOOBradiusSQ;
546 }
547 }
548
549 public override PrimitiveBaseShape Shape
550 {
551 set
552 {
553// AddChange(changes.Shape, value);
554 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype);
555 }
556 }
557
558 public override byte PhysicsShapeType
559 {
560 get
561 {
562 return m_shapetype;
563 }
564 set
565 {
566 m_shapetype = value;
567 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
568 }
569 }
570
571 public override Vector3 Velocity
572 {
573 get
574 {
575 if (_zeroFlag)
576 return Vector3.Zero;
577 return _velocity;
578 }
579 set
580 {
581 if (value.IsFinite())
582 {
583 AddChange(changes.Velocity, value);
584 }
585 else
586 {
587 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
588 }
589
590 }
591 }
592
593 public override Vector3 Torque
594 {
595 get
596 {
597 if (!IsPhysical || Body == IntPtr.Zero)
598 return Vector3.Zero;
599
600 return m_torque;
601 }
602
603 set
604 {
605 if (value.IsFinite())
606 {
607 AddChange(changes.Torque, value);
608 }
609 else
610 {
611 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
612 }
613 }
614 }
615
616 public override float CollisionScore
617 {
618 get { return m_collisionscore; }
619 set { m_collisionscore = value; }
620 }
621
622 public override bool Kinematic
623 {
624 get { return false; }
625 set { }
626 }
627
628 public override Quaternion Orientation
629 {
630 get
631 {
632 if (givefakeori > 0)
633 return fakeori;
634 else
635
636 return _orientation;
637 }
638 set
639 {
640 if (QuaternionIsFinite(value))
641 {
642 fakeori = value;
643 givefakeori++;
644
645 value.Normalize();
646
647 AddChange(changes.Orientation, value);
648 }
649 else
650 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
651
652 }
653 }
654
655 public override Vector3 Acceleration
656 {
657 get { return _acceleration; }
658 set { }
659 }
660
661 public override Vector3 RotationalVelocity
662 {
663 get
664 {
665 Vector3 pv = Vector3.Zero;
666 if (_zeroFlag)
667 return pv;
668
669 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
670 return pv;
671
672 return m_rotationalVelocity;
673 }
674 set
675 {
676 if (value.IsFinite())
677 {
678 AddChange(changes.AngVelocity, value);
679 }
680 else
681 {
682 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
683 }
684 }
685 }
686
687 public override float Buoyancy
688 {
689 get { return m_buoyancy; }
690 set
691 {
692 AddChange(changes.Buoyancy,value);
693 }
694 }
695
696 public override bool FloatOnWater
697 {
698 set
699 {
700 AddChange(changes.CollidesWater, value);
701 }
702 }
703
704 public override Vector3 PIDTarget
705 {
706 set
707 {
708 if (value.IsFinite())
709 {
710 AddChange(changes.PIDTarget,value);
711 }
712 else
713 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
714 }
715 }
716
717 public override bool PIDActive
718 {
719 set
720 {
721 AddChange(changes.PIDActive,value);
722 }
723 }
724
725 public override float PIDTau
726 {
727 set
728 {
729 float tmp = 0;
730 if (value > 0)
731 {
732 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
733 if (value < mint)
734 tmp = mint;
735 else
736 tmp = value;
737 }
738 AddChange(changes.PIDTau,tmp);
739 }
740 }
741
742 public override float PIDHoverHeight
743 {
744 set
745 {
746 AddChange(changes.PIDHoverHeight,value);
747 }
748 }
749 public override bool PIDHoverActive
750 {
751 set
752 {
753 AddChange(changes.PIDHoverActive, value);
754 }
755 }
756
757 public override PIDHoverType PIDHoverType
758 {
759 set
760 {
761 AddChange(changes.PIDHoverType,value);
762 }
763 }
764
765 public override float PIDHoverTau
766 {
767 set
768 {
769 float tmp =0;
770 if (value > 0)
771 {
772 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
773 if (value < mint)
774 tmp = mint;
775 else
776 tmp = value;
777 }
778 AddChange(changes.PIDHoverTau, tmp);
779 }
780 }
781
782 public override Quaternion APIDTarget { set { return; } }
783
784 public override bool APIDActive { set { return; } }
785
786 public override float APIDStrength { set { return; } }
787
788 public override float APIDDamping { set { return; } }
789
790 public override int VehicleType
791 {
792 // we may need to put a fake on this
793 get
794 {
795 if (m_vehicle == null)
796 return (int)Vehicle.TYPE_NONE;
797 else
798 return (int)m_vehicle.Type;
799 }
800 set
801 {
802 AddChange(changes.VehicleType, value);
803 }
804 }
805
806 public override void VehicleFloatParam(int param, float value)
807 {
808 strVehicleFloatParam fp = new strVehicleFloatParam();
809 fp.param = param;
810 fp.value = value;
811 AddChange(changes.VehicleFloatParam, fp);
812 }
813
814 public override void VehicleVectorParam(int param, Vector3 value)
815 {
816 strVehicleVectorParam fp = new strVehicleVectorParam();
817 fp.param = param;
818 fp.value = value;
819 AddChange(changes.VehicleVectorParam, fp);
820 }
821
822 public override void VehicleRotationParam(int param, Quaternion value)
823 {
824 strVehicleQuatParam fp = new strVehicleQuatParam();
825 fp.param = param;
826 fp.value = value;
827 AddChange(changes.VehicleRotationParam, fp);
828 }
829
830 public override void VehicleFlags(int param, bool value)
831 {
832 strVehicleBoolParam bp = new strVehicleBoolParam();
833 bp.param = param;
834 bp.value = value;
835 AddChange(changes.VehicleFlags, bp);
836 }
837
838 public override void SetVehicle(object vdata)
839 {
840 AddChange(changes.SetVehicle, vdata);
841 }
842 public void SetAcceleration(Vector3 accel)
843 {
844 _acceleration = accel;
845 }
846
847 public override void AddForce(Vector3 force, bool pushforce)
848 {
849 if (force.IsFinite())
850 {
851 if(pushforce)
852 AddChange(changes.AddForce, force);
853 else // a impulse
854 AddChange(changes.AddForce, force * m_invTimeStep);
855 }
856 else
857 {
858 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
859 }
860 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
861 }
862
863 public override void AddAngularForce(Vector3 force, bool pushforce)
864 {
865 if (force.IsFinite())
866 {
867// if(pushforce) for now applyrotationimpulse seems more happy applied as a force
868 AddChange(changes.AddAngForce, force);
869// else // a impulse
870// AddChange(changes.AddAngForce, force * m_invTimeStep);
871 }
872 else
873 {
874 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
875 }
876 }
877
878 public override void CrossingFailure()
879 {
880 if (m_outbounds)
881 {
882 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
883 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
884 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
885
886 m_lastposition = _position;
887 _velocity.X = 0;
888 _velocity.Y = 0;
889 _velocity.Z = 0;
890
891 m_lastVelocity = _velocity;
892 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
893 m_vehicle.Stop();
894
895 if(Body != IntPtr.Zero)
896 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
897 if (prim_geom != IntPtr.Zero)
898 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
899
900 m_outbounds = false;
901 changeDisable(false);
902 base.RequestPhysicsterseUpdate();
903 }
904 }
905
906 public override void SetMomentum(Vector3 momentum)
907 {
908 }
909
910 public override void SetMaterial(int pMaterial)
911 {
912 m_material = pMaterial;
913 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
914 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
915 }
916
917 public void setPrimForRemoval()
918 {
919 AddChange(changes.Remove, null);
920 }
921
922 public override void link(PhysicsActor obj)
923 {
924 AddChange(changes.Link, obj);
925 }
926
927 public override void delink()
928 {
929 AddChange(changes.DeLink, null);
930 }
931
932 public override void LockAngularMotion(Vector3 axis)
933 {
934 // reverse the zero/non zero values for ODE.
935 if (axis.IsFinite())
936 {
937 axis.X = (axis.X > 0) ? 1f : 0f;
938 axis.Y = (axis.Y > 0) ? 1f : 0f;
939 axis.Z = (axis.Z > 0) ? 1f : 0f;
940// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
941 AddChange(changes.AngLock, axis);
942 }
943 else
944 {
945 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
946 }
947 }
948
949 public override void SubscribeEvents(int ms)
950 {
951 m_eventsubscription = ms;
952 m_cureventsubscription = 0;
953 if (CollisionEventsThisFrame == null)
954 CollisionEventsThisFrame = new CollisionEventUpdate();
955 SentEmptyCollisionsEvent = false;
956 }
957
958 public override void UnSubscribeEvents()
959 {
960 if (CollisionEventsThisFrame != null)
961 {
962 CollisionEventsThisFrame.Clear();
963 CollisionEventsThisFrame = null;
964 }
965 m_eventsubscription = 0;
966 _parent_scene.RemoveCollisionEventReporting(this);
967 }
968
969 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
970 {
971 if (CollisionEventsThisFrame == null)
972 CollisionEventsThisFrame = new CollisionEventUpdate();
973// if(CollisionEventsThisFrame.Count < 32)
974 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
975 }
976
977 public void SendCollisions()
978 {
979 if (CollisionEventsThisFrame == null)
980 return;
981
982 if (m_cureventsubscription < m_eventsubscription)
983 return;
984
985 m_cureventsubscription = 0;
986
987 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
988
989 if (!SentEmptyCollisionsEvent || ncolisions > 0)
990 {
991 base.SendCollisionUpdate(CollisionEventsThisFrame);
992
993 if (ncolisions == 0)
994 {
995 SentEmptyCollisionsEvent = true;
996 _parent_scene.RemoveCollisionEventReporting(this);
997 }
998 else
999 {
1000 SentEmptyCollisionsEvent = false;
1001 CollisionEventsThisFrame.Clear();
1002 }
1003 }
1004 }
1005
1006 internal void AddCollisionFrameTime(int t)
1007 {
1008 if (m_cureventsubscription < 50000)
1009 m_cureventsubscription += t;
1010 }
1011
1012 public override bool SubscribedEvents()
1013 {
1014 if (m_eventsubscription > 0)
1015 return true;
1016 return false;
1017 }
1018
1019 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
1020 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1021 {
1022 Name = primName;
1023 LocalID = plocalID;
1024
1025 m_vehicle = null;
1026
1027 if (!pos.IsFinite())
1028 {
1029 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1030 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1031 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1032 }
1033 _position = pos;
1034 givefakepos = 0;
1035
1036 m_timeStep = parent_scene.ODE_STEPSIZE;
1037 m_invTimeStep = 1f / m_timeStep;
1038
1039 m_density = parent_scene.geomDefaultDensity;
1040 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
1041
1042 prim_geom = IntPtr.Zero;
1043 collide_geom = IntPtr.Zero;
1044 Body = IntPtr.Zero;
1045
1046 if (!size.IsFinite())
1047 {
1048 size = new Vector3(0.5f, 0.5f, 0.5f);
1049 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
1050 }
1051
1052 if (size.X <= 0) size.X = 0.01f;
1053 if (size.Y <= 0) size.Y = 0.01f;
1054 if (size.Z <= 0) size.Z = 0.01f;
1055
1056 _size = size;
1057
1058 if (!QuaternionIsFinite(rotation))
1059 {
1060 rotation = Quaternion.Identity;
1061 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
1062 }
1063
1064 _orientation = rotation;
1065 givefakeori = 0;
1066
1067 _pbs = pbs;
1068
1069 _parent_scene = parent_scene;
1070 m_targetSpace = IntPtr.Zero;
1071
1072 if (pos.Z < 0)
1073 {
1074 m_isphysical = false;
1075 }
1076 else
1077 {
1078 m_isphysical = pisPhysical;
1079 }
1080 m_fakeisphysical = m_isphysical;
1081
1082 m_isVolumeDetect = false;
1083 m_fakeisVolumeDetect = false;
1084
1085 m_force = Vector3.Zero;
1086
1087 m_iscolliding = false;
1088 m_colliderfilter = 0;
1089 m_NoColide = false;
1090
1091 _triMeshData = IntPtr.Zero;
1092
1093 m_shapetype = _shapeType;
1094
1095 m_lastdoneSelected = false;
1096 m_isSelected = false;
1097 m_delaySelect = false;
1098
1099 m_isphantom = pisPhantom;
1100 m_fakeisphantom = pisPhantom;
1101
1102 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
1103 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
1104
1105 m_building = true; // control must set this to false when done
1106
1107 // get basic mass parameters
1108 ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype);
1109
1110 primVolume = repData.volume;
1111 m_OBB = repData.OBB;
1112 m_OBBOffset = repData.OBBOffset;
1113
1114 UpdatePrimBodyData();
1115 }
1116
1117 private void resetCollisionAccounting()
1118 {
1119 m_collisionscore = 0;
1120 }
1121
1122 private void UpdateCollisionCatFlags()
1123 {
1124 if(m_isphysical && m_disabled)
1125 {
1126 m_collisionCategories = 0;
1127 m_collisionFlags = 0;
1128 }
1129
1130 else if (m_isSelected)
1131 {
1132 m_collisionCategories = CollisionCategories.Selected;
1133 m_collisionFlags = 0;
1134 }
1135
1136 else if (m_isVolumeDetect)
1137 {
1138 m_collisionCategories = CollisionCategories.VolumeDtc;
1139 if (m_isphysical)
1140 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1141 else
1142 m_collisionFlags = 0;
1143 }
1144 else if (m_isphantom)
1145 {
1146 m_collisionCategories = CollisionCategories.Phantom;
1147 if (m_isphysical)
1148 m_collisionFlags = CollisionCategories.Land;
1149 else
1150 m_collisionFlags = 0;
1151 }
1152 else
1153 {
1154 m_collisionCategories = CollisionCategories.Geom;
1155 if (m_isphysical)
1156 m_collisionFlags = m_default_collisionFlagsPhysical;
1157 else
1158 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1159 }
1160 }
1161
1162 private void ApplyCollisionCatFlags()
1163 {
1164 if (prim_geom != IntPtr.Zero)
1165 {
1166 if (!childPrim && childrenPrim.Count > 0)
1167 {
1168 foreach (OdePrim prm in childrenPrim)
1169 {
1170 if (m_isphysical && m_disabled)
1171 {
1172 prm.m_collisionCategories = 0;
1173 prm.m_collisionFlags = 0;
1174 }
1175 else
1176 {
1177 // preserve some
1178 if (prm.m_isSelected)
1179 {
1180 prm.m_collisionCategories = CollisionCategories.Selected;
1181 prm.m_collisionFlags = 0;
1182 }
1183 else if (prm.m_isVolumeDetect)
1184 {
1185 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1186 if (m_isphysical)
1187 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1188 else
1189 prm.m_collisionFlags = 0;
1190 }
1191 else if (prm.m_isphantom)
1192 {
1193 prm.m_collisionCategories = CollisionCategories.Phantom;
1194 if (m_isphysical)
1195 prm.m_collisionFlags = CollisionCategories.Land;
1196 else
1197 prm.m_collisionFlags = 0;
1198 }
1199 else
1200 {
1201 prm.m_collisionCategories = m_collisionCategories;
1202 prm.m_collisionFlags = m_collisionFlags;
1203 }
1204 }
1205
1206 if (prm.prim_geom != IntPtr.Zero)
1207 {
1208 if (prm.m_NoColide)
1209 {
1210 d.GeomSetCategoryBits(prm.prim_geom, 0);
1211 if (m_isphysical)
1212 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1213 else
1214 d.GeomSetCollideBits(prm.prim_geom, 0);
1215 }
1216 else
1217 {
1218 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1219 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1220 }
1221 }
1222 }
1223 }
1224
1225 if (m_NoColide)
1226 {
1227 d.GeomSetCategoryBits(prim_geom, 0);
1228 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1229 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1230 {
1231 d.GeomSetCategoryBits(collide_geom, 0);
1232 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
1233 }
1234 }
1235 else
1236 {
1237 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1238 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1239 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1240 {
1241 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1242 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1243 }
1244 }
1245 }
1246 }
1247
1248 private void createAMotor(Vector3 axis)
1249 {
1250 if (Body == IntPtr.Zero)
1251 return;
1252
1253 if (Amotor != IntPtr.Zero)
1254 {
1255 d.JointDestroy(Amotor);
1256 Amotor = IntPtr.Zero;
1257 }
1258
1259 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1260
1261 if (axisnum <= 0)
1262 return;
1263
1264 // stop it
1265 d.BodySetTorque(Body, 0, 0, 0);
1266 d.BodySetAngularVel(Body, 0, 0, 0);
1267
1268 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1269 d.JointAttach(Amotor, Body, IntPtr.Zero);
1270
1271 d.JointSetAMotorMode(Amotor, 0);
1272
1273 d.JointSetAMotorNumAxes(Amotor, axisnum);
1274
1275 // get current orientation to lock
1276
1277 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1278 Quaternion curr; // crap convertion between identical things
1279 curr.X = dcur.X;
1280 curr.Y = dcur.Y;
1281 curr.Z = dcur.Z;
1282 curr.W = dcur.W;
1283 Vector3 ax;
1284
1285 int i = 0;
1286 int j = 0;
1287 if (axis.X == 0)
1288 {
1289 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1290 // ODE should do this with axis relative to body 1 but seems to fail
1291 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1292 d.JointSetAMotorAngle(Amotor, 0, 0);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1294 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1295 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1296 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1297 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1298 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1299 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1300 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1301 i++;
1302 j = 256; // move to next axis set
1303 }
1304
1305 if (axis.Y == 0)
1306 {
1307 ax = (new Vector3(0, 1, 0)) * curr;
1308 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1309 d.JointSetAMotorAngle(Amotor, i, 0);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1311 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1312 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1313 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1314 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1315 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1316 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1317 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1318 i++;
1319 j += 256;
1320 }
1321
1322 if (axis.Z == 0)
1323 {
1324 ax = (new Vector3(0, 0, 1)) * curr;
1325 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1326 d.JointSetAMotorAngle(Amotor, i, 0);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1328 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1329 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1330 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1331 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1332 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1333 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1334 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1335 }
1336 }
1337
1338
1339 private void SetGeom(IntPtr geom)
1340 {
1341 prim_geom = geom;
1342 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1343 if (prim_geom != IntPtr.Zero)
1344 {
1345
1346 if (m_NoColide)
1347 {
1348 d.GeomSetCategoryBits(prim_geom, 0);
1349 if (m_isphysical)
1350 {
1351 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1352 }
1353 else
1354 {
1355 d.GeomSetCollideBits(prim_geom, 0);
1356 d.GeomDisable(prim_geom);
1357 }
1358 }
1359 else
1360 {
1361 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1362 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1363 }
1364
1365 UpdatePrimBodyData();
1366 _parent_scene.actor_name_map[prim_geom] = this;
1367
1368/*
1369// debug
1370 d.AABB aabb;
1371 d.GeomGetAABB(prim_geom, out aabb);
1372 float x = aabb.MaxX - aabb.MinX;
1373 float y = aabb.MaxY - aabb.MinY;
1374 float z = aabb.MaxZ - aabb.MinZ;
1375 if( x > 60.0f || y > 60.0f || z > 60.0f)
1376 m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1377 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1378 else if (x < 0.001f || y < 0.001f || z < 0.001f)
1379 m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1380 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1381
1382//
1383*/
1384
1385 }
1386 else
1387 m_log.Warn("Setting bad Geom");
1388 }
1389
1390 private bool GetMeshGeom()
1391 {
1392 IntPtr vertices, indices;
1393 int vertexCount, indexCount;
1394 int vertexStride, triStride;
1395
1396 IMesh mesh = m_mesh;
1397
1398 if (mesh == null)
1399 return false;
1400
1401 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
1402 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
1403
1404 if (vertexCount == 0 || indexCount == 0)
1405 {
1406 m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}",
1407 Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString());
1408
1409 m_hasOBB = false;
1410 m_OBBOffset = Vector3.Zero;
1411 m_OBB = _size * 0.5f;
1412
1413 m_physCost = 0.1f;
1414 m_streamCost = 1.0f;
1415
1416 _parent_scene.mesher.ReleaseMesh(mesh);
1417 m_meshState = MeshState.MeshFailed;
1418 m_mesh = null;
1419 return false;
1420 }
1421
1422 IntPtr geo = IntPtr.Zero;
1423
1424 try
1425 {
1426 _triMeshData = d.GeomTriMeshDataCreate();
1427
1428 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1429 d.GeomTriMeshDataPreprocess(_triMeshData);
1430
1431 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1432 }
1433
1434 catch (Exception e)
1435 {
1436 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1437 if (_triMeshData != IntPtr.Zero)
1438 {
1439 try
1440 {
1441 d.GeomTriMeshDataDestroy(_triMeshData);
1442 }
1443 catch
1444 {
1445 }
1446 }
1447 _triMeshData = IntPtr.Zero;
1448
1449 m_hasOBB = false;
1450 m_OBBOffset = Vector3.Zero;
1451 m_OBB = _size * 0.5f;
1452 m_physCost = 0.1f;
1453 m_streamCost = 1.0f;
1454
1455 _parent_scene.mesher.ReleaseMesh(mesh);
1456 m_meshState = MeshState.MeshFailed;
1457 m_mesh = null;
1458 return false;
1459 }
1460
1461 m_physCost = 0.0013f * (float)indexCount;
1462 // todo
1463 m_streamCost = 1.0f;
1464
1465 SetGeom(geo);
1466
1467 return true;
1468 }
1469
1470 private void CreateGeom()
1471 {
1472 bool hasMesh = false;
1473
1474 m_NoColide = false;
1475
1476 if ((m_meshState & MeshState.MeshNoColide) != 0)
1477 m_NoColide = true;
1478
1479 else if(m_mesh != null)
1480 {
1481 if (GetMeshGeom())
1482 hasMesh = true;
1483 else
1484 m_NoColide = true;
1485 }
1486
1487
1488 if (!hasMesh)
1489 {
1490 IntPtr geo = IntPtr.Zero;
1491
1492 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1493 && _size.X == _size.Y && _size.Y == _size.Z)
1494 { // it's a sphere
1495 try
1496 {
1497 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
1498 }
1499 catch (Exception e)
1500 {
1501 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1502 return;
1503 }
1504 }
1505 else
1506 {// do it as a box
1507 try
1508 {
1509 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
1510 }
1511 catch (Exception e)
1512 {
1513 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1514 return;
1515 }
1516 }
1517 m_physCost = 0.1f;
1518 m_streamCost = 1.0f;
1519 SetGeom(geo);
1520 }
1521 }
1522
1523 private void RemoveGeom()
1524 {
1525 if (prim_geom != IntPtr.Zero)
1526 {
1527 _parent_scene.actor_name_map.Remove(prim_geom);
1528
1529 try
1530 {
1531 d.GeomDestroy(prim_geom);
1532 if (_triMeshData != IntPtr.Zero)
1533 {
1534 d.GeomTriMeshDataDestroy(_triMeshData);
1535 _triMeshData = IntPtr.Zero;
1536 }
1537 }
1538 catch (Exception e)
1539 {
1540 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1541 }
1542
1543 prim_geom = IntPtr.Zero;
1544 collide_geom = IntPtr.Zero;
1545 m_targetSpace = IntPtr.Zero;
1546 }
1547 else
1548 {
1549 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1550 }
1551
1552 lock (m_meshlock)
1553 {
1554 if (m_mesh != null)
1555 {
1556 _parent_scene.mesher.ReleaseMesh(m_mesh);
1557 m_mesh = null;
1558 }
1559 }
1560
1561 Body = IntPtr.Zero;
1562 m_hasOBB = false;
1563 }
1564
1565 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1566 // should only be called for non physical prims unless they are becoming non physical
1567 private void SetInStaticSpace(OdePrim prim)
1568 {
1569 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1570 prim.m_targetSpace = targetSpace;
1571 collide_geom = IntPtr.Zero;
1572 }
1573
1574 public void enableBodySoft()
1575 {
1576 m_disabled = false;
1577 if (!childPrim && !m_isSelected)
1578 {
1579 if (m_isphysical && Body != IntPtr.Zero)
1580 {
1581 UpdateCollisionCatFlags();
1582 ApplyCollisionCatFlags();
1583
1584 d.BodyEnable(Body);
1585 }
1586 }
1587 resetCollisionAccounting();
1588 }
1589
1590 private void disableBodySoft()
1591 {
1592 m_disabled = true;
1593 if (!childPrim)
1594 {
1595 if (m_isphysical && Body != IntPtr.Zero)
1596 {
1597 if (m_isSelected)
1598 m_collisionFlags = CollisionCategories.Selected;
1599 else
1600 m_collisionCategories = 0;
1601 m_collisionFlags = 0;
1602 ApplyCollisionCatFlags();
1603 d.BodyDisable(Body);
1604 }
1605 }
1606 }
1607
1608 private void MakeBody()
1609 {
1610 if (!m_isphysical) // only physical get bodies
1611 return;
1612
1613 if (childPrim) // child prims don't get bodies;
1614 return;
1615
1616 if (m_building)
1617 return;
1618
1619 if (prim_geom == IntPtr.Zero)
1620 {
1621 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1622 return;
1623 }
1624
1625 if (Body != IntPtr.Zero)
1626 {
1627 DestroyBody();
1628 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1629 }
1630
1631 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1632 {
1633 d.GeomSetBody(prim_geom, IntPtr.Zero);
1634 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1635 }
1636
1637 d.Matrix3 mymat = new d.Matrix3();
1638 d.Quaternion myrot = new d.Quaternion();
1639 d.Mass objdmass = new d.Mass { };
1640
1641 Body = d.BodyCreate(_parent_scene.world);
1642
1643 objdmass = primdMass;
1644
1645 // rotate inertia
1646 myrot.X = _orientation.X;
1647 myrot.Y = _orientation.Y;
1648 myrot.Z = _orientation.Z;
1649 myrot.W = _orientation.W;
1650
1651 d.RfromQ(out mymat, ref myrot);
1652 d.MassRotate(ref objdmass, ref mymat);
1653
1654 // set the body rotation
1655 d.BodySetRotation(Body, ref mymat);
1656
1657 // recompute full object inertia if needed
1658 if (childrenPrim.Count > 0)
1659 {
1660 d.Matrix3 mat = new d.Matrix3();
1661 d.Quaternion quat = new d.Quaternion();
1662 d.Mass tmpdmass = new d.Mass { };
1663 Vector3 rcm;
1664
1665 rcm.X = _position.X;
1666 rcm.Y = _position.Y;
1667 rcm.Z = _position.Z;
1668
1669 lock (childrenPrim)
1670 {
1671 foreach (OdePrim prm in childrenPrim)
1672 {
1673 if (prm.prim_geom == IntPtr.Zero)
1674 {
1675 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1676 continue;
1677 }
1678
1679 tmpdmass = prm.primdMass;
1680
1681 // apply prim current rotation to inertia
1682 quat.X = prm._orientation.X;
1683 quat.Y = prm._orientation.Y;
1684 quat.Z = prm._orientation.Z;
1685 quat.W = prm._orientation.W;
1686 d.RfromQ(out mat, ref quat);
1687 d.MassRotate(ref tmpdmass, ref mat);
1688
1689 Vector3 ppos = prm._position;
1690 ppos.X -= rcm.X;
1691 ppos.Y -= rcm.Y;
1692 ppos.Z -= rcm.Z;
1693 // refer inertia to root prim center of mass position
1694 d.MassTranslate(ref tmpdmass,
1695 ppos.X,
1696 ppos.Y,
1697 ppos.Z);
1698
1699 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1700 // fix prim colision cats
1701
1702 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1703 {
1704 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1705 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1706 }
1707
1708 d.GeomClearOffset(prm.prim_geom);
1709 d.GeomSetBody(prm.prim_geom, Body);
1710 prm.Body = Body;
1711 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1712 }
1713 }
1714 }
1715
1716 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1717 // associate root geom with body
1718 d.GeomSetBody(prim_geom, Body);
1719
1720 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1721 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1722
1723 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1724 myrot.X = -myrot.X;
1725 myrot.Y = -myrot.Y;
1726 myrot.Z = -myrot.Z;
1727
1728 d.RfromQ(out mymat, ref myrot);
1729 d.MassRotate(ref objdmass, ref mymat);
1730
1731 d.BodySetMass(Body, ref objdmass);
1732 _mass = objdmass.mass;
1733
1734 // disconnect from world gravity so we can apply buoyancy
1735 d.BodySetGravityMode(Body, false);
1736
1737 d.BodySetAutoDisableFlag(Body, true);
1738 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1739 d.BodySetDamping(Body, .005f, .005f);
1740
1741 if (m_targetSpace != IntPtr.Zero)
1742 {
1743 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1744 if (d.SpaceQuery(m_targetSpace, prim_geom))
1745 d.SpaceRemove(m_targetSpace, prim_geom);
1746 }
1747
1748 if (childrenPrim.Count == 0)
1749 {
1750 collide_geom = prim_geom;
1751 m_targetSpace = _parent_scene.ActiveSpace;
1752 }
1753 else
1754 {
1755 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1756 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1757 d.SpaceSetSublevel(m_targetSpace, 3);
1758 d.SpaceSetCleanup(m_targetSpace, false);
1759
1760 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1761 CollisionCategories.Geom |
1762 CollisionCategories.Phantom |
1763 CollisionCategories.VolumeDtc
1764 ));
1765 d.GeomSetCollideBits(m_targetSpace, 0);
1766 collide_geom = m_targetSpace;
1767 }
1768
1769 d.SpaceAdd(m_targetSpace, prim_geom);
1770
1771 if (m_delaySelect)
1772 {
1773 m_isSelected = true;
1774 m_delaySelect = false;
1775 }
1776
1777 m_collisionscore = 0;
1778
1779 UpdateCollisionCatFlags();
1780 ApplyCollisionCatFlags();
1781
1782 _parent_scene.addActivePrim(this);
1783
1784 lock (childrenPrim)
1785 {
1786 foreach (OdePrim prm in childrenPrim)
1787 {
1788 if (prm.prim_geom == IntPtr.Zero)
1789 continue;
1790
1791 Vector3 ppos = prm._position;
1792 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1793
1794 if (prm.m_targetSpace != m_targetSpace)
1795 {
1796 if (prm.m_targetSpace != IntPtr.Zero)
1797 {
1798 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1799 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1800 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1801 }
1802 prm.m_targetSpace = m_targetSpace;
1803 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1804 }
1805
1806 prm.m_collisionscore = 0;
1807
1808 if(!m_disabled)
1809 prm.m_disabled = false;
1810
1811 _parent_scene.addActivePrim(prm);
1812 }
1813 }
1814
1815 // The body doesn't already have a finite rotation mode set here
1816 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1817 {
1818 createAMotor(m_angularlock);
1819 }
1820
1821
1822 if (m_isSelected || m_disabled)
1823 {
1824 d.BodyDisable(Body);
1825 }
1826 else
1827 {
1828 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1829 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1830 }
1831 _parent_scene.addActiveGroups(this);
1832 }
1833
1834 private void DestroyBody()
1835 {
1836 if (Body != IntPtr.Zero)
1837 {
1838 _parent_scene.remActivePrim(this);
1839
1840 collide_geom = IntPtr.Zero;
1841
1842 if (m_disabled)
1843 m_collisionCategories = 0;
1844 else if (m_isSelected)
1845 m_collisionCategories = CollisionCategories.Selected;
1846 else if (m_isVolumeDetect)
1847 m_collisionCategories = CollisionCategories.VolumeDtc;
1848 else if (m_isphantom)
1849 m_collisionCategories = CollisionCategories.Phantom;
1850 else
1851 m_collisionCategories = CollisionCategories.Geom;
1852
1853 m_collisionFlags = 0;
1854
1855 if (prim_geom != IntPtr.Zero)
1856 {
1857 if (m_NoColide)
1858 {
1859 d.GeomSetCategoryBits(prim_geom, 0);
1860 d.GeomSetCollideBits(prim_geom, 0);
1861 }
1862 else
1863 {
1864 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1865 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1866 }
1867 UpdateDataFromGeom();
1868 d.GeomSetBody(prim_geom, IntPtr.Zero);
1869 SetInStaticSpace(this);
1870 }
1871
1872 if (!childPrim)
1873 {
1874 lock (childrenPrim)
1875 {
1876 foreach (OdePrim prm in childrenPrim)
1877 {
1878 _parent_scene.remActivePrim(prm);
1879
1880 if (prm.m_isSelected)
1881 prm.m_collisionCategories = CollisionCategories.Selected;
1882 else if (prm.m_isVolumeDetect)
1883 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1884 else if (prm.m_isphantom)
1885 prm.m_collisionCategories = CollisionCategories.Phantom;
1886 else
1887 prm.m_collisionCategories = CollisionCategories.Geom;
1888
1889 prm.m_collisionFlags = 0;
1890
1891 if (prm.prim_geom != IntPtr.Zero)
1892 {
1893 if (prm.m_NoColide)
1894 {
1895 d.GeomSetCategoryBits(prm.prim_geom, 0);
1896 d.GeomSetCollideBits(prm.prim_geom, 0);
1897 }
1898 else
1899 {
1900 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1901 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1902 }
1903 prm.UpdateDataFromGeom();
1904 SetInStaticSpace(prm);
1905 }
1906 prm.Body = IntPtr.Zero;
1907 prm._mass = prm.primMass;
1908 prm.m_collisionscore = 0;
1909 }
1910 }
1911 if (Amotor != IntPtr.Zero)
1912 {
1913 d.JointDestroy(Amotor);
1914 Amotor = IntPtr.Zero;
1915 }
1916 _parent_scene.remActiveGroup(this);
1917 d.BodyDestroy(Body);
1918 }
1919 Body = IntPtr.Zero;
1920 }
1921 _mass = primMass;
1922 m_collisionscore = 0;
1923 }
1924
1925 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1926 {
1927 d.Matrix3 mat = new d.Matrix3();
1928 d.Quaternion quat = new d.Quaternion();
1929
1930 d.Mass tmpdmass = new d.Mass { };
1931 d.Mass objdmass = new d.Mass { };
1932
1933 d.BodyGetMass(Body, out tmpdmass);
1934 objdmass = tmpdmass;
1935
1936 d.Vector3 dobjpos;
1937 d.Vector3 thispos;
1938
1939 // get current object position and rotation
1940 dobjpos = d.BodyGetPosition(Body);
1941
1942 // get prim own inertia in its local frame
1943 tmpdmass = primdMass;
1944
1945 // transform to object frame
1946 mat = d.GeomGetOffsetRotation(prim_geom);
1947 d.MassRotate(ref tmpdmass, ref mat);
1948
1949 thispos = d.GeomGetOffsetPosition(prim_geom);
1950 d.MassTranslate(ref tmpdmass,
1951 thispos.X,
1952 thispos.Y,
1953 thispos.Z);
1954
1955 // subtract current prim inertia from object
1956 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1957
1958 // back prim own inertia
1959 tmpdmass = primdMass;
1960
1961 // update to new position and orientation
1962 _position = NewPos;
1963 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1964 _orientation = newrot;
1965 quat.X = newrot.X;
1966 quat.Y = newrot.Y;
1967 quat.Z = newrot.Z;
1968 quat.W = newrot.W;
1969 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1970
1971 mat = d.GeomGetOffsetRotation(prim_geom);
1972 d.MassRotate(ref tmpdmass, ref mat);
1973
1974 thispos = d.GeomGetOffsetPosition(prim_geom);
1975 d.MassTranslate(ref tmpdmass,
1976 thispos.X,
1977 thispos.Y,
1978 thispos.Z);
1979
1980 d.MassAdd(ref objdmass, ref tmpdmass);
1981
1982 // fix all positions
1983 IntPtr g = d.BodyGetFirstGeom(Body);
1984 while (g != IntPtr.Zero)
1985 {
1986 thispos = d.GeomGetOffsetPosition(g);
1987 thispos.X -= objdmass.c.X;
1988 thispos.Y -= objdmass.c.Y;
1989 thispos.Z -= objdmass.c.Z;
1990 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1991 g = d.dBodyGetNextGeom(g);
1992 }
1993 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1994
1995 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1996 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1997 d.BodySetMass(Body, ref objdmass);
1998 _mass = objdmass.mass;
1999 }
2000
2001
2002
2003 private void FixInertia(Vector3 NewPos)
2004 {
2005 d.Matrix3 primmat = new d.Matrix3();
2006 d.Mass tmpdmass = new d.Mass { };
2007 d.Mass objdmass = new d.Mass { };
2008 d.Mass primmass = new d.Mass { };
2009
2010 d.Vector3 dobjpos;
2011 d.Vector3 thispos;
2012
2013 d.BodyGetMass(Body, out objdmass);
2014
2015 // get prim own inertia in its local frame
2016 primmass = primdMass;
2017 // transform to object frame
2018 primmat = d.GeomGetOffsetRotation(prim_geom);
2019 d.MassRotate(ref primmass, ref primmat);
2020
2021 tmpdmass = primmass;
2022
2023 thispos = d.GeomGetOffsetPosition(prim_geom);
2024 d.MassTranslate(ref tmpdmass,
2025 thispos.X,
2026 thispos.Y,
2027 thispos.Z);
2028
2029 // subtract current prim inertia from object
2030 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2031
2032 // update to new position
2033 _position = NewPos;
2034 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2035
2036 thispos = d.GeomGetOffsetPosition(prim_geom);
2037 d.MassTranslate(ref primmass,
2038 thispos.X,
2039 thispos.Y,
2040 thispos.Z);
2041
2042 d.MassAdd(ref objdmass, ref primmass);
2043
2044 // fix all positions
2045 IntPtr g = d.BodyGetFirstGeom(Body);
2046 while (g != IntPtr.Zero)
2047 {
2048 thispos = d.GeomGetOffsetPosition(g);
2049 thispos.X -= objdmass.c.X;
2050 thispos.Y -= objdmass.c.Y;
2051 thispos.Z -= objdmass.c.Z;
2052 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2053 g = d.dBodyGetNextGeom(g);
2054 }
2055
2056 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2057
2058 // get current object position and rotation
2059 dobjpos = d.BodyGetPosition(Body);
2060
2061 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2062 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2063 d.BodySetMass(Body, ref objdmass);
2064 _mass = objdmass.mass;
2065 }
2066
2067 private void FixInertia(Quaternion newrot)
2068 {
2069 d.Matrix3 mat = new d.Matrix3();
2070 d.Quaternion quat = new d.Quaternion();
2071
2072 d.Mass tmpdmass = new d.Mass { };
2073 d.Mass objdmass = new d.Mass { };
2074 d.Vector3 dobjpos;
2075 d.Vector3 thispos;
2076
2077 d.BodyGetMass(Body, out objdmass);
2078
2079 // get prim own inertia in its local frame
2080 tmpdmass = primdMass;
2081 mat = d.GeomGetOffsetRotation(prim_geom);
2082 d.MassRotate(ref tmpdmass, ref mat);
2083 // transform to object frame
2084 thispos = d.GeomGetOffsetPosition(prim_geom);
2085 d.MassTranslate(ref tmpdmass,
2086 thispos.X,
2087 thispos.Y,
2088 thispos.Z);
2089
2090 // subtract current prim inertia from object
2091 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2092
2093 // update to new orientation
2094 _orientation = newrot;
2095 quat.X = newrot.X;
2096 quat.Y = newrot.Y;
2097 quat.Z = newrot.Z;
2098 quat.W = newrot.W;
2099 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2100
2101 tmpdmass = primdMass;
2102 mat = d.GeomGetOffsetRotation(prim_geom);
2103 d.MassRotate(ref tmpdmass, ref mat);
2104 d.MassTranslate(ref tmpdmass,
2105 thispos.X,
2106 thispos.Y,
2107 thispos.Z);
2108
2109 d.MassAdd(ref objdmass, ref tmpdmass);
2110
2111 // fix all positions
2112 IntPtr g = d.BodyGetFirstGeom(Body);
2113 while (g != IntPtr.Zero)
2114 {
2115 thispos = d.GeomGetOffsetPosition(g);
2116 thispos.X -= objdmass.c.X;
2117 thispos.Y -= objdmass.c.Y;
2118 thispos.Z -= objdmass.c.Z;
2119 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2120 g = d.dBodyGetNextGeom(g);
2121 }
2122
2123 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2124 // get current object position and rotation
2125 dobjpos = d.BodyGetPosition(Body);
2126
2127 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2128 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2129 d.BodySetMass(Body, ref objdmass);
2130 _mass = objdmass.mass;
2131 }
2132
2133
2134 #region Mass Calculation
2135
2136 private void UpdatePrimBodyData()
2137 {
2138 primMass = m_density * primVolume;
2139
2140 if (primMass <= 0)
2141 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2142 if (primMass > _parent_scene.maximumMassObject)
2143 primMass = _parent_scene.maximumMassObject;
2144
2145 _mass = primMass; // just in case
2146
2147 d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z);
2148
2149 d.MassTranslate(ref primdMass,
2150 m_OBBOffset.X,
2151 m_OBBOffset.Y,
2152 m_OBBOffset.Z);
2153
2154 primOOBradiusSQ = m_OBB.LengthSquared();
2155
2156 if (_triMeshData != IntPtr.Zero)
2157 {
2158 float pc = m_physCost;
2159 float psf = primOOBradiusSQ;
2160 psf *= 1.33f * .2f;
2161 pc *= psf;
2162 if (pc < 0.1f)
2163 pc = 0.1f;
2164
2165 m_physCost = pc;
2166 }
2167 else
2168 m_physCost = 0.1f;
2169
2170 m_streamCost = 1.0f;
2171 }
2172
2173 #endregion
2174
2175
2176 /// <summary>
2177 /// Add a child prim to this parent prim.
2178 /// </summary>
2179 /// <param name="prim">Child prim</param>
2180 // I'm the parent
2181 // prim is the child
2182 public void ParentPrim(OdePrim prim)
2183 {
2184 //Console.WriteLine("ParentPrim " + m_primName);
2185 if (this.m_localID != prim.m_localID)
2186 {
2187 DestroyBody(); // for now we need to rebuil entire object on link change
2188
2189 lock (childrenPrim)
2190 {
2191 // adopt the prim
2192 if (!childrenPrim.Contains(prim))
2193 childrenPrim.Add(prim);
2194
2195 // see if this prim has kids and adopt them also
2196 // should not happen for now
2197 foreach (OdePrim prm in prim.childrenPrim)
2198 {
2199 if (!childrenPrim.Contains(prm))
2200 {
2201 if (prm.Body != IntPtr.Zero)
2202 {
2203 if (prm.prim_geom != IntPtr.Zero)
2204 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2205 if (prm.Body != prim.Body)
2206 prm.DestroyBody(); // don't loose bodies around
2207 prm.Body = IntPtr.Zero;
2208 }
2209
2210 childrenPrim.Add(prm);
2211 prm._parent = this;
2212 }
2213 }
2214 }
2215 //Remove old children from the prim
2216 prim.childrenPrim.Clear();
2217
2218 if (prim.Body != IntPtr.Zero)
2219 {
2220 if (prim.prim_geom != IntPtr.Zero)
2221 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2222 prim.DestroyBody(); // don't loose bodies around
2223 prim.Body = IntPtr.Zero;
2224 }
2225
2226 prim.childPrim = true;
2227 prim._parent = this;
2228
2229 MakeBody(); // full nasty reconstruction
2230 }
2231 }
2232
2233 private void UpdateChildsfromgeom()
2234 {
2235 if (childrenPrim.Count > 0)
2236 {
2237 foreach (OdePrim prm in childrenPrim)
2238 prm.UpdateDataFromGeom();
2239 }
2240 }
2241
2242 private void UpdateDataFromGeom()
2243 {
2244 if (prim_geom != IntPtr.Zero)
2245 {
2246 d.Quaternion qtmp;
2247 d.GeomCopyQuaternion(prim_geom, out qtmp);
2248 _orientation.X = qtmp.X;
2249 _orientation.Y = qtmp.Y;
2250 _orientation.Z = qtmp.Z;
2251 _orientation.W = qtmp.W;
2252/*
2253// Debug
2254 float qlen = _orientation.Length();
2255 if (qlen > 1.01f || qlen < 0.99)
2256 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
2257//
2258*/
2259 _orientation.Normalize();
2260
2261 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2262 _position.X = lpos.X;
2263 _position.Y = lpos.Y;
2264 _position.Z = lpos.Z;
2265 }
2266 }
2267
2268 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2269 {
2270 // Okay, we have a delinked child.. destroy all body and remake
2271 if (odePrim != this && !childrenPrim.Contains(odePrim))
2272 return;
2273
2274 DestroyBody();
2275
2276 if (odePrim == this) // delinking the root prim
2277 {
2278 OdePrim newroot = null;
2279 lock (childrenPrim)
2280 {
2281 if (childrenPrim.Count > 0)
2282 {
2283 newroot = childrenPrim[0];
2284 childrenPrim.RemoveAt(0);
2285 foreach (OdePrim prm in childrenPrim)
2286 {
2287 newroot.childrenPrim.Add(prm);
2288 }
2289 childrenPrim.Clear();
2290 }
2291 if (newroot != null)
2292 {
2293 newroot.childPrim = false;
2294 newroot._parent = null;
2295 if (remakebodies)
2296 newroot.MakeBody();
2297 }
2298 }
2299 }
2300
2301 else
2302 {
2303 lock (childrenPrim)
2304 {
2305 childrenPrim.Remove(odePrim);
2306 odePrim.childPrim = false;
2307 odePrim._parent = null;
2308 // odePrim.UpdateDataFromGeom();
2309 if (remakebodies)
2310 odePrim.MakeBody();
2311 }
2312 }
2313 if (remakebodies)
2314 MakeBody();
2315 }
2316
2317 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2318 {
2319 // Okay, we have a delinked child.. destroy all body and remake
2320 if (odePrim != this && !childrenPrim.Contains(odePrim))
2321 return;
2322
2323 DestroyBody();
2324
2325 if (odePrim == this)
2326 {
2327 OdePrim newroot = null;
2328 lock (childrenPrim)
2329 {
2330 if (childrenPrim.Count > 0)
2331 {
2332 newroot = childrenPrim[0];
2333 childrenPrim.RemoveAt(0);
2334 foreach (OdePrim prm in childrenPrim)
2335 {
2336 newroot.childrenPrim.Add(prm);
2337 }
2338 childrenPrim.Clear();
2339 }
2340 if (newroot != null)
2341 {
2342 newroot.childPrim = false;
2343 newroot._parent = null;
2344 newroot.MakeBody();
2345 }
2346 }
2347 if (reMakeBody)
2348 MakeBody();
2349 return;
2350 }
2351 else
2352 {
2353 lock (childrenPrim)
2354 {
2355 childrenPrim.Remove(odePrim);
2356 odePrim.childPrim = false;
2357 odePrim._parent = null;
2358 if (reMakeBody)
2359 odePrim.MakeBody();
2360 }
2361 }
2362 MakeBody();
2363 }
2364
2365 #region changes
2366
2367 private void changeadd()
2368 {
2369 }
2370
2371 private void changeAngularLock(Vector3 newLock)
2372 {
2373 // do we have a Physical object?
2374 if (Body != IntPtr.Zero)
2375 {
2376 //Check that we have a Parent
2377 //If we have a parent then we're not authorative here
2378 if (_parent == null)
2379 {
2380 if (!newLock.ApproxEquals(Vector3.One, 0f))
2381 {
2382 createAMotor(newLock);
2383 }
2384 else
2385 {
2386 if (Amotor != IntPtr.Zero)
2387 {
2388 d.JointDestroy(Amotor);
2389 Amotor = IntPtr.Zero;
2390 }
2391 }
2392 }
2393 }
2394 // Store this for later in case we get turned into a separate body
2395 m_angularlock = newLock;
2396 }
2397
2398 private void changeLink(OdePrim NewParent)
2399 {
2400 if (_parent == null && NewParent != null)
2401 {
2402 NewParent.ParentPrim(this);
2403 }
2404 else if (_parent != null)
2405 {
2406 if (_parent is OdePrim)
2407 {
2408 if (NewParent != _parent)
2409 {
2410 (_parent as OdePrim).ChildDelink(this, false); // for now...
2411 childPrim = false;
2412
2413 if (NewParent != null)
2414 {
2415 NewParent.ParentPrim(this);
2416 }
2417 }
2418 }
2419 }
2420 _parent = NewParent;
2421 }
2422
2423
2424 private void Stop()
2425 {
2426 if (!childPrim)
2427 {
2428// m_force = Vector3.Zero;
2429 m_forceacc = Vector3.Zero;
2430 m_angularForceacc = Vector3.Zero;
2431// m_torque = Vector3.Zero;
2432 _velocity = Vector3.Zero;
2433 _acceleration = Vector3.Zero;
2434 m_rotationalVelocity = Vector3.Zero;
2435 _target_velocity = Vector3.Zero;
2436 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2437 m_vehicle.Stop();
2438
2439 _zeroFlag = false;
2440 base.RequestPhysicsterseUpdate();
2441 }
2442
2443 if (Body != IntPtr.Zero)
2444 {
2445 d.BodySetForce(Body, 0f, 0f, 0f);
2446 d.BodySetTorque(Body, 0f, 0f, 0f);
2447 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2448 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2449 }
2450 }
2451
2452 private void changePhantomStatus(bool newval)
2453 {
2454 m_isphantom = newval;
2455
2456 UpdateCollisionCatFlags();
2457 ApplyCollisionCatFlags();
2458 }
2459
2460/* not in use
2461 internal void ChildSelectedChange(bool childSelect)
2462 {
2463 if(childPrim)
2464 return;
2465
2466 if (childSelect == m_isSelected)
2467 return;
2468
2469 if (childSelect)
2470 {
2471 DoSelectedStatus(true);
2472 }
2473
2474 else
2475 {
2476 foreach (OdePrim prm in childrenPrim)
2477 {
2478 if (prm.m_isSelected)
2479 return;
2480 }
2481 DoSelectedStatus(false);
2482 }
2483 }
2484*/
2485 private void changeSelectedStatus(bool newval)
2486 {
2487 if (m_lastdoneSelected == newval)
2488 return;
2489
2490 m_lastdoneSelected = newval;
2491 DoSelectedStatus(newval);
2492 }
2493
2494 private void CheckDelaySelect()
2495 {
2496 if (m_delaySelect)
2497 {
2498 DoSelectedStatus(m_isSelected);
2499 }
2500 }
2501
2502 private void DoSelectedStatus(bool newval)
2503 {
2504 m_isSelected = newval;
2505 Stop();
2506
2507 if (newval)
2508 {
2509 if (!childPrim && Body != IntPtr.Zero)
2510 d.BodyDisable(Body);
2511
2512 if (m_delaySelect || m_isphysical)
2513 {
2514 m_collisionCategories = CollisionCategories.Selected;
2515 m_collisionFlags = 0;
2516
2517 if (!childPrim)
2518 {
2519 foreach (OdePrim prm in childrenPrim)
2520 {
2521 prm.m_collisionCategories = m_collisionCategories;
2522 prm.m_collisionFlags = m_collisionFlags;
2523
2524 if (prm.prim_geom != IntPtr.Zero)
2525 {
2526
2527 if (prm.m_NoColide)
2528 {
2529 d.GeomSetCategoryBits(prm.prim_geom, 0);
2530 d.GeomSetCollideBits(prm.prim_geom, 0);
2531 }
2532 else
2533 {
2534 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2535 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2536 }
2537 }
2538 prm.m_delaySelect = false;
2539 }
2540 }
2541// else if (_parent != null)
2542// ((OdePrim)_parent).ChildSelectedChange(true);
2543
2544
2545 if (prim_geom != IntPtr.Zero)
2546 {
2547 if (m_NoColide)
2548 {
2549 d.GeomSetCategoryBits(prim_geom, 0);
2550 d.GeomSetCollideBits(prim_geom, 0);
2551 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2552 {
2553 d.GeomSetCategoryBits(collide_geom, 0);
2554 d.GeomSetCollideBits(collide_geom, 0);
2555 }
2556
2557 }
2558 else
2559 {
2560 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2561 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2562 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2563 {
2564 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
2565 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2566 }
2567 }
2568 }
2569
2570 m_delaySelect = false;
2571 }
2572 else if(!m_isphysical)
2573 {
2574 m_delaySelect = true;
2575 }
2576 }
2577 else
2578 {
2579 if (!childPrim)
2580 {
2581 if (Body != IntPtr.Zero && !m_disabled)
2582 d.BodyEnable(Body);
2583 }
2584// else if (_parent != null)
2585// ((OdePrim)_parent).ChildSelectedChange(false);
2586
2587 UpdateCollisionCatFlags();
2588 ApplyCollisionCatFlags();
2589
2590 m_delaySelect = false;
2591 }
2592
2593 resetCollisionAccounting();
2594 }
2595
2596 private void changePosition(Vector3 newPos)
2597 {
2598 CheckDelaySelect();
2599 if (m_isphysical)
2600 {
2601 if (childPrim) // inertia is messed, must rebuild
2602 {
2603 if (m_building)
2604 {
2605 _position = newPos;
2606 }
2607
2608 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2609 {
2610 FixInertia(newPos);
2611 if (!d.BodyIsEnabled(Body))
2612 d.BodyEnable(Body);
2613 }
2614 }
2615 else
2616 {
2617 if (_position != newPos)
2618 {
2619 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2620 _position = newPos;
2621 }
2622 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2623 d.BodyEnable(Body);
2624 }
2625 }
2626 else
2627 {
2628 if (prim_geom != IntPtr.Zero)
2629 {
2630 if (newPos != _position)
2631 {
2632 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2633 _position = newPos;
2634
2635 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2636 }
2637 }
2638 }
2639 givefakepos--;
2640 if (givefakepos < 0)
2641 givefakepos = 0;
2642// changeSelectedStatus();
2643 resetCollisionAccounting();
2644 }
2645
2646 private void changeOrientation(Quaternion newOri)
2647 {
2648 CheckDelaySelect();
2649 if (m_isphysical)
2650 {
2651 if (childPrim) // inertia is messed, must rebuild
2652 {
2653 if (m_building)
2654 {
2655 _orientation = newOri;
2656 }
2657/*
2658 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2659 {
2660 FixInertia(_position, newOri);
2661 if (!d.BodyIsEnabled(Body))
2662 d.BodyEnable(Body);
2663 }
2664*/
2665 }
2666 else
2667 {
2668 if (newOri != _orientation)
2669 {
2670 d.Quaternion myrot = new d.Quaternion();
2671 myrot.X = newOri.X;
2672 myrot.Y = newOri.Y;
2673 myrot.Z = newOri.Z;
2674 myrot.W = newOri.W;
2675 d.GeomSetQuaternion(prim_geom, ref myrot);
2676 _orientation = newOri;
2677 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2678 createAMotor(m_angularlock);
2679 }
2680 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2681 d.BodyEnable(Body);
2682 }
2683 }
2684 else
2685 {
2686 if (prim_geom != IntPtr.Zero)
2687 {
2688 if (newOri != _orientation)
2689 {
2690 d.Quaternion myrot = new d.Quaternion();
2691 myrot.X = newOri.X;
2692 myrot.Y = newOri.Y;
2693 myrot.Z = newOri.Z;
2694 myrot.W = newOri.W;
2695 d.GeomSetQuaternion(prim_geom, ref myrot);
2696 _orientation = newOri;
2697 }
2698 }
2699 }
2700 givefakeori--;
2701 if (givefakeori < 0)
2702 givefakeori = 0;
2703 resetCollisionAccounting();
2704 }
2705
2706 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2707 {
2708 CheckDelaySelect();
2709 if (m_isphysical)
2710 {
2711 if (childPrim && m_building) // inertia is messed, must rebuild
2712 {
2713 _position = newPos;
2714 _orientation = newOri;
2715 }
2716 else
2717 {
2718 if (newOri != _orientation)
2719 {
2720 d.Quaternion myrot = new d.Quaternion();
2721 myrot.X = newOri.X;
2722 myrot.Y = newOri.Y;
2723 myrot.Z = newOri.Z;
2724 myrot.W = newOri.W;
2725 d.GeomSetQuaternion(prim_geom, ref myrot);
2726 _orientation = newOri;
2727 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2728 createAMotor(m_angularlock);
2729 }
2730 if (_position != newPos)
2731 {
2732 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2733 _position = newPos;
2734 }
2735 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2736 d.BodyEnable(Body);
2737 }
2738 }
2739 else
2740 {
2741 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2742 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2743
2744 if (prim_geom != IntPtr.Zero)
2745 {
2746 if (newOri != _orientation)
2747 {
2748 d.Quaternion myrot = new d.Quaternion();
2749 myrot.X = newOri.X;
2750 myrot.Y = newOri.Y;
2751 myrot.Z = newOri.Z;
2752 myrot.W = newOri.W;
2753 d.GeomSetQuaternion(prim_geom, ref myrot);
2754 _orientation = newOri;
2755 }
2756
2757 if (newPos != _position)
2758 {
2759 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2760 _position = newPos;
2761
2762 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2763 }
2764 }
2765 }
2766 givefakepos--;
2767 if (givefakepos < 0)
2768 givefakepos = 0;
2769 givefakeori--;
2770 if (givefakeori < 0)
2771 givefakeori = 0;
2772 resetCollisionAccounting();
2773 }
2774
2775 private void changeDisable(bool disable)
2776 {
2777 if (disable)
2778 {
2779 if (!m_disabled)
2780 disableBodySoft();
2781 }
2782 else
2783 {
2784 if (m_disabled)
2785 enableBodySoft();
2786 }
2787 }
2788
2789 private void changePhysicsStatus(bool NewStatus)
2790 {
2791 CheckDelaySelect();
2792
2793 m_isphysical = NewStatus;
2794
2795 if (!childPrim)
2796 {
2797 if (NewStatus)
2798 {
2799 if (Body == IntPtr.Zero)
2800 MakeBody();
2801 }
2802 else
2803 {
2804 if (Body != IntPtr.Zero)
2805 {
2806 DestroyBody();
2807 }
2808 Stop();
2809 }
2810 }
2811
2812 resetCollisionAccounting();
2813 }
2814
2815 private void changeSize(Vector3 newSize)
2816 {
2817 }
2818
2819 private void changeShape(PrimitiveBaseShape newShape)
2820 {
2821 }
2822
2823 private void changeAddPhysRep(ODEPhysRepData repData)
2824 {
2825 _size = repData.size; //??
2826 _pbs = repData.pbs;
2827 m_shapetype = repData.shapetype;
2828
2829 m_mesh = repData.mesh;
2830
2831 m_assetID = repData.assetID;
2832 m_meshState = repData.meshState;
2833
2834 m_hasOBB = repData.hasOBB;
2835 m_OBBOffset = repData.OBBOffset;
2836 m_OBB = repData.OBB;
2837
2838 primVolume = repData.volume;
2839
2840 CreateGeom();
2841
2842 if (prim_geom != IntPtr.Zero)
2843 {
2844 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2845 d.Quaternion myrot = new d.Quaternion();
2846 myrot.X = _orientation.X;
2847 myrot.Y = _orientation.Y;
2848 myrot.Z = _orientation.Z;
2849 myrot.W = _orientation.W;
2850 d.GeomSetQuaternion(prim_geom, ref myrot);
2851 }
2852
2853 if (!m_isphysical)
2854 {
2855 SetInStaticSpace(this);
2856 UpdateCollisionCatFlags();
2857 ApplyCollisionCatFlags();
2858 }
2859 else
2860 MakeBody();
2861
2862 if ((m_meshState & MeshState.NeedMask) != 0)
2863 {
2864 repData.size = _size;
2865 repData.pbs = _pbs;
2866 repData.shapetype = m_shapetype;
2867 _parent_scene.m_meshWorker.RequestMesh(repData);
2868 }
2869 }
2870
2871 private void changePhysRepData(ODEPhysRepData repData)
2872 {
2873 CheckDelaySelect();
2874
2875 OdePrim parent = (OdePrim)_parent;
2876
2877 bool chp = childPrim;
2878
2879 if (chp)
2880 {
2881 if (parent != null)
2882 {
2883 parent.DestroyBody();
2884 }
2885 }
2886 else
2887 {
2888 DestroyBody();
2889 }
2890
2891 RemoveGeom();
2892
2893 _size = repData.size;
2894 _pbs = repData.pbs;
2895 m_shapetype = repData.shapetype;
2896
2897 m_mesh = repData.mesh;
2898
2899 m_assetID = repData.assetID;
2900 m_meshState = repData.meshState;
2901
2902 m_hasOBB = repData.hasOBB;
2903 m_OBBOffset = repData.OBBOffset;
2904 m_OBB = repData.OBB;
2905
2906 primVolume = repData.volume;
2907
2908 CreateGeom();
2909
2910 if (prim_geom != IntPtr.Zero)
2911 {
2912 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2913 d.Quaternion myrot = new d.Quaternion();
2914 myrot.X = _orientation.X;
2915 myrot.Y = _orientation.Y;
2916 myrot.Z = _orientation.Z;
2917 myrot.W = _orientation.W;
2918 d.GeomSetQuaternion(prim_geom, ref myrot);
2919 }
2920
2921 if (m_isphysical)
2922 {
2923 if (chp)
2924 {
2925 if (parent != null)
2926 {
2927 parent.MakeBody();
2928 }
2929 }
2930 else
2931 MakeBody();
2932 }
2933 else
2934 {
2935 SetInStaticSpace(this);
2936 UpdateCollisionCatFlags();
2937 ApplyCollisionCatFlags();
2938 }
2939
2940 resetCollisionAccounting();
2941
2942 if ((m_meshState & MeshState.NeedMask) != 0)
2943 {
2944 repData.size = _size;
2945 repData.pbs = _pbs;
2946 repData.shapetype = m_shapetype;
2947 _parent_scene.m_meshWorker.RequestMesh(repData);
2948 }
2949 }
2950
2951 private void changeFloatOnWater(bool newval)
2952 {
2953 m_collidesWater = newval;
2954
2955 UpdateCollisionCatFlags();
2956 ApplyCollisionCatFlags();
2957 }
2958
2959 private void changeSetTorque(Vector3 newtorque)
2960 {
2961 if (!m_isSelected)
2962 {
2963 if (m_isphysical && Body != IntPtr.Zero)
2964 {
2965 if (m_disabled)
2966 enableBodySoft();
2967 else if (!d.BodyIsEnabled(Body))
2968 d.BodyEnable(Body);
2969
2970 }
2971 m_torque = newtorque;
2972 }
2973 }
2974
2975 private void changeForce(Vector3 force)
2976 {
2977 m_force = force;
2978 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2979 d.BodyEnable(Body);
2980 }
2981
2982 private void changeAddForce(Vector3 theforce)
2983 {
2984 m_forceacc += theforce;
2985 if (!m_isSelected)
2986 {
2987 lock (this)
2988 {
2989 //m_log.Info("[PHYSICS]: dequeing forcelist");
2990 if (m_isphysical && Body != IntPtr.Zero)
2991 {
2992 if (m_disabled)
2993 enableBodySoft();
2994 else if (!d.BodyIsEnabled(Body))
2995 d.BodyEnable(Body);
2996 }
2997 }
2998 m_collisionscore = 0;
2999 }
3000 }
3001
3002 // actually angular impulse
3003 private void changeAddAngularImpulse(Vector3 aimpulse)
3004 {
3005 m_angularForceacc += aimpulse * m_invTimeStep;
3006 if (!m_isSelected)
3007 {
3008 lock (this)
3009 {
3010 if (m_isphysical && Body != IntPtr.Zero)
3011 {
3012 if (m_disabled)
3013 enableBodySoft();
3014 else if (!d.BodyIsEnabled(Body))
3015 d.BodyEnable(Body);
3016 }
3017 }
3018 m_collisionscore = 0;
3019 }
3020 }
3021
3022 private void changevelocity(Vector3 newVel)
3023 {
3024 float len = newVel.LengthSquared();
3025 if (len > 100000.0f) // limit to 100m/s
3026 {
3027 len = 100.0f / (float)Math.Sqrt(len);
3028 newVel *= len;
3029 }
3030
3031 if (!m_isSelected)
3032 {
3033 if (Body != IntPtr.Zero)
3034 {
3035 if (m_disabled)
3036 enableBodySoft();
3037 else if (!d.BodyIsEnabled(Body))
3038 d.BodyEnable(Body);
3039
3040 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3041 }
3042 //resetCollisionAccounting();
3043 }
3044 _velocity = newVel;
3045 }
3046
3047 private void changeangvelocity(Vector3 newAngVel)
3048 {
3049 float len = newAngVel.LengthSquared();
3050 if (len > 144.0f) // limit to 12rad/s
3051 {
3052 len = 12.0f / (float)Math.Sqrt(len);
3053 newAngVel *= len;
3054 }
3055
3056 if (!m_isSelected)
3057 {
3058 if (Body != IntPtr.Zero)
3059 {
3060 if (m_disabled)
3061 enableBodySoft();
3062 else if (!d.BodyIsEnabled(Body))
3063 d.BodyEnable(Body);
3064
3065
3066 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
3067 }
3068 //resetCollisionAccounting();
3069 }
3070 m_rotationalVelocity = newAngVel;
3071 }
3072
3073 private void changeVolumedetetion(bool newVolDtc)
3074 {
3075 m_isVolumeDetect = newVolDtc;
3076 m_fakeisVolumeDetect = newVolDtc;
3077 UpdateCollisionCatFlags();
3078 ApplyCollisionCatFlags();
3079 }
3080
3081 protected void changeBuilding(bool newbuilding)
3082 {
3083 // Check if we need to do anything
3084 if (newbuilding == m_building)
3085 return;
3086
3087 if ((bool)newbuilding)
3088 {
3089 m_building = true;
3090 if (!childPrim)
3091 DestroyBody();
3092 }
3093 else
3094 {
3095 m_building = false;
3096 CheckDelaySelect();
3097 if (!childPrim)
3098 MakeBody();
3099 }
3100 if (!childPrim && childrenPrim.Count > 0)
3101 {
3102 foreach (OdePrim prm in childrenPrim)
3103 prm.changeBuilding(m_building); // call directly
3104 }
3105 }
3106
3107 public void changeSetVehicle(VehicleData vdata)
3108 {
3109 if (m_vehicle == null)
3110 m_vehicle = new ODEDynamics(this);
3111 m_vehicle.DoSetVehicle(vdata);
3112 }
3113
3114 private void changeVehicleType(int value)
3115 {
3116 if (value == (int)Vehicle.TYPE_NONE)
3117 {
3118 if (m_vehicle != null)
3119 m_vehicle = null;
3120 }
3121 else
3122 {
3123 if (m_vehicle == null)
3124 m_vehicle = new ODEDynamics(this);
3125
3126 m_vehicle.ProcessTypeChange((Vehicle)value);
3127 }
3128 }
3129
3130 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3131 {
3132 if (m_vehicle == null)
3133 return;
3134
3135 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3136 }
3137
3138 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3139 {
3140 if (m_vehicle == null)
3141 return;
3142 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3143 }
3144
3145 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3146 {
3147 if (m_vehicle == null)
3148 return;
3149 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3150 }
3151
3152 private void changeVehicleFlags(strVehicleBoolParam bp)
3153 {
3154 if (m_vehicle == null)
3155 return;
3156 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3157 }
3158
3159 private void changeBuoyancy(float b)
3160 {
3161 m_buoyancy = b;
3162 }
3163
3164 private void changePIDTarget(Vector3 trg)
3165 {
3166 m_PIDTarget = trg;
3167 }
3168
3169 private void changePIDTau(float tau)
3170 {
3171 m_PIDTau = tau;
3172 }
3173
3174 private void changePIDActive(bool val)
3175 {
3176 m_usePID = val;
3177 }
3178
3179 private void changePIDHoverHeight(float val)
3180 {
3181 m_PIDHoverHeight = val;
3182 if (val == 0)
3183 m_useHoverPID = false;
3184 }
3185
3186 private void changePIDHoverType(PIDHoverType type)
3187 {
3188 m_PIDHoverType = type;
3189 }
3190
3191 private void changePIDHoverTau(float tau)
3192 {
3193 m_PIDHoverTau = tau;
3194 }
3195
3196 private void changePIDHoverActive(bool active)
3197 {
3198 m_useHoverPID = active;
3199 }
3200
3201 #endregion
3202
3203 public void Move()
3204 {
3205 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3206 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3207 {
3208 if (!d.BodyIsEnabled(Body))
3209 {
3210 // let vehicles sleep
3211 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3212 return;
3213
3214 if (++bodydisablecontrol < 20)
3215 return;
3216
3217
3218 d.BodyEnable(Body);
3219 }
3220
3221 bodydisablecontrol = 0;
3222
3223 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3224
3225 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3226 {
3227 // 'VEHICLES' are dealt with in ODEDynamics.cs
3228 m_vehicle.Step();
3229 return;
3230 }
3231
3232 float fx = 0;
3233 float fy = 0;
3234 float fz = 0;
3235
3236 float m_mass = _mass;
3237
3238 if (m_usePID && m_PIDTau > 0)
3239 {
3240 // for now position error
3241 _target_velocity =
3242 new Vector3(
3243 (m_PIDTarget.X - lpos.X),
3244 (m_PIDTarget.Y - lpos.Y),
3245 (m_PIDTarget.Z - lpos.Z)
3246 );
3247
3248 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3249 {
3250 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3251 d.BodySetLinearVel(Body, 0, 0, 0);
3252 return;
3253 }
3254 else
3255 {
3256 _zeroFlag = false;
3257
3258 float tmp = 1 / m_PIDTau;
3259 _target_velocity *= tmp;
3260
3261 // apply limits
3262 tmp = _target_velocity.Length();
3263 if (tmp > 50.0f)
3264 {
3265 tmp = 50 / tmp;
3266 _target_velocity *= tmp;
3267 }
3268 else if (tmp < 0.05f)
3269 {
3270 tmp = 0.05f / tmp;
3271 _target_velocity *= tmp;
3272 }
3273
3274 d.Vector3 vel = d.BodyGetLinearVel(Body);
3275 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3276 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3277 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3278// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3279 }
3280 } // end if (m_usePID)
3281
3282 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3283 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3284 {
3285
3286 // Non-Vehicles have a limited set of Hover options.
3287 // determine what our target height really is based on HoverType
3288
3289 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3290
3291 switch (m_PIDHoverType)
3292 {
3293 case PIDHoverType.Ground:
3294 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3295 break;
3296
3297 case PIDHoverType.GroundAndWater:
3298 m_waterHeight = _parent_scene.GetWaterLevel();
3299 if (m_groundHeight > m_waterHeight)
3300 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3301 else
3302 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3303 break;
3304 } // end switch (m_PIDHoverType)
3305
3306 // don't go underground unless volumedetector
3307
3308 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3309 {
3310 d.Vector3 vel = d.BodyGetLinearVel(Body);
3311
3312 fz = (m_targetHoverHeight - lpos.Z);
3313
3314 // if error is zero, use position control; otherwise, velocity control
3315 if (Math.Abs(fz) < 0.01f)
3316 {
3317 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3318 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3319 }
3320 else
3321 {
3322 _zeroFlag = false;
3323 fz /= m_PIDHoverTau;
3324
3325 float tmp = Math.Abs(fz);
3326 if (tmp > 50)
3327 fz = 50 * Math.Sign(fz);
3328 else if (tmp < 0.1)
3329 fz = 0.1f * Math.Sign(fz);
3330
3331 fz = ((fz - vel.Z) * m_invTimeStep);
3332 }
3333 }
3334 }
3335 else
3336 {
3337 float b = (1.0f - m_buoyancy);
3338 fx = _parent_scene.gravityx * b;
3339 fy = _parent_scene.gravityy * b;
3340 fz = _parent_scene.gravityz * b;
3341 }
3342
3343 fx *= m_mass;
3344 fy *= m_mass;
3345 fz *= m_mass;
3346
3347 // constant force
3348 fx += m_force.X;
3349 fy += m_force.Y;
3350 fz += m_force.Z;
3351
3352 fx += m_forceacc.X;
3353 fy += m_forceacc.Y;
3354 fz += m_forceacc.Z;
3355
3356 m_forceacc = Vector3.Zero;
3357
3358 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3359 if (fx != 0 || fy != 0 || fz != 0)
3360 {
3361 d.BodyAddForce(Body, fx, fy, fz);
3362 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3363 }
3364
3365 Vector3 trq;
3366
3367 trq = m_torque;
3368 trq += m_angularForceacc;
3369 m_angularForceacc = Vector3.Zero;
3370 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3371 {
3372 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3373 }
3374 }
3375 else
3376 { // is not physical, or is not a body or is selected
3377 // _zeroPosition = d.BodyGetPosition(Body);
3378 return;
3379 //Console.WriteLine("Nothing " + Name);
3380
3381 }
3382 }
3383
3384 public void UpdatePositionAndVelocity()
3385 {
3386 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3387 {
3388 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3389 {
3390 bool lastZeroFlag = _zeroFlag;
3391
3392 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
3393
3394 // check outside region
3395 if (lpos.Z < -100 || lpos.Z > 100000f)
3396 {
3397 m_outbounds = true;
3398
3399 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3400 _acceleration.X = 0;
3401 _acceleration.Y = 0;
3402 _acceleration.Z = 0;
3403
3404 _velocity.X = 0;
3405 _velocity.Y = 0;
3406 _velocity.Z = 0;
3407 m_rotationalVelocity.X = 0;
3408 m_rotationalVelocity.Y = 0;
3409 m_rotationalVelocity.Z = 0;
3410
3411 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3412 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3413 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3414 m_lastposition = _position;
3415 m_lastorientation = _orientation;
3416
3417 base.RequestPhysicsterseUpdate();
3418
3419// throttleCounter = 0;
3420 _zeroFlag = true;
3421
3422 disableBodySoft(); // disable it and colisions
3423 base.RaiseOutOfBounds(_position);
3424 return;
3425 }
3426
3427 if (lpos.X < 0f)
3428 {
3429 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3430 m_outbounds = true;
3431 }
3432 else if (lpos.X > _parent_scene.WorldExtents.X)
3433 {
3434 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3435 m_outbounds = true;
3436 }
3437 if (lpos.Y < 0f)
3438 {
3439 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3440 m_outbounds = true;
3441 }
3442 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3443 {
3444 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3445 m_outbounds = true;
3446 }
3447
3448 if (m_outbounds)
3449 {
3450 m_lastposition = _position;
3451 m_lastorientation = _orientation;
3452
3453 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3454 m_rotationalVelocity.X = dtmp.X;
3455 m_rotationalVelocity.Y = dtmp.Y;
3456 m_rotationalVelocity.Z = dtmp.Z;
3457
3458 dtmp = d.BodyGetLinearVel(Body);
3459 _velocity.X = dtmp.X;
3460 _velocity.Y = dtmp.Y;
3461 _velocity.Z = dtmp.Z;
3462
3463 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3464 d.BodySetAngularVel(Body, 0, 0, 0);
3465 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3466 disableBodySoft(); // stop collisions
3467 UnSubscribeEvents();
3468
3469 base.RequestPhysicsterseUpdate();
3470 return;
3471 }
3472
3473 d.Quaternion ori;
3474 d.GeomCopyQuaternion(prim_geom, out ori);
3475
3476 // decide if moving
3477 // use positions since this are integrated quantities
3478 // tolerance values depende a lot on simulation noise...
3479 // use simple math.abs since we dont need to be exact
3480
3481 if (
3482 (Math.Abs(_position.X - lpos.X) < 0.001f)
3483 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3484 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3485 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3486 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3487 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3488 )
3489 {
3490 _zeroFlag = true;
3491 }
3492 else
3493 _zeroFlag = false;
3494
3495 // update velocities and aceleration
3496 if (!(_zeroFlag && lastZeroFlag))
3497 {
3498 d.Vector3 vel = d.BodyGetLinearVel(Body);
3499
3500 _acceleration = _velocity;
3501
3502 if ((Math.Abs(vel.X) < 0.001f) &&
3503 (Math.Abs(vel.Y) < 0.001f) &&
3504 (Math.Abs(vel.Z) < 0.001f))
3505 {
3506 _velocity = Vector3.Zero;
3507 float t = -m_invTimeStep;
3508 _acceleration = _acceleration * t;
3509 }
3510 else
3511 {
3512 _velocity.X = vel.X;
3513 _velocity.Y = vel.Y;
3514 _velocity.Z = vel.Z;
3515 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3516 }
3517
3518 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3519 (Math.Abs(_acceleration.Y) < 0.01f) &&
3520 (Math.Abs(_acceleration.Z) < 0.01f))
3521 {
3522 _acceleration = Vector3.Zero;
3523 }
3524
3525 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3526 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3527 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3528 )
3529 {
3530 m_rotationalVelocity = Vector3.Zero;
3531 }
3532 else
3533 {
3534 vel = d.BodyGetAngularVel(Body);
3535 m_rotationalVelocity.X = vel.X;
3536 m_rotationalVelocity.Y = vel.Y;
3537 m_rotationalVelocity.Z = vel.Z;
3538 }
3539 }
3540
3541 if (_zeroFlag)
3542 {
3543 if (lastZeroFlag)
3544 {
3545 _velocity = Vector3.Zero;
3546 _acceleration = Vector3.Zero;
3547 m_rotationalVelocity = Vector3.Zero;
3548 }
3549
3550 if (!m_lastUpdateSent)
3551 {
3552 base.RequestPhysicsterseUpdate();
3553 if (lastZeroFlag)
3554 m_lastUpdateSent = true;
3555 }
3556 return;
3557 }
3558
3559 _position.X = lpos.X;
3560 _position.Y = lpos.Y;
3561 _position.Z = lpos.Z;
3562
3563 _orientation.X = ori.X;
3564 _orientation.Y = ori.Y;
3565 _orientation.Z = ori.Z;
3566 _orientation.W = ori.W;
3567 base.RequestPhysicsterseUpdate();
3568 m_lastUpdateSent = false;
3569 }
3570 }
3571 }
3572
3573 internal static bool QuaternionIsFinite(Quaternion q)
3574 {
3575 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3576 return false;
3577 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3578 return false;
3579 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3580 return false;
3581 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3582 return false;
3583 return true;
3584 }
3585
3586 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
3587 {
3588 // assumes object center of mass is zero
3589 float smass = part.mass;
3590 theobj.mass -= smass;
3591
3592 smass *= 1.0f / (theobj.mass); ;
3593
3594 theobj.c.X -= part.c.X * smass;
3595 theobj.c.Y -= part.c.Y * smass;
3596 theobj.c.Z -= part.c.Z * smass;
3597
3598 theobj.I.M00 -= part.I.M00;
3599 theobj.I.M01 -= part.I.M01;
3600 theobj.I.M02 -= part.I.M02;
3601 theobj.I.M10 -= part.I.M10;
3602 theobj.I.M11 -= part.I.M11;
3603 theobj.I.M12 -= part.I.M12;
3604 theobj.I.M20 -= part.I.M20;
3605 theobj.I.M21 -= part.I.M21;
3606 theobj.I.M22 -= part.I.M22;
3607 }
3608
3609 private void donullchange()
3610 {
3611 }
3612
3613 public bool DoAChange(changes what, object arg)
3614 {
3615 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
3616 {
3617 return false;
3618 }
3619
3620 // nasty switch
3621 switch (what)
3622 {
3623 case changes.Add:
3624 changeadd();
3625 break;
3626
3627 case changes.AddPhysRep:
3628 changeAddPhysRep((ODEPhysRepData)arg);
3629 break;
3630
3631 case changes.Remove:
3632 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3633 //When we return true, it destroys all of the prims in the linkset anyway
3634 if (_parent != null)
3635 {
3636 OdePrim parent = (OdePrim)_parent;
3637 parent.ChildRemove(this, false);
3638 }
3639 else
3640 ChildRemove(this, false);
3641
3642 m_vehicle = null;
3643 RemoveGeom();
3644 m_targetSpace = IntPtr.Zero;
3645 UnSubscribeEvents();
3646 return true;
3647
3648 case changes.Link:
3649 OdePrim tmp = (OdePrim)arg;
3650 changeLink(tmp);
3651 break;
3652
3653 case changes.DeLink:
3654 changeLink(null);
3655 break;
3656
3657 case changes.Position:
3658 changePosition((Vector3)arg);
3659 break;
3660
3661 case changes.Orientation:
3662 changeOrientation((Quaternion)arg);
3663 break;
3664
3665 case changes.PosOffset:
3666 donullchange();
3667 break;
3668
3669 case changes.OriOffset:
3670 donullchange();
3671 break;
3672
3673 case changes.Velocity:
3674 changevelocity((Vector3)arg);
3675 break;
3676
3677// case changes.Acceleration:
3678// changeacceleration((Vector3)arg);
3679// break;
3680
3681 case changes.AngVelocity:
3682 changeangvelocity((Vector3)arg);
3683 break;
3684
3685 case changes.Force:
3686 changeForce((Vector3)arg);
3687 break;
3688
3689 case changes.Torque:
3690 changeSetTorque((Vector3)arg);
3691 break;
3692
3693 case changes.AddForce:
3694 changeAddForce((Vector3)arg);
3695 break;
3696
3697 case changes.AddAngForce:
3698 changeAddAngularImpulse((Vector3)arg);
3699 break;
3700
3701 case changes.AngLock:
3702 changeAngularLock((Vector3)arg);
3703 break;
3704
3705 case changes.Size:
3706 changeSize((Vector3)arg);
3707 break;
3708
3709 case changes.Shape:
3710 changeShape((PrimitiveBaseShape)arg);
3711 break;
3712
3713 case changes.PhysRepData:
3714 changePhysRepData((ODEPhysRepData) arg);
3715 break;
3716
3717 case changes.CollidesWater:
3718 changeFloatOnWater((bool)arg);
3719 break;
3720
3721 case changes.VolumeDtc:
3722 changeVolumedetetion((bool)arg);
3723 break;
3724
3725 case changes.Phantom:
3726 changePhantomStatus((bool)arg);
3727 break;
3728
3729 case changes.Physical:
3730 changePhysicsStatus((bool)arg);
3731 break;
3732
3733 case changes.Selected:
3734 changeSelectedStatus((bool)arg);
3735 break;
3736
3737 case changes.disabled:
3738 changeDisable((bool)arg);
3739 break;
3740
3741 case changes.building:
3742 changeBuilding((bool)arg);
3743 break;
3744
3745 case changes.VehicleType:
3746 changeVehicleType((int)arg);
3747 break;
3748
3749 case changes.VehicleFlags:
3750 changeVehicleFlags((strVehicleBoolParam) arg);
3751 break;
3752
3753 case changes.VehicleFloatParam:
3754 changeVehicleFloatParam((strVehicleFloatParam) arg);
3755 break;
3756
3757 case changes.VehicleVectorParam:
3758 changeVehicleVectorParam((strVehicleVectorParam) arg);
3759 break;
3760
3761 case changes.VehicleRotationParam:
3762 changeVehicleRotationParam((strVehicleQuatParam) arg);
3763 break;
3764
3765 case changes.SetVehicle:
3766 changeSetVehicle((VehicleData) arg);
3767 break;
3768
3769 case changes.Buoyancy:
3770 changeBuoyancy((float)arg);
3771 break;
3772
3773 case changes.PIDTarget:
3774 changePIDTarget((Vector3)arg);
3775 break;
3776
3777 case changes.PIDTau:
3778 changePIDTau((float)arg);
3779 break;
3780
3781 case changes.PIDActive:
3782 changePIDActive((bool)arg);
3783 break;
3784
3785 case changes.PIDHoverHeight:
3786 changePIDHoverHeight((float)arg);
3787 break;
3788
3789 case changes.PIDHoverType:
3790 changePIDHoverType((PIDHoverType)arg);
3791 break;
3792
3793 case changes.PIDHoverTau:
3794 changePIDHoverTau((float)arg);
3795 break;
3796
3797 case changes.PIDHoverActive:
3798 changePIDHoverActive((bool)arg);
3799 break;
3800
3801 case changes.Null:
3802 donullchange();
3803 break;
3804
3805
3806
3807 default:
3808 donullchange();
3809 break;
3810 }
3811 return false;
3812 }
3813
3814 public void AddChange(changes what, object arg)
3815 {
3816 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3817 }
3818
3819
3820 private struct strVehicleBoolParam
3821 {
3822 public int param;
3823 public bool value;
3824 }
3825
3826 private struct strVehicleFloatParam
3827 {
3828 public int param;
3829 public float value;
3830 }
3831
3832 private struct strVehicleQuatParam
3833 {
3834 public int param;
3835 public Quaternion value;
3836 }
3837
3838 private struct strVehicleVectorParam
3839 {
3840 public int param;
3841 public Vector3 value;
3842 }
3843 }
3844}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..2923ccf
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,683 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// Processes raycast requests as ODE is in a state to be able to do them.
43 /// This ensures that it's thread safe and there will be no conflicts.
44 /// Requests get returned by a different thread then they were requested by.
45 /// </summary>
46 public class ODERayCastRequestManager
47 {
48 /// <summary>
49 /// Pending ray requests
50 /// </summary>
51 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
52
53 /// <summary>
54 /// Scene that created this object.
55 /// </summary>
56 private OdeScene m_scene;
57
58 IntPtr ray; // the ray. we only need one for our lifetime
59 IntPtr Sphere;
60 IntPtr Box;
61 IntPtr Plane;
62
63 private int CollisionContactGeomsPerTest = 25;
64 private const int DefaultMaxCount = 25;
65 private const int MaxTimePerCallMS = 30;
66
67 /// <summary>
68 /// ODE near callback delegate
69 /// </summary>
70 private d.NearCallback nearCallback;
71 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
72 private List<ContactResult> m_contactResults = new List<ContactResult>();
73 private RayFilterFlags CurrentRayFilter;
74 private int CurrentMaxCount;
75
76 public ODERayCastRequestManager(OdeScene pScene)
77 {
78 m_scene = pScene;
79 nearCallback = near;
80 ray = d.CreateRay(IntPtr.Zero, 1.0f);
81 d.GeomSetCategoryBits(ray, 0);
82 Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
83 d.GeomSetCategoryBits(Box, 0);
84 Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
85 d.GeomSetCategoryBits(Sphere, 0);
86 Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
87 d.GeomSetCategoryBits(Sphere, 0);
88 }
89
90 public void QueueRequest(ODERayRequest req)
91 {
92 if (req.Count == 0)
93 req.Count = DefaultMaxCount;
94
95 m_PendingRequests.Enqueue(req);
96 }
97
98 /// <summary>
99 /// Process all queued raycast requests
100 /// </summary>
101 /// <returns>Time in MS the raycasts took to process.</returns>
102 public int ProcessQueuedRequests()
103 {
104
105 if (m_PendingRequests.Count <= 0)
106 return 0;
107
108 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
109 // oops something got wrong or scene isn't ready still
110 {
111 m_PendingRequests.Clear();
112 return 0;
113 }
114
115 int time = Util.EnvironmentTickCount();
116
117 ODERayRequest req;
118 int closestHit;
119 int backfacecull;
120 CollisionCategories catflags;
121
122 while (m_PendingRequests.Dequeue(out req))
123 {
124 if (req.callbackMethod != null)
125 {
126 IntPtr geom = IntPtr.Zero;
127 if (req.actor != null)
128 {
129 if (m_scene.haveActor(req.actor))
130 {
131 if (req.actor is OdePrim)
132 geom = ((OdePrim)req.actor).prim_geom;
133 else if (req.actor is OdeCharacter)
134 geom = ((OdePrim)req.actor).prim_geom;
135 }
136 if (geom == IntPtr.Zero)
137 {
138 NoContacts(req);
139 continue;
140 }
141 }
142
143
144 CurrentRayFilter = req.filter;
145 CurrentMaxCount = req.Count;
146
147 CollisionContactGeomsPerTest = req.Count & 0xffff;
148
149 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
150 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
151
152 if (req.callbackMethod is ProbeBoxCallback)
153 {
154 if (CollisionContactGeomsPerTest > 80)
155 CollisionContactGeomsPerTest = 80;
156 d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
157 d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
158 d.Quaternion qtmp;
159 qtmp.X = req.orientation.X;
160 qtmp.Y = req.orientation.Y;
161 qtmp.Z = req.orientation.Z;
162 qtmp.W = req.orientation.W;
163 d.GeomSetQuaternion(Box, ref qtmp);
164 }
165 else if (req.callbackMethod is ProbeSphereCallback)
166 {
167 if (CollisionContactGeomsPerTest > 80)
168 CollisionContactGeomsPerTest = 80;
169
170 d.GeomSphereSetRadius(Sphere, req.length);
171 d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
172 }
173 else if (req.callbackMethod is ProbePlaneCallback)
174 {
175 if (CollisionContactGeomsPerTest > 80)
176 CollisionContactGeomsPerTest = 80;
177
178 d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
179 }
180
181 else
182 {
183 if (CollisionContactGeomsPerTest > 25)
184 CollisionContactGeomsPerTest = 25;
185
186 d.GeomRaySetLength(ray, req.length);
187 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
188 d.GeomRaySetParams(ray, 0, backfacecull);
189 d.GeomRaySetClosestHit(ray, closestHit);
190
191 if (req.callbackMethod is RaycastCallback)
192 {
193 // if we only want one get only one per Collision pair saving memory
194 CurrentRayFilter |= RayFilterFlags.ClosestHit;
195 d.GeomRaySetClosestHit(ray, 1);
196 }
197 else
198 d.GeomRaySetClosestHit(ray, closestHit);
199 }
200
201 if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
202 unchecked
203 {
204 CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
205 }
206
207 if (geom == IntPtr.Zero)
208 {
209 // translate ray filter to Collision flags
210 catflags = 0;
211 if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
212 catflags |= CollisionCategories.VolumeDtc;
213 if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
214 catflags |= CollisionCategories.Phantom;
215 if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
216 catflags |= CollisionCategories.Character;
217 if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
218 catflags |= CollisionCategories.Geom;
219 if ((CurrentRayFilter & RayFilterFlags.land) != 0)
220 catflags |= CollisionCategories.Land;
221 if ((CurrentRayFilter & RayFilterFlags.water) != 0)
222 catflags |= CollisionCategories.Water;
223
224 if (catflags != 0)
225 {
226 if (req.callbackMethod is ProbeBoxCallback)
227 {
228 catflags |= CollisionCategories.Space;
229 d.GeomSetCollideBits(Box, (uint)catflags);
230 d.GeomSetCategoryBits(Box, (uint)catflags);
231 doProbe(req, Box);
232 }
233 else if (req.callbackMethod is ProbeSphereCallback)
234 {
235 catflags |= CollisionCategories.Space;
236 d.GeomSetCollideBits(Sphere, (uint)catflags);
237 d.GeomSetCategoryBits(Sphere, (uint)catflags);
238 doProbe(req, Sphere);
239 }
240 else if (req.callbackMethod is ProbePlaneCallback)
241 {
242 catflags |= CollisionCategories.Space;
243 d.GeomSetCollideBits(Plane, (uint)catflags);
244 d.GeomSetCategoryBits(Plane, (uint)catflags);
245 doPlane(req,IntPtr.Zero);
246 }
247 else
248 {
249 d.GeomSetCollideBits(ray, (uint)catflags);
250 doSpaceRay(req);
251 }
252 }
253 }
254 else
255 {
256 // if we select a geom don't use filters
257
258 if (req.callbackMethod is ProbePlaneCallback)
259 {
260 d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
261 doPlane(req,geom);
262 }
263 else
264 {
265 d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
266 doGeomRay(req,geom);
267 }
268 }
269 }
270
271 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
272 break;
273 }
274
275 lock (m_contactResults)
276 m_contactResults.Clear();
277
278 return Util.EnvironmentTickCountSubtract(time);
279 }
280 /// <summary>
281 /// Method that actually initiates the raycast with spaces
282 /// </summary>
283 /// <param name="req"></param>
284 ///
285
286 private void NoContacts(ODERayRequest req)
287 {
288 if (req.callbackMethod is RaycastCallback)
289 {
290 ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero);
291 return;
292 }
293 List<ContactResult> cresult = new List<ContactResult>();
294
295 if (req.callbackMethod is RayCallback)
296 ((RayCallback)req.callbackMethod)(cresult);
297 else if (req.callbackMethod is ProbeBoxCallback)
298 ((ProbeBoxCallback)req.callbackMethod)(cresult);
299 else if (req.callbackMethod is ProbeSphereCallback)
300 ((ProbeSphereCallback)req.callbackMethod)(cresult);
301 }
302
303 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
304// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
305 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
306
307 private void doSpaceRay(ODERayRequest req)
308 {
309 // Collide tests
310 if ((CurrentRayFilter & FilterActiveSpace) != 0)
311 {
312 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
313 d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
314 }
315 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
316 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
317 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
318 {
319 // current ode land to ray collisions is very bad
320 // so for now limit its range badly
321
322 if (req.length > 30.0f)
323 d.GeomRaySetLength(ray, 30.0f);
324
325 d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
326 }
327
328 if (req.callbackMethod is RaycastCallback)
329 {
330 // Define default results
331 bool hitYN = false;
332 uint hitConsumerID = 0;
333 float distance = float.MaxValue;
334 Vector3 closestcontact = Vector3.Zero;
335 Vector3 snormal = Vector3.Zero;
336
337 // Find closest contact and object.
338 lock (m_contactResults)
339 {
340 foreach (ContactResult cResult in m_contactResults)
341 {
342 if(cResult.Depth < distance)
343 {
344 closestcontact = cResult.Pos;
345 hitConsumerID = cResult.ConsumerID;
346 distance = cResult.Depth;
347 snormal = cResult.Normal;
348 }
349 }
350 m_contactResults.Clear();
351 }
352
353 if (distance > 0 && distance < float.MaxValue)
354 hitYN = true;
355 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
356 }
357 else
358 {
359 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
360 lock (m_PendingRequests)
361 {
362 cresult.AddRange(m_contactResults);
363 m_contactResults.Clear();
364 }
365 ((RayCallback)req.callbackMethod)(cresult);
366 }
367 }
368
369 private void doProbe(ODERayRequest req, IntPtr probe)
370 {
371 // Collide tests
372 if ((CurrentRayFilter & FilterActiveSpace) != 0)
373 {
374 d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
375 d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
376 }
377 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
378 d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
379 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
380 d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
381
382 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
383 lock (m_PendingRequests)
384 {
385 cresult.AddRange(m_contactResults);
386 m_contactResults.Clear();
387 }
388 if (req.callbackMethod is ProbeBoxCallback)
389 ((ProbeBoxCallback)req.callbackMethod)(cresult);
390 else if (req.callbackMethod is ProbeSphereCallback)
391 ((ProbeSphereCallback)req.callbackMethod)(cresult);
392 }
393
394 private void doPlane(ODERayRequest req,IntPtr geom)
395 {
396 // Collide tests
397 if (geom == IntPtr.Zero)
398 {
399 if ((CurrentRayFilter & FilterActiveSpace) != 0)
400 {
401 d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
402 d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
403 }
404 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
405 d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
406 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
407 d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
408 }
409 else
410 {
411 d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback);
412 }
413
414 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
415 lock (m_PendingRequests)
416 {
417 cresult.AddRange(m_contactResults);
418 m_contactResults.Clear();
419 }
420
421 ((ProbePlaneCallback)req.callbackMethod)(cresult);
422 }
423
424 /// <summary>
425 /// Method that actually initiates the raycast with a geom
426 /// </summary>
427 /// <param name="req"></param>
428 private void doGeomRay(ODERayRequest req, IntPtr geom)
429 {
430 // Collide test
431 d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
432
433 if (req.callbackMethod is RaycastCallback)
434 {
435 // Define default results
436 bool hitYN = false;
437 uint hitConsumerID = 0;
438 float distance = float.MaxValue;
439 Vector3 closestcontact = Vector3.Zero;
440 Vector3 snormal = Vector3.Zero;
441
442 // Find closest contact and object.
443 lock (m_contactResults)
444 {
445 foreach (ContactResult cResult in m_contactResults)
446 {
447 if(cResult.Depth < distance )
448 {
449 closestcontact = cResult.Pos;
450 hitConsumerID = cResult.ConsumerID;
451 distance = cResult.Depth;
452 snormal = cResult.Normal;
453 }
454 }
455 m_contactResults.Clear();
456 }
457
458 if (distance > 0 && distance < float.MaxValue)
459 hitYN = true;
460
461 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
462 }
463 else
464 {
465 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
466 lock (m_PendingRequests)
467 {
468 cresult.AddRange(m_contactResults);
469 m_contactResults.Clear();
470 }
471 ((RayCallback)req.callbackMethod)(cresult);
472 }
473 }
474
475 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
476 {
477 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
478 if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
479 return false;
480
481 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
482 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
483 return true;
484 }
485
486 // This is the standard Near. g1 is the ray
487 private void near(IntPtr space, IntPtr g1, IntPtr g2)
488 {
489 if (g2 == IntPtr.Zero || g1 == g2)
490 return;
491
492 if (m_contactResults.Count >= CurrentMaxCount)
493 return;
494
495 if (d.GeomIsSpace(g2))
496 {
497 try
498 {
499 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
500 }
501 catch (Exception e)
502 {
503 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
504 }
505 return;
506 }
507
508 int count = 0;
509 try
510 {
511 count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
512 }
513 catch (Exception e)
514 {
515 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
516 return;
517 }
518
519 if (count == 0)
520 return;
521/*
522 uint cat1 = d.GeomGetCategoryBits(g1);
523 uint cat2 = d.GeomGetCategoryBits(g2);
524 uint col1 = d.GeomGetCollideBits(g1);
525 uint col2 = d.GeomGetCollideBits(g2);
526*/
527
528 uint ID = 0;
529 PhysicsActor p2 = null;
530
531 m_scene.actor_name_map.TryGetValue(g2, out p2);
532
533 if (p2 == null)
534 return;
535
536 switch (p2.PhysicsActorType)
537 {
538 case (int)ActorTypes.Prim:
539
540 RayFilterFlags thisFlags;
541
542 if (p2.IsPhysical)
543 thisFlags = RayFilterFlags.physical;
544 else
545 thisFlags = RayFilterFlags.nonphysical;
546
547 if (p2.Phantom)
548 thisFlags |= RayFilterFlags.phantom;
549
550 if (p2.IsVolumeDtc)
551 thisFlags |= RayFilterFlags.volumedtc;
552
553 if ((thisFlags & CurrentRayFilter) == 0)
554 return;
555
556 ID = ((OdePrim)p2).LocalID;
557 break;
558
559 case (int)ActorTypes.Agent:
560
561 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
562 return;
563 else
564 ID = ((OdeCharacter)p2).LocalID;
565 break;
566
567 case (int)ActorTypes.Ground:
568
569 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
570 return;
571 break;
572
573 case (int)ActorTypes.Water:
574
575 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
576 return;
577 break;
578
579 default:
580 break;
581 }
582
583 d.ContactGeom curcontact = new d.ContactGeom();
584
585 // closestHit for now only works for meshs, so must do it for others
586 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
587 {
588 // Loop all contacts, build results.
589 for (int i = 0; i < count; i++)
590 {
591 if (!GetCurContactGeom(i, ref curcontact))
592 break;
593
594 ContactResult collisionresult = new ContactResult();
595 collisionresult.ConsumerID = ID;
596 collisionresult.Pos.X = curcontact.pos.X;
597 collisionresult.Pos.Y = curcontact.pos.Y;
598 collisionresult.Pos.Z = curcontact.pos.Z;
599 collisionresult.Depth = curcontact.depth;
600 collisionresult.Normal.X = curcontact.normal.X;
601 collisionresult.Normal.Y = curcontact.normal.Y;
602 collisionresult.Normal.Z = curcontact.normal.Z;
603 lock (m_contactResults)
604 {
605 m_contactResults.Add(collisionresult);
606 if (m_contactResults.Count >= CurrentMaxCount)
607 return;
608 }
609 }
610 }
611 else
612 {
613 // keep only closest contact
614 ContactResult collisionresult = new ContactResult();
615 collisionresult.ConsumerID = ID;
616 collisionresult.Depth = float.MaxValue;
617
618 for (int i = 0; i < count; i++)
619 {
620 if (!GetCurContactGeom(i, ref curcontact))
621 break;
622
623 if (curcontact.depth < collisionresult.Depth)
624 {
625 collisionresult.Pos.X = curcontact.pos.X;
626 collisionresult.Pos.Y = curcontact.pos.Y;
627 collisionresult.Pos.Z = curcontact.pos.Z;
628 collisionresult.Depth = curcontact.depth;
629 collisionresult.Normal.X = curcontact.normal.X;
630 collisionresult.Normal.Y = curcontact.normal.Y;
631 collisionresult.Normal.Z = curcontact.normal.Z;
632 }
633 }
634
635 if (collisionresult.Depth != float.MaxValue)
636 {
637 lock (m_contactResults)
638 m_contactResults.Add(collisionresult);
639 }
640 }
641 }
642
643 /// <summary>
644 /// Dereference the creator scene so that it can be garbage collected if needed.
645 /// </summary>
646 internal void Dispose()
647 {
648 m_scene = null;
649 if (ray != IntPtr.Zero)
650 {
651 d.GeomDestroy(ray);
652 ray = IntPtr.Zero;
653 }
654 if (Box != IntPtr.Zero)
655 {
656 d.GeomDestroy(Box);
657 Box = IntPtr.Zero;
658 }
659 if (Sphere != IntPtr.Zero)
660 {
661 d.GeomDestroy(Sphere);
662 Sphere = IntPtr.Zero;
663 }
664 if (Plane != IntPtr.Zero)
665 {
666 d.GeomDestroy(Plane);
667 Plane = IntPtr.Zero;
668 }
669 }
670 }
671
672 public struct ODERayRequest
673 {
674 public PhysicsActor actor;
675 public Vector3 Origin;
676 public Vector3 Normal;
677 public int Count;
678 public float length;
679 public object callbackMethod;
680 public RayFilterFlags filter;
681 public Quaternion orientation;
682 }
683}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
new file mode 100644
index 0000000..e9023c3
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -0,0 +1,356 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27// Ubit 2012
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// </summary>
43 public class ODESitAvatar
44 {
45 private OdeScene m_scene;
46 private ODERayCastRequestManager m_raymanager;
47
48 public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
49 {
50 m_scene = pScene;
51 m_raymanager = raymanager;
52 }
53
54 private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
55 private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
56
57 private void RotAroundZ(float x, float y, ref Quaternion ori)
58 {
59 double ang = Math.Atan2(y, x);
60 ang *= 0.5d;
61 float s = (float)Math.Sin(ang);
62 float c = (float)Math.Cos(ang);
63
64 ori.X = 0;
65 ori.Y = 0;
66 ori.Z = s;
67 ori.W = c;
68 }
69
70
71 public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
72 {
73 if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
74 {
75 PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
76 return;
77 }
78
79 IntPtr geom = ((OdePrim)actor).prim_geom;
80
81// Vector3 geopos = d.GeomGetPositionOMV(geom);
82// Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
83
84 Vector3 geopos = actor.Position;
85 Quaternion geomOri = actor.Orientation;
86
87 Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
88
89 Quaternion ori = Quaternion.Identity;
90
91 Vector3 rayDir = geopos + offset - avCameraPosition;
92
93 float raylen = rayDir.Length();
94 if (raylen < 0.001f)
95 {
96 PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
97 return;
98 }
99 float t = 1 / raylen;
100 rayDir.X *= t;
101 rayDir.Y *= t;
102 rayDir.Z *= t;
103
104 raylen += 30f; // focal point may be far
105 List<ContactResult> rayResults;
106
107 rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
108 if (rayResults.Count == 0)
109 {
110/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
111 d.AABB aabb;
112 d.GeomGetAABB(geom, out aabb);
113 offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
114 ori = geomInvOri;
115 offset *= geomInvOri;
116 PhysicsSitResponse(1, actor.LocalID, offset, ori);
117*/
118 PhysicsSitResponse(0, actor.LocalID, offset, ori);
119 return;
120 }
121
122 int status = 1;
123
124 offset = rayResults[0].Pos - geopos;
125
126 d.GeomClassID geoclass = d.GeomGetClass(geom);
127
128 if (geoclass == d.GeomClassID.SphereClass)
129 {
130 float r = d.GeomSphereGetRadius(geom);
131
132 offset.Normalize();
133 offset *= r;
134
135 RotAroundZ(offset.X, offset.Y, ref ori);
136
137 if (r < 0.4f)
138 {
139 offset = new Vector3(0, 0, r);
140 }
141 else
142 {
143 if (offset.Z < 0.4f)
144 {
145 t = offset.Z;
146 float rsq = r * r;
147
148 t = 1.0f / (rsq - t * t);
149 offset.X *= t;
150 offset.Y *= t;
151 offset.Z = 0.4f;
152 t = rsq - 0.16f;
153 offset.X *= t;
154 offset.Y *= t;
155 }
156 else if (r > 0.8f && offset.Z > 0.8f * r)
157 {
158 status = 3;
159 avOffset.X = -avOffset.X;
160 avOffset.Z *= 1.6f;
161 }
162 }
163
164 offset += avOffset * ori;
165
166 ori = geomInvOri * ori;
167 offset *= geomInvOri;
168
169 PhysicsSitResponse(status, actor.LocalID, offset, ori);
170 return;
171 }
172
173 Vector3 norm = rayResults[0].Normal;
174
175 if (norm.Z < -0.4f)
176 {
177 PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
178 return;
179 }
180
181
182 float SitNormX = -rayDir.X;
183 float SitNormY = -rayDir.Y;
184
185 Vector3 pivot = geopos + offset;
186
187 float edgeNormalX = norm.X;
188 float edgeNormalY = norm.Y;
189 float edgeDirX = -rayDir.X;
190 float edgeDirY = -rayDir.Y;
191 Vector3 edgePos = rayResults[0].Pos;
192 float edgeDist = float.MaxValue;
193
194 bool foundEdge = false;
195
196 if (norm.Z < 0.5f)
197 {
198 float rayDist = 4.0f;
199
200 for (int i = 0; i < 6; i++)
201 {
202 pivot.X -= 0.01f * norm.X;
203 pivot.Y -= 0.01f * norm.Y;
204 pivot.Z -= 0.01f * norm.Z;
205
206 rayDir.X = -norm.X * norm.Z;
207 rayDir.Y = -norm.Y * norm.Z;
208 rayDir.Z = 1.0f - norm.Z * norm.Z;
209 rayDir.Normalize();
210
211 rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
212 if (rayResults.Count == 0)
213 break;
214
215 if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
216 {
217 rayDist -= rayResults[0].Depth;
218 if (rayDist < 0f)
219 break;
220
221 pivot = rayResults[0].Pos;
222 norm = rayResults[0].Normal;
223 edgeNormalX = norm.X;
224 edgeNormalY = norm.Y;
225 edgeDirX = -rayDir.X;
226 edgeDirY = -rayDir.Y;
227 }
228 else
229 {
230 foundEdge = true;
231 edgePos = rayResults[0].Pos;
232 break;
233 }
234 }
235
236 if (!foundEdge)
237 {
238 PhysicsSitResponse(0, actor.LocalID, offset, ori);
239 return;
240 }
241 avOffset.X *= 0.5f;
242 }
243
244 else if (norm.Z > 0.866f)
245 {
246 float toCamBaseX = avCameraPosition.X - pivot.X;
247 float toCamBaseY = avCameraPosition.Y - pivot.Y;
248 float toCamX = toCamBaseX;
249 float toCamY = toCamBaseY;
250
251 for (int j = 0; j < 4; j++)
252 {
253 float rayDist = 1.0f;
254 float curEdgeDist = 0.0f;
255
256 for (int i = 0; i < 3; i++)
257 {
258 pivot.Z -= 0.01f;
259 rayDir.X = toCamX;
260 rayDir.Y = toCamY;
261 rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
262 rayDir.Normalize();
263
264 rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
265 if (rayResults.Count == 0)
266 break;
267
268 curEdgeDist += rayResults[0].Depth;
269
270 if (rayResults[0].Normal.Z > 0.5f)
271 {
272 rayDist -= rayResults[0].Depth;
273 if (rayDist < 0f)
274 break;
275
276 pivot = rayResults[0].Pos;
277 norm = rayResults[0].Normal;
278 }
279 else
280 {
281 foundEdge = true;
282 if (curEdgeDist < edgeDist)
283 {
284 edgeDist = curEdgeDist;
285 edgeNormalX = rayResults[0].Normal.X;
286 edgeNormalY = rayResults[0].Normal.Y;
287 edgeDirX = rayDir.X;
288 edgeDirY = rayDir.Y;
289 edgePos = rayResults[0].Pos;
290 }
291 break;
292 }
293 }
294 if (foundEdge && edgeDist < 0.2f)
295 break;
296
297 pivot = geopos + offset;
298
299 switch (j)
300 {
301 case 0:
302 toCamX = -toCamBaseY;
303 toCamY = toCamBaseX;
304 break;
305 case 1:
306 toCamX = toCamBaseY;
307 toCamY = -toCamBaseX;
308 break;
309 case 2:
310 toCamX = -toCamBaseX;
311 toCamY = -toCamBaseY;
312 break;
313 default:
314 break;
315 }
316 }
317
318 if (!foundEdge)
319 {
320 avOffset.X = -avOffset.X;
321 avOffset.Z *= 1.6f;
322
323 RotAroundZ(SitNormX, SitNormY, ref ori);
324
325 offset += avOffset * ori;
326
327 ori = geomInvOri * ori;
328 offset *= geomInvOri;
329
330 PhysicsSitResponse(3, actor.LocalID, offset, ori);
331 return;
332 }
333 avOffset.X *= 0.5f;
334 }
335
336 SitNormX = edgeNormalX;
337 SitNormY = edgeNormalY;
338 if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
339 {
340 SitNormX = -SitNormX;
341 SitNormY = -SitNormY;
342 }
343
344 RotAroundZ(SitNormX, SitNormY, ref ori);
345
346 offset = edgePos + avOffset * ori;
347 offset -= geopos;
348
349 ori = geomInvOri * ori;
350 offset *= geomInvOri;
351
352 PhysicsSitResponse(1, actor.LocalID, offset, ori);
353 return;
354 }
355 }
356} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
new file mode 100644
index 0000000..10d7d50
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,2025 @@
1/*
2 * based on:
3 * Ode.NET - .NET bindings for ODE
4 * Jason Perkins (starkos@industriousone.com)
5 * Licensed under the New BSD
6 * Part of the OpenDynamicsEngine
7Open Dynamics Engine
8Copyright (c) 2001-2007, Russell L. Smith.
9All rights reserved.
10
11Redistribution and use in source and binary forms, with or without
12modification, are permitted provided that the following conditions
13are met:
14
15Redistributions of source code must retain the above copyright notice,
16this list of conditions and the following disclaimer.
17
18Redistributions in binary form must reproduce the above copyright notice,
19this list of conditions and the following disclaimer in the documentation
20and/or other materials provided with the distribution.
21
22Neither the names of ODE's copyright owner nor the names of its
23contributors may be used to endorse or promote products derived from
24this software without specific prior written permission.
25
26THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
30OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
31SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
32TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
33PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
34LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
35NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
36SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
37 *
38 * changes by opensim team;
39 * changes by Aurora team http://www.aurora-sim.org/
40
41 * Revision/fixs by Ubit Umarov
42 */
43
44using System;
45using System.Runtime.InteropServices;
46using System.Security;
47using OMV = OpenMetaverse;
48namespace OdeAPI
49{
50//#if dDOUBLE
51// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
52// at least we save same memory and memory access time, FPU performance on intel usually is similar
53// using dReal = System.Double;
54//#else
55 using dReal = System.Single;
56//#endif
57
58 public static class d
59 {
60 public static dReal Infinity = dReal.MaxValue;
61 public static int NTotalBodies = 0;
62 public static int NTotalGeoms = 0;
63
64 public const uint CONTACTS_UNIMPORTANT = 0x80000000;
65
66 #region Flags and Enumerations
67
68 [Flags]
69 public enum AllocateODEDataFlags : uint
70 {
71 BasicData = 0,
72 CollisionData = 0x00000001,
73 All = ~0u
74 }
75
76 [Flags]
77 public enum IniteODEFlags : uint
78 {
79 dInitFlagManualThreadCleanup = 0x00000001
80 }
81
82 [Flags]
83 public enum ContactFlags : int
84 {
85 Mu2 = 0x001,
86 FDir1 = 0x002,
87 Bounce = 0x004,
88 SoftERP = 0x008,
89 SoftCFM = 0x010,
90 Motion1 = 0x020,
91 Motion2 = 0x040,
92 MotionN = 0x080,
93 Slip1 = 0x100,
94 Slip2 = 0x200,
95 Approx0 = 0x0000,
96 Approx1_1 = 0x1000,
97 Approx1_2 = 0x2000,
98 Approx1 = 0x3000
99 }
100
101 public enum GeomClassID : int
102 {
103 SphereClass,
104 BoxClass,
105 CapsuleClass,
106 CylinderClass,
107 PlaneClass,
108 RayClass,
109 ConvexClass,
110 GeomTransformClass,
111 TriMeshClass,
112 HeightfieldClass,
113 FirstSpaceClass,
114 SimpleSpaceClass = FirstSpaceClass,
115 HashSpaceClass,
116 QuadTreeSpaceClass,
117 LastSpaceClass = QuadTreeSpaceClass,
118 UbitTerrainClass,
119 FirstUserClass,
120 LastUserClass = FirstUserClass + MaxUserClasses - 1,
121 NumClasses,
122 MaxUserClasses = 5
123 }
124
125 public enum JointType : int
126 {
127 None,
128 Ball,
129 Hinge,
130 Slider,
131 Contact,
132 Universal,
133 Hinge2,
134 Fixed,
135 Null,
136 AMotor,
137 LMotor,
138 Plane2D
139 }
140
141 public enum JointParam : int
142 {
143 LoStop,
144 HiStop,
145 Vel,
146 FMax,
147 FudgeFactor,
148 Bounce,
149 CFM,
150 StopERP,
151 StopCFM,
152 SuspensionERP,
153 SuspensionCFM,
154 LoStop2 = 256,
155 HiStop2,
156 Vel2,
157 FMax2,
158 FudgeFactor2,
159 Bounce2,
160 CFM2,
161 StopERP2,
162 StopCFM2,
163 SuspensionERP2,
164 SuspensionCFM2,
165 LoStop3 = 512,
166 HiStop3,
167 Vel3,
168 FMax3,
169 FudgeFactor3,
170 Bounce3,
171 CFM3,
172 StopERP3,
173 StopCFM3,
174 SuspensionERP3,
175 SuspensionCFM3
176 }
177
178 public enum dSweepAndPruneAxis : int
179 {
180 XYZ = ((0)|(1<<2)|(2<<4)),
181 XZY = ((0)|(2<<2)|(1<<4)),
182 YXZ = ((1)|(0<<2)|(2<<4)),
183 YZX = ((1)|(2<<2)|(0<<4)),
184 ZXY = ((2)|(0<<2)|(1<<4)),
185 ZYX = ((2)|(1<<2)|(0<<4))
186 }
187
188 #endregion
189
190 #region Callbacks
191
192 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
193 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
194
195 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
196 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
197
198 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
199 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
200
201 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
202 public delegate ColliderFn GetColliderFnFn(int num);
203
204 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
205 public delegate void GeomDtorFn(IntPtr o);
206
207 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
208 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
209
210 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
211 public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z);
212
213 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
214 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
215
216 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
217 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
218
219 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
220 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
221
222 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
223 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
224
225 #endregion
226
227 #region Structs
228
229 [StructLayout(LayoutKind.Sequential)]
230 public struct AABB
231 {
232 public dReal MinX, MaxX;
233 public dReal MinY, MaxY;
234 public dReal MinZ, MaxZ;
235 }
236
237
238 [StructLayout(LayoutKind.Sequential)]
239 public struct Contact
240 {
241 public SurfaceParameters surface;
242 public ContactGeom geom;
243 public Vector3 fdir1;
244 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
245 }
246
247
248 [StructLayout(LayoutKind.Sequential)]
249 public struct ContactGeom
250 {
251
252 public Vector3 pos;
253 public Vector3 normal;
254 public dReal depth;
255 public IntPtr g1;
256 public IntPtr g2;
257 public int side1;
258 public int side2;
259 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
260 }
261
262 [StructLayout(LayoutKind.Sequential)]
263 public struct GeomClass
264 {
265 public int bytes;
266 public GetColliderFnFn collider;
267 public GetAABBFn aabb;
268 public AABBTestFn aabb_test;
269 public GeomDtorFn dtor;
270 }
271
272
273 [StructLayout(LayoutKind.Sequential)]
274 public struct JointFeedback
275 {
276 public Vector3 f1;
277 public Vector3 t1;
278 public Vector3 f2;
279 public Vector3 t2;
280 }
281
282
283 [StructLayout(LayoutKind.Sequential)]
284 public struct Mass
285 {
286 public dReal mass;
287 public Vector4 c;
288 public Matrix3 I;
289 }
290
291
292 [StructLayout(LayoutKind.Sequential)]
293 public struct Matrix3
294 {
295 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
296 {
297 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
298 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
299 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
300 }
301 public dReal M00, M10, M20;
302 private dReal _m30;
303 public dReal M01, M11, M21;
304 private dReal _m31;
305 public dReal M02, M12, M22;
306 private dReal _m32;
307 }
308
309 [StructLayout(LayoutKind.Sequential)]
310 public struct Matrix4
311 {
312 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
313 dReal m01, dReal m11, dReal m21, dReal m31,
314 dReal m02, dReal m12, dReal m22, dReal m32,
315 dReal m03, dReal m13, dReal m23, dReal m33)
316 {
317 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
318 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
319 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
320 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
321 }
322 public dReal M00, M10, M20, M30;
323 public dReal M01, M11, M21, M31;
324 public dReal M02, M12, M22, M32;
325 public dReal M03, M13, M23, M33;
326 }
327
328 [StructLayout(LayoutKind.Sequential)]
329 public struct Quaternion
330 {
331 public dReal W, X, Y, Z;
332 }
333
334
335 [StructLayout(LayoutKind.Sequential)]
336 public struct SurfaceParameters
337 {
338 public ContactFlags mode;
339 public dReal mu;
340 public dReal mu2;
341 public dReal bounce;
342 public dReal bounce_vel;
343 public dReal soft_erp;
344 public dReal soft_cfm;
345 public dReal motion1;
346 public dReal motion2;
347 public dReal motionN;
348 public dReal slip1;
349 public dReal slip2;
350 }
351
352
353 [StructLayout(LayoutKind.Sequential)]
354 public struct Vector3
355 {
356 public Vector3(dReal x, dReal y, dReal z)
357 {
358 X = x; Y = y; Z = z; _w = 0.0f;
359 }
360 public dReal X, Y, Z;
361 private dReal _w;
362 }
363
364
365 [StructLayout(LayoutKind.Sequential)]
366 public struct Vector4
367 {
368 public Vector4(dReal x, dReal y, dReal z, dReal w)
369 {
370 X = x; Y = y; Z = z; W = w;
371 }
372 public dReal X, Y, Z, W;
373 }
374
375 #endregion
376
377 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
378 public static extern int AllocateODEDataForThread(uint ODEInitFlags);
379
380 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
381 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
382
383 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
384 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
385
386 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
387 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
388
389 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
390 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
391
392 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
393 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
394
395 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
396 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
397
398 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
399 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
400
401 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
402 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
403
404 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
405 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
406
407 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
408 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
409
410 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
411 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
412
413 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
414 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
415
416 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
417 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
418
419 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
420 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
421
422 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
423 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
424
425 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
426 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
427
428 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
429 public static extern IntPtr BodyiCreate(IntPtr world);
430 public static IntPtr BodyCreate(IntPtr world)
431 {
432 NTotalBodies++;
433 return BodyiCreate(world);
434 }
435
436 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
437 public static extern void BodyiDestroy(IntPtr body);
438 public static void BodyDestroy(IntPtr body)
439 {
440 NTotalBodies--;
441 BodyiDestroy(body);
442 }
443
444 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
445 public static extern void BodyDisable(IntPtr body);
446
447 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
448 public static extern void BodyEnable(IntPtr body);
449
450 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
451 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
452
453 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
454 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
455
456 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
457 public static extern void BodyGetAutoDisableDefaults(IntPtr body);
458
459 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
460 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
461
462 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
463 public static extern int BodyGetAutoDisableSteps(IntPtr body);
464
465 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
466 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
467
468 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
469 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
470 public static Vector3 BodyGetAngularVel(IntPtr body)
471 {
472 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
473 }
474
475 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
476 public static extern IntPtr BodyGetData(IntPtr body);
477
478 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
479 public static extern int BodyGetFiniteRotationMode(IntPtr body);
480
481 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
482 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
483
484 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
485 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
486 public static Vector3 BodyGetForce(IntPtr body)
487 {
488 unsafe { return *(BodyGetForceUnsafe(body)); }
489 }
490
491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
492 public static extern bool BodyGetGravityMode(IntPtr body);
493
494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
495 public static extern int BodyGetGyroscopicMode(IntPtr body);
496
497 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
498 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
499
500 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
501 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
502 public static Vector3 BodyGetLinearVel(IntPtr body)
503 {
504 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
505 }
506
507 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
508 public static extern void BodyGetMass(IntPtr body, out Mass mass);
509
510 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
511 public static extern int BodyGetNumJoints(IntPtr body);
512
513 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
514 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
515
516 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
517 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
518 public static Vector3 BodyGetPosition(IntPtr body)
519 {
520 unsafe { return *(BodyGetPositionUnsafe(body)); }
521 }
522
523 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
524 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
525
526 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
527 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
528 public static Quaternion BodyGetQuaternion(IntPtr body)
529 {
530 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
531 }
532
533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
534 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
535
536 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
537 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
538
539 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
540 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
541 public static Matrix3 BodyGetRotation(IntPtr body)
542 {
543 unsafe { return *(BodyGetRotationUnsafe(body)); }
544 }
545
546 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
547 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
548 public static Vector3 BodyGetTorque(IntPtr body)
549 {
550 unsafe { return *(BodyGetTorqueUnsafe(body)); }
551 }
552
553 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
554 public static extern IntPtr BodyGetWorld(IntPtr body);
555
556 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
557 public static extern IntPtr BodyGetFirstGeom(IntPtr body);
558
559 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
560 public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
561
562
563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
564 public static extern bool BodyIsEnabled(IntPtr body);
565
566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
567 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
568
569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
570 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
571
572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
573 public static extern void BodySetAutoDisableDefaults(IntPtr body);
574
575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
576 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
577
578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
579 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
580
581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
582 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
583
584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
585 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
586
587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
588 public static extern void BodySetData(IntPtr body, IntPtr data);
589
590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
591 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
592
593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
594 public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
595
596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
597 public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
598
599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
600 public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
601
602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
603 public static extern dReal BodyGetLinearDamping(IntPtr body);
604
605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
606 public static extern dReal BodyGetAngularDamping(IntPtr body);
607
608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
609 public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
610
611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
612 public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
613
614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
615 public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
616
617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
618 public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
619
620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
621 public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
622
623 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
624 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
625
626 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
627 public static extern void BodySetGravityMode(IntPtr body, bool mode);
628
629 /// <summary>
630 /// Sets the Gyroscopic term status on the body specified.
631 /// </summary>
632 /// <param name="body">Pointer to body</param>
633 /// <param name="enabled">NonZero enabled, Zero disabled</param>
634 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
635 public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
636
637 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
638 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
639
640 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
641 public static extern void BodySetMass(IntPtr body, ref Mass mass);
642
643 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
644 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
645
646 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
647 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
648
649 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
650 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
651
652 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
653 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
654
655 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
656 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
657
658 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
659 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
660
661 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
662 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
663
664 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
665 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
666
667 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
668 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
669 ref Vector3 side1, ref Vector3 p2,
670 ref Matrix3 R2, ref Vector3 side2,
671 ref Vector3 normal, out dReal depth, out int return_code,
672 int maxc, out ContactGeom contact, int skip);
673
674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
675 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
676 ref Vector3 side1, ref Vector3 _p2,
677 ref Matrix3 R2, ref Vector3 side2);
678
679 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
680 public static extern void CleanupODEAllDataForThread();
681
682 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
683 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
684 ref Vector3 b1, ref Vector3 b2,
685 ref Vector3 cp1, ref Vector3 cp2);
686
687 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
688 public static extern void CloseODE();
689
690 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
691 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
692 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
693 public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
694
695 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
696 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
697
698 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
699 public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
700 public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
701 {
702 NTotalGeoms++;
703 return CreateiBox(space, lx, ly, lz);
704 }
705
706 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
707 public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
708 public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
709 {
710 NTotalGeoms++;
711 return CreateiCapsule(space, radius, length);
712 }
713
714 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
715 public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
716 public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
717 {
718 NTotalGeoms++;
719 return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
720 }
721
722 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
723 public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
724 public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
725 {
726 NTotalGeoms++;
727 return CreateiCylinder(space, radius, length);
728 }
729
730 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
731 public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
732 public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
733 {
734 NTotalGeoms++;
735 return CreateiHeightfield(space, data, bPlaceable);
736 }
737
738 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity]
739 public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable);
740 public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable)
741 {
742 NTotalGeoms++;
743 return CreateiUbitTerrain(space, data, bPlaceable);
744 }
745
746
747
748
749
750 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
751 public static extern IntPtr CreateiGeom(int classnum);
752 public static IntPtr CreateGeom(int classnum)
753 {
754 NTotalGeoms++;
755 return CreateiGeom(classnum);
756 }
757
758 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
759 public static extern int CreateGeomClass(ref GeomClass classptr);
760
761 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
762 public static extern IntPtr CreateGeomTransform(IntPtr space);
763
764 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
765 public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
766 public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
767 {
768 NTotalGeoms++;
769 return CreateiPlane(space, a, b, c, d);
770 }
771
772 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
773 public static extern IntPtr CreateiRay(IntPtr space, dReal length);
774 public static IntPtr CreateRay(IntPtr space, dReal length)
775 {
776 NTotalGeoms++;
777 return CreateiRay(space, length);
778 }
779
780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
781 public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
782 public static IntPtr CreateSphere(IntPtr space, dReal radius)
783 {
784 NTotalGeoms++;
785 return CreateiSphere(space, radius);
786 }
787
788 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
789 public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
790 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
791 public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
792 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
793 {
794 NTotalGeoms++;
795 return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
796 }
797 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
798 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
799
800 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
801 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
802
803 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
804 public static extern int FactorCholesky(ref dReal A00, int n);
805
806 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
807 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
808
809 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
810 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
811
812 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
813 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
814
815 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
816 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
817
818 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
819 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
820
821 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
822 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
823
824 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
825 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
826
827 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
828 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
829
830 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
831 public static extern void GeomClearOffset(IntPtr geom);
832
833 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
834 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
835
836 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
837 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
838
839 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
840 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
841
842 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
843 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
844
845 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
846 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
847
848 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
849 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
850
851 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
852 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
853
854 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
855 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
856
857 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
858 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
859
860 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
861 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
862
863 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
864 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
865
866 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
867 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
868
869 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
870 public static extern void GeomiDestroy(IntPtr geom);
871 public static void GeomDestroy(IntPtr geom)
872 {
873 NTotalGeoms--;
874 GeomiDestroy(geom);
875 }
876
877
878 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
879 public static extern void GeomDisable(IntPtr geom);
880
881 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
882 public static extern void GeomEnable(IntPtr geom);
883
884 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
885 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
886
887 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
888 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
889
890 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
891 public static extern IntPtr GeomGetBody(IntPtr geom);
892
893 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
894 public static extern uint GeomGetCategoryBits(IntPtr geom);
895
896 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
897 public static extern IntPtr GeomGetClassData(IntPtr geom);
898
899 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
900 public static extern uint GeomGetCollideBits(IntPtr geom);
901
902 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
903 public static extern GeomClassID GeomGetClass(IntPtr geom);
904
905 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
906 public static extern IntPtr GeomGetData(IntPtr geom);
907
908 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
909 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
910 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
911 {
912 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
913 }
914
915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
916 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
917 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
918 {
919 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
920 }
921
922 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
923 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
924 public static Vector3 GeomGetPosition(IntPtr geom)
925 {
926 unsafe { return *(GeomGetPositionUnsafe(geom)); }
927 }
928 public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom)
929 {
930 Vector3 vtmp = GeomGetPosition(geom);
931 return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z);
932 }
933
934 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
935 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
936 public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom)
937 {
938 Quaternion qtmp;
939 GeomCopyQuaternion(geom, out qtmp);
940 return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W);
941 }
942
943 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
944 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
945
946 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
947 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
948 public static Matrix3 GeomGetRotation(IntPtr geom)
949 {
950 unsafe { return *(GeomGetRotationUnsafe(geom)); }
951 }
952
953 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
954 public static extern IntPtr GeomGetSpace(IntPtr geom);
955
956 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
957 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
958 dReal width, dReal depth, int widthSamples, int depthSamples,
959 dReal scale, dReal offset, dReal thickness, int bWrap);
960
961 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
962 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
963 dReal width, dReal depth, int widthSamples, int depthSamples,
964 dReal scale, dReal offset, dReal thickness, int bWrap);
965
966 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
967 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
968 dReal width, dReal depth, int widthSamples, int depthSamples,
969 dReal scale, dReal offset, dReal thickness, int bWrap);
970
971 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
972 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
973 dReal width, dReal depth, int widthSamples, int depthSamples,
974 dReal scale, dReal offset, dReal thickness, int bWrap);
975
976 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
977 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
978 dReal width, dReal depth, int widthSamples, int depthSamples,
979 dReal scale, dReal offset, dReal thickness, int bWrap);
980
981 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
982 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
983 dReal width, dReal depth, int widthSamples, int depthSamples,
984 dReal scale, dReal offset, dReal thickness, int bWrap);
985
986 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
987 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
988 dReal width, dReal depth, int widthSamples, int depthSamples,
989 dReal scale, dReal offset, dReal thickness, int bWrap);
990
991 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
992 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
993 dReal width, dReal depth, int widthSamples, int depthSamples,
994 dReal scale, dReal offset, dReal thickness, int bWrap);
995
996
997
998 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
999 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
1000 dReal width, dReal depth, int widthSamples, int depthSamples,
1001 dReal scale, dReal offset, dReal thickness, int bWrap);
1002
1003 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1004 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
1005 dReal width, dReal depth, int widthSamples, int depthSamples,
1006 dReal scale, dReal offset, dReal thickness, int bWrap);
1007
1008 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
1009 public static extern IntPtr GeomHeightfieldDataCreate();
1010
1011 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
1012 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
1013
1014 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
1015 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1016
1017 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1018 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
1019
1020 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1021 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
1022
1023
1024 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1025 public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
1026 dReal sampleSize, int widthSamples, int depthSamples,
1027 dReal offset, dReal thickness, int bWrap);
1028
1029 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1030 public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
1031 dReal sampleSize, int widthSamples, int depthSamples,
1032 dReal thickness, int bWrap);
1033
1034 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
1035 public static extern IntPtr GeomUbitTerrainDataCreate();
1036
1037 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
1038 public static extern void GeomUbitTerrainDataDestroy(IntPtr d);
1039
1040 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
1041 public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1042
1043 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1044 public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g);
1045
1046 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1047 public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d);
1048
1049
1050 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
1051 public static extern bool GeomIsEnabled(IntPtr geom);
1052
1053 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
1054 public static extern bool GeomIsOffset(IntPtr geom);
1055
1056 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
1057 public static extern bool GeomIsSpace(IntPtr geom);
1058
1059 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1060 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
1061
1062 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1063 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
1064
1065 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
1066 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1067
1068 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
1069 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
1070
1071 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1072 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
1073
1074 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1075 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
1076
1077 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
1078 public static extern int GeomRayGetClosestHit(IntPtr ray);
1079
1080 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
1081 public static extern dReal GeomRayGetLength(IntPtr ray);
1082
1083 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
1084 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
1085
1086 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
1087 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
1088
1089 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
1090 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
1091
1092 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
1093 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
1094
1095 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
1096 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
1097
1098 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
1099 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
1100
1101 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
1102 public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
1103
1104 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1105 public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
1106
1107 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
1108 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
1109
1110 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
1111 public static extern void GeomSetData(IntPtr geom, IntPtr data);
1112
1113 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
1114 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1115
1116 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1117 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
1118
1119 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1120 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
1121
1122 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1123 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
1124
1125 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1126 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
1127
1128 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
1129 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
1130
1131 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1132 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
1133
1134 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1135 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
1136
1137 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1138 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
1139
1140 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1141 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
1142
1143 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
1144 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1145
1146 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1147 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
1148
1149 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1150 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
1151
1152 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1153 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
1154
1155 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1156 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
1157
1158 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
1159 public static extern dReal GeomSphereGetRadius(IntPtr geom);
1160
1161 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
1162 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1163
1164 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
1165 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
1166
1167 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
1168 public static extern int GeomTransformGetCleanup(IntPtr geom);
1169
1170 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
1171 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
1172
1173 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
1174 public static extern int GeomTransformGetInfo(IntPtr geom);
1175
1176 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
1177 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
1178
1179 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
1180 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
1181
1182 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
1183 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
1184
1185 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1186 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1187 double[] vertices, int vertexStride, int vertexCount,
1188 int[] indices, int indexCount, int triStride);
1189
1190 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1191 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1192 IntPtr vertices, int vertexStride, int vertexCount,
1193 IntPtr indices, int indexCount, int triStride);
1194
1195 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1196 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
1197 double[] vertices, int vertexStride, int vertexCount,
1198 int[] indices, int indexCount, int triStride,
1199 double[] normals);
1200
1201 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1202 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1203 IntPtr vertices, int vertexStride, int vertexCount,
1204 IntPtr indices, int indexCount, int triStride,
1205 IntPtr normals);
1206
1207 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1208 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1209 dReal[] vertices, int vertexStride, int vertexCount,
1210 int[] indices, int indexCount, int triStride);
1211
1212 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1213 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1214 IntPtr vertices, int vertexStride, int vertexCount,
1215 IntPtr indices, int indexCount, int triStride);
1216
1217 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1218 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1219 dReal[] vertices, int vertexStride, int vertexCount,
1220 int[] indices, int indexCount, int triStride,
1221 dReal[] normals);
1222
1223 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1224 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1225 IntPtr vertices, int vertexStride, int vertexCount,
1226 IntPtr indices, int indexCount, int triStride,
1227 IntPtr normals);
1228
1229 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1230 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1231 float[] vertices, int vertexStride, int vertexCount,
1232 int[] indices, int indexCount, int triStride);
1233
1234 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1235 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1236 IntPtr vertices, int vertexStride, int vertexCount,
1237 IntPtr indices, int indexCount, int triStride);
1238
1239 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1240 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1241 float[] vertices, int vertexStride, int vertexCount,
1242 int[] indices, int indexCount, int triStride,
1243 float[] normals);
1244
1245 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1246 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1247 IntPtr vertices, int vertexStride, int vertexCount,
1248 IntPtr indices, int indexCount, int triStride,
1249 IntPtr normals);
1250
1251 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1252 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1253
1254 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1255 public static extern IntPtr GeomTriMeshDataCreate();
1256
1257 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1258 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1259
1260 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1261 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1262
1263 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1264 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1265
1266 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1267 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1268
1269 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1270 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1271
1272 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1273 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1274
1275 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1276 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1277
1278 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1279 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1280
1281 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1282 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1283
1284 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1285 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1286 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1287 {
1288 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1289 }
1290
1291 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1292 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1293
1294 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1295 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1296
1297 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1298 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1299
1300 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1301 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1302
1303 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1304 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1305
1306 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1307 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1308
1309 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1310 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1311
1312 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1313 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1314
1315 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1316 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1317
1318 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1319 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1320
1321 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1322 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1323
1324 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1325 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1326
1327 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
1328 public static extern IntPtr iGetConfiguration();
1329
1330 public static string GetConfiguration()
1331 {
1332 IntPtr ptr = iGetConfiguration();
1333 string s = Marshal.PtrToStringAnsi(ptr);
1334 return s;
1335 }
1336
1337 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1338 public static extern IntPtr HashSpaceCreate(IntPtr space);
1339
1340 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1341 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1342
1343 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1344 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1345
1346 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1347 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1348
1349 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1350 public static extern void InitODE();
1351
1352 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
1353 public static extern int InitODE2(uint ODEInitFlags);
1354
1355 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1356 public static extern int IsPositiveDefinite(ref dReal A, int n);
1357
1358 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1359 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1360
1361 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1362 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1363
1364 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1365 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1366
1367 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1368 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1369
1370 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1371 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1372
1373 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1374 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1375
1376 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1377 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1378
1379 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1380 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1381
1382 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1383 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1384
1385 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1386 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1387
1388 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1389 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1390 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1391 public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
1392
1393 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1394 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1395
1396 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1397 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1398
1399 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1400 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1401
1402 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1403 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1404
1405 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1406 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1407
1408 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1409 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1410
1411 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1412 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1413
1414 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1415 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1416
1417 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1418 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1419
1420 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1421 public static extern void JointDestroy(IntPtr j);
1422
1423 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1424 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1425
1426 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1427 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1428
1429 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1430 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1431
1432 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1433 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1434
1435 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1436 public static extern int JointGetAMotorMode(IntPtr j);
1437
1438 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1439 public static extern int JointGetAMotorNumAxes(IntPtr j);
1440
1441 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1442 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1443
1444 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1445 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1446
1447 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1448 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1449
1450 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1451 public static extern IntPtr JointGetBody(IntPtr j);
1452
1453 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1454 public static extern IntPtr JointGetData(IntPtr j);
1455
1456 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1457 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1458 public static JointFeedback JointGetFeedback(IntPtr j)
1459 {
1460 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1461 }
1462
1463 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1464 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1465
1466 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1467 public static extern dReal JointGetHingeAngle(IntPtr j);
1468
1469 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1470 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1471
1472 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1473 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1474
1475 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1476 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1477
1478 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1479 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1480
1481 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1482 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1483
1484 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1485 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1486
1487 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1488 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1489
1490 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1491 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1492
1493 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1494 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1495
1496 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1497 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1498
1499 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1500 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1501
1502 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1503 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1504
1505 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1506 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1507
1508 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1509 public static extern int JointGetLMotorNumAxes(IntPtr j);
1510
1511 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1512 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1513
1514 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1515 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1516
1517 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1518 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1519
1520 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1521 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1522
1523 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1524 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1525
1526 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1527 public static extern dReal JointGetPRPosition(IntPtr j);
1528
1529 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1530 public static extern dReal JointGetPRPositionRate(IntPtr j);
1531
1532 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1533 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1534
1535 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1536 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1537
1538 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1539 public static extern dReal JointGetSliderPosition(IntPtr j);
1540
1541 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1542 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1543
1544 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1545 public static extern JointType JointGetType(IntPtr j);
1546
1547 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1548 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1549
1550 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1551 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1552
1553 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1554 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1555
1556 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1557 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1558
1559 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1560 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1561
1562 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1563 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1564
1565 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1566 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1567
1568 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1569 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1570
1571 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1572 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1573
1574 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1575 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1576
1577 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1578 public static extern IntPtr JointGroupCreate(int max_size);
1579
1580 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1581 public static extern void JointGroupDestroy(IntPtr group);
1582
1583 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1584 public static extern void JointGroupEmpty(IntPtr group);
1585
1586 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1587 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1588
1589 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1590 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1591
1592 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1593 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1594
1595 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1596 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1597
1598 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1599 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1600
1601 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1602 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1603
1604 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1605 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1606
1607 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1608 public static extern void JointSetData(IntPtr j, IntPtr data);
1609
1610 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1611 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1612
1613 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1614 public static extern void JointSetFixed(IntPtr j);
1615
1616 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1617 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1618
1619 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1620 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1621
1622 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1623 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1624
1625 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1626 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1627
1628 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1629 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1630
1631 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1632 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1633
1634 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1635 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1636
1637 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1638 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1639
1640 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1641 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1642
1643 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1644 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1645
1646 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1647 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1648
1649 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1650 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1651
1652 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1653 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1654
1655 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1656 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1657
1658 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1659 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1660
1661 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1662 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1663
1664 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1665 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1666
1667 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1668 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1669
1670 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1671 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1672
1673 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1674 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1675
1676 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1677 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1678
1679 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1680 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1681
1682 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1683 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1684
1685 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1686 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1687
1688 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1689 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1690
1691 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1692 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1693
1694 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1695 public static extern void MassAdd(ref Mass a, ref Mass b);
1696
1697 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1698 public static extern void MassAdjust(ref Mass m, dReal newmass);
1699
1700 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1701 public static extern bool MassCheck(ref Mass m);
1702
1703 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1704 public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
1705
1706 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1707 public static extern void MassRotate(ref Mass mass, ref dReal M00);
1708
1709 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1710 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1711
1712 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1713 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1714
1715 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1716 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1717
1718 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1719 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1720
1721 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1722 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1723
1724 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1725 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1726
1727 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1728 public static extern void MassSetParameters(out Mass mass, dReal themass,
1729 dReal cgx, dReal cgy, dReal cgz,
1730 dReal i11, dReal i22, dReal i33,
1731 dReal i12, dReal i13, dReal i23);
1732
1733 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1734 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1735
1736 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1737 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1738
1739 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1740 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1741
1742 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1743 public static extern void MassSetZero(out Mass mass);
1744
1745 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1746 public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
1747
1748 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1749 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1750
1751 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1752 private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
1753 public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
1754 {
1755 MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
1756 }
1757
1758 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1759 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1760
1761 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1762 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1763
1764 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1765 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1766
1767 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1768 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1769
1770 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1771 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1772
1773 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1774 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1775
1776 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1777 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1778
1779 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1780 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1781
1782 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1783 public static extern void QSetIdentity(out Quaternion q);
1784
1785 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1786 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1787
1788 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1789 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1790
1791 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1792 public static extern dReal RandReal();
1793
1794 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1795 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1796
1797 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1798 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1799
1800 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1801 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1802
1803 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1804 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1805
1806 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1807 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1808
1809 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1810 public static extern void RSetIdentity(out Matrix3 R);
1811
1812 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1813 public static extern void SetValue(out dReal a, int n);
1814
1815 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1816 public static extern void SetZero(out dReal a, int n);
1817
1818 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1819 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1820
1821 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1822 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1823
1824 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1825 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1826
1827 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1828 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1829
1830 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1831 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1832
1833 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1834 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1835
1836 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
1837 public static extern bool SpaceLockQuery(IntPtr space);
1838
1839 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1840 public static extern void SpaceClean(IntPtr space);
1841
1842 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1843 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1844
1845 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1846 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1847
1848 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1849 public static extern void SpaceDestroy(IntPtr space);
1850
1851 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1852 public static extern bool SpaceGetCleanup(IntPtr space);
1853
1854 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1855 public static extern int SpaceGetNumGeoms(IntPtr space);
1856
1857 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1858 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1859
1860 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
1861 public static extern int SpaceGetSublevel(IntPtr space);
1862
1863 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1864 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1865
1866 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1867 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1868
1869 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1870 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1871
1872 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
1873 public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
1874
1875 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
1876 public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
1877
1878 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1879 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1880
1881 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1882 public static extern IntPtr WorldCreate();
1883
1884 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1885 public static extern void WorldDestroy(IntPtr world);
1886
1887 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1888 public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
1889
1890 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1891 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1892
1893 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1894 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1895
1896 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1897 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1898
1899 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1900 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1901
1902 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1903 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1904
1905 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1906 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1907
1908 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1909 public static extern dReal WorldGetCFM(IntPtr world);
1910
1911 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1912 public static extern dReal WorldGetERP(IntPtr world);
1913
1914 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1915 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1916
1917 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1918 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1919
1920 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1921 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1922
1923 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1924 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1925
1926 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
1927 public static extern dReal WorldGetAngularDamping(IntPtr world);
1928
1929 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1930 public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
1931
1932 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
1933 public static extern dReal WorldGetLinearDamping(IntPtr world);
1934
1935 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1936 public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
1937
1938 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1939 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1940
1941 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1942 public static extern dReal WorldGetQuickStepW(IntPtr world);
1943
1944 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1945 public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
1946
1947 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1948 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1949
1950 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1951 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1952
1953 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1954 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1955
1956 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
1957 public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
1958
1959 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1960 public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
1961
1962 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1963 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1964
1965 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1966 public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
1967
1968 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1969 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1970
1971 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1972 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1973
1974 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1975 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1976
1977 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1978 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1979
1980 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1981 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1982
1983 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1984 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1985
1986 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1987 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1988
1989 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1990 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1991
1992 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
1993 public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
1994
1995 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1996 public static extern void WorldSetERP(IntPtr world, dReal erp);
1997
1998 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1999 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
2000
2001 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
2002 public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
2003
2004 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
2005 public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
2006
2007 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
2008 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
2009
2010 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
2011 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
2012
2013 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
2014 public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
2015
2016 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
2017 public static extern void WorldStep(IntPtr world, dReal stepsize);
2018
2019 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
2020 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
2021
2022 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
2023 public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
2024 }
2025}
diff --git a/OpenSim/Region/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..754bc86
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2859 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28//#define SPAM
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using OdeAPI;
40using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43
44namespace OpenSim.Region.Physics.OdePlugin
45{
46 public enum StatusIndicators : int
47 {
48 Generic = 0,
49 Start = 1,
50 End = 2
51 }
52
53 public struct sCollisionData
54 {
55 public uint ColliderLocalId;
56 public uint CollidedWithLocalId;
57 public int NumberOfCollisions;
58 public int CollisionType;
59 public int StatusIndicator;
60 public int lastframe;
61 }
62
63
64 // colision flags of things others can colide with
65 // rays, sensors, probes removed since can't be colided with
66 // The top space where things are placed provided further selection
67 // ie physical are in active space nonphysical in static
68 // this should be exclusive as possible
69
70 [Flags]
71 public enum CollisionCategories : uint
72 {
73 Disabled = 0,
74 //by 'things' types
75 Space = 0x01,
76 Geom = 0x02, // aka prim/part
77 Character = 0x04,
78 Land = 0x08,
79 Water = 0x010,
80
81 // by state
82 Phantom = 0x01000,
83 VolumeDtc = 0x02000,
84 Selected = 0x04000,
85 NoShape = 0x08000,
86
87
88 All = 0xffffffff
89 }
90
91 /// <summary>
92 /// Material type for a primitive
93 /// </summary>
94 public enum Material : int
95 {
96 /// <summary></summary>
97 Stone = 0,
98 /// <summary></summary>
99 Metal = 1,
100 /// <summary></summary>
101 Glass = 2,
102 /// <summary></summary>
103 Wood = 3,
104 /// <summary></summary>
105 Flesh = 4,
106 /// <summary></summary>
107 Plastic = 5,
108 /// <summary></summary>
109 Rubber = 6,
110
111 light = 7 // compatibility with old viewers
112 }
113
114 public enum changes : int
115 {
116 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
117 Remove,
118 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
119 // or removes from a object if arg is null
120 DeLink,
121 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
122 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
123 PosOffset, // not in use
124 // arg Vector3 new position in local coords. Changes prim position in object
125 OriOffset, // not in use
126 // arg Vector3 new position in local coords. Changes prim position in object
127 Velocity,
128 AngVelocity,
129 Acceleration,
130 Force,
131 Torque,
132 Momentum,
133
134 AddForce,
135 AddAngForce,
136 AngLock,
137
138 Buoyancy,
139
140 PIDTarget,
141 PIDTau,
142 PIDActive,
143
144 PIDHoverHeight,
145 PIDHoverType,
146 PIDHoverTau,
147 PIDHoverActive,
148
149 Size,
150 AvatarSize,
151 Shape,
152 PhysRepData,
153 AddPhysRep,
154
155 CollidesWater,
156 VolumeDtc,
157
158 Physical,
159 Phantom,
160 Selected,
161 disabled,
162 building,
163
164 VehicleType,
165 VehicleFloatParam,
166 VehicleVectorParam,
167 VehicleRotationParam,
168 VehicleFlags,
169 SetVehicle,
170
171 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
172 }
173
174 public struct ODEchangeitem
175 {
176 public PhysicsActor actor;
177 public OdeCharacter character;
178 public changes what;
179 public Object arg;
180 }
181
182
183
184 public class OdeScene : PhysicsScene
185 {
186 private readonly ILog m_log;
187 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
188
189 public bool OdeUbitLib = false;
190// private int threadid = 0;
191 private Random fluidRandomizer = new Random(Environment.TickCount);
192
193 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
194 const float MaxERP = 0.8f;
195 const float minERP = 0.1f;
196 const float comumContactCFM = 0.0001f;
197
198 float frictionMovementMult = 0.8f;
199
200 float TerrainBounce = 0.1f;
201 float TerrainFriction = 0.3f;
202
203 public float AvatarFriction = 0;// 0.9f * 0.5f;
204
205 private const uint m_regionWidth = Constants.RegionSize;
206 private const uint m_regionHeight = Constants.RegionSize;
207
208 public float ODE_STEPSIZE = 0.020f;
209 public float HalfOdeStep = 0.01f;
210 public int odetimestepMS = 20; // rounded
211 private float metersInSpace = 25.6f;
212 private float m_timeDilation = 1.0f;
213
214 private DateTime m_lastframe;
215 private DateTime m_lastMeshExpire;
216
217 public float gravityx = 0f;
218 public float gravityy = 0f;
219 public float gravityz = -9.8f;
220
221 private float waterlevel = 0f;
222 private int framecount = 0;
223
224 private int m_meshExpireCntr;
225
226// private IntPtr WaterGeom = IntPtr.Zero;
227// private IntPtr WaterHeightmapData = IntPtr.Zero;
228// private GCHandle WaterMapHandler = new GCHandle();
229
230 private float avDensity = 3f;
231 private float avMovementDivisorWalk = 1.3f;
232 private float avMovementDivisorRun = 0.8f;
233 private float minimumGroundFlightOffset = 3f;
234 public float maximumMassObject = 10000.01f;
235
236
237 public float geomDefaultDensity = 10.000006836f;
238
239 public int geomContactPointsStartthrottle = 3;
240 public int geomUpdatesPerThrottledUpdate = 15;
241
242 public float bodyPIDD = 35f;
243 public float bodyPIDG = 25;
244
245// public int geomCrossingFailuresBeforeOutofbounds = 6;
246
247 public int bodyFramesAutoDisable = 5;
248
249
250 private d.NearCallback nearCallback;
251
252 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
253 private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
254 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
255 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
256
257 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
258
259 /// <summary>
260 /// A list of actors that should receive collision events.
261 /// </summary>
262 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
263 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
264
265 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
266// public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
267 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
268
269 private float contactsurfacelayer = 0.002f;
270
271 private int contactsPerCollision = 80;
272 internal IntPtr ContactgeomsArray = IntPtr.Zero;
273 private IntPtr GlobalContactsArray = IntPtr.Zero;
274
275 const int maxContactsbeforedeath = 4000;
276 private volatile int m_global_contactcount = 0;
277
278 private IntPtr contactgroup;
279
280 public ContactData[] m_materialContactsData = new ContactData[8];
281
282 private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
283 private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
284 private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
285
286 private int m_physicsiterations = 10;
287 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
288// private PhysicsActor PANull = new NullPhysicsActor();
289 private float step_time = 0.0f;
290
291 public IntPtr world;
292
293
294 // split the spaces acording to contents type
295 // ActiveSpace contains characters and active prims
296 // StaticSpace contains land and other that is mostly static in enviroment
297 // this can contain subspaces, like the grid in staticspace
298 // as now space only contains this 2 top spaces
299
300 public IntPtr TopSpace; // the global space
301 public IntPtr ActiveSpace; // space for active prims
302 public IntPtr CharsSpace; // space for active prims
303 public IntPtr StaticSpace; // space for the static things around
304 public IntPtr GroundSpace; // space for ground
305
306 public IntPtr SharedRay;
307
308 // some speedup variables
309 private int spaceGridMaxX;
310 private int spaceGridMaxY;
311 private float spacesPerMeter;
312
313 // split static geometry collision into a grid as before
314 private IntPtr[,] staticPrimspace;
315 private IntPtr[] staticPrimspaceOffRegion;
316
317 public Object OdeLock;
318 public static Object SimulationLock;
319
320 public IMesher mesher;
321
322 private IConfigSource m_config;
323
324 public bool physics_logging = false;
325 public int physics_logging_interval = 0;
326 public bool physics_logging_append_existing_logfile = false;
327
328 private Vector3 m_worldOffset = Vector3.Zero;
329 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
330 private PhysicsScene m_parentScene = null;
331
332 private ODERayCastRequestManager m_rayCastManager;
333 public ODEMeshWorker m_meshWorker;
334
335/* maybe needed if ode uses tls
336 private void checkThread()
337 {
338
339 int th = Thread.CurrentThread.ManagedThreadId;
340 if(th != threadid)
341 {
342 threadid = th;
343 d.AllocateODEDataForThread(~0U);
344 }
345 }
346 */
347 /// <summary>
348 /// Initiailizes the scene
349 /// Sets many properties that ODE requires to be stable
350 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
351 /// </summary>
352 public OdeScene(string sceneIdentifier)
353 {
354 m_log
355 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
356
357// checkThread();
358 Name = sceneIdentifier;
359
360 OdeLock = new Object();
361 SimulationLock = new Object();
362
363 nearCallback = near;
364
365 m_rayCastManager = new ODERayCastRequestManager(this);
366
367 lock (OdeLock)
368 {
369 // Create the world and the first space
370 try
371 {
372 world = d.WorldCreate();
373 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
374
375 // now the major subspaces
376 ActiveSpace = d.HashSpaceCreate(TopSpace);
377 CharsSpace = d.HashSpaceCreate(TopSpace);
378 StaticSpace = d.HashSpaceCreate(TopSpace);
379 GroundSpace = d.HashSpaceCreate(TopSpace);
380 }
381 catch
382 {
383 // i must RtC#FM
384 // i did!
385 }
386
387 d.HashSpaceSetLevels(TopSpace, -2, 8);
388 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
389 d.HashSpaceSetLevels(CharsSpace, -4, 3);
390 d.HashSpaceSetLevels(StaticSpace, -2, 8);
391 d.HashSpaceSetLevels(GroundSpace, 0, 8);
392
393 // demote to second level
394 d.SpaceSetSublevel(ActiveSpace, 1);
395 d.SpaceSetSublevel(CharsSpace, 1);
396 d.SpaceSetSublevel(StaticSpace, 1);
397 d.SpaceSetSublevel(GroundSpace, 1);
398
399 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
400 CollisionCategories.Geom |
401 CollisionCategories.Character |
402 CollisionCategories.Phantom |
403 CollisionCategories.VolumeDtc
404 ));
405 d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
406 CollisionCategories.Geom |
407 CollisionCategories.Character |
408 CollisionCategories.Phantom |
409 CollisionCategories.VolumeDtc
410 ));
411 d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
412 CollisionCategories.Geom |
413 CollisionCategories.Character |
414 CollisionCategories.Phantom |
415 CollisionCategories.VolumeDtc
416 ));
417 d.GeomSetCollideBits(CharsSpace, 0);
418
419 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
420 CollisionCategories.Geom |
421// CollisionCategories.Land |
422// CollisionCategories.Water |
423 CollisionCategories.Phantom |
424 CollisionCategories.VolumeDtc
425 ));
426 d.GeomSetCollideBits(StaticSpace, 0);
427
428 d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
429 d.GeomSetCollideBits(GroundSpace, 0);
430
431 contactgroup = d.JointGroupCreate(0);
432 //contactgroup
433
434 SharedRay = d.CreateRay(TopSpace, 1.0f);
435
436 d.WorldSetAutoDisableFlag(world, false);
437 }
438 }
439
440 // Initialize the mesh plugin
441// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
442 public override void Initialise(IMesher meshmerizer, IConfigSource config)
443 {
444// checkThread();
445 mesher = meshmerizer;
446 m_config = config;
447
448 string ode_config = d.GetConfiguration();
449 if (ode_config != null && ode_config != "")
450 {
451 m_log.WarnFormat("ODE configuration: {0}", ode_config);
452
453 if (ode_config.Contains("ODE_Ubit"))
454 {
455 OdeUbitLib = true;
456 }
457 }
458
459 /*
460 if (region != null)
461 {
462 WorldExtents.X = region.RegionSizeX;
463 WorldExtents.Y = region.RegionSizeY;
464 }
465 */
466
467 // Defaults
468
469 int contactsPerCollision = 80;
470
471 IConfig physicsconfig = null;
472
473 if (m_config != null)
474 {
475 physicsconfig = m_config.Configs["ODEPhysicsSettings"];
476 if (physicsconfig != null)
477 {
478 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
479 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
480 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
481
482 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
483
484 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
485
486 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
487 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
488
489 avDensity = physicsconfig.GetFloat("av_density", avDensity);
490 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
491 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
492
493 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
494
495 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
496 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
497// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
498
499 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
500 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
501
502 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
503 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
504 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
505
506 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
507 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
508 }
509 }
510
511 m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig);
512
513 HalfOdeStep = ODE_STEPSIZE * 0.5f;
514 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
515
516 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
517 GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
518
519 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
520 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
521
522 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
523 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
524
525 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
526 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
527
528 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
529 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
530
531 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
532 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
533
534 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
535 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
536
537 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
538 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
539
540 m_materialContactsData[(int)Material.light].mu = 0.0f;
541 m_materialContactsData[(int)Material.light].bounce = 0.0f;
542
543 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
544
545 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
546 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
547
548 d.WorldSetLinearDamping(world, 0.002f);
549 d.WorldSetAngularDamping(world, 0.002f);
550 d.WorldSetAngularDampingThreshold(world, 0f);
551 d.WorldSetLinearDampingThreshold(world, 0f);
552 d.WorldSetMaxAngularSpeed(world, 100f);
553
554 d.WorldSetCFM(world,1e-6f); // a bit harder than default
555 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
556 d.WorldSetERP(world, 0.6f); // higher than original
557
558 // Set how many steps we go without running collision testing
559 // This is in addition to the step size.
560 // Essentially Steps * m_physicsiterations
561 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
562
563 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
564
565 spacesPerMeter = 1 / metersInSpace;
566 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
567 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
568
569 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
570
571 // create all spaces now
572 int i, j;
573 IntPtr newspace;
574
575 for (i = 0; i < spaceGridMaxX; i++)
576 for (j = 0; j < spaceGridMaxY; j++)
577 {
578 newspace = d.HashSpaceCreate(StaticSpace);
579 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
580 waitForSpaceUnlock(newspace);
581 d.SpaceSetSublevel(newspace, 2);
582 d.HashSpaceSetLevels(newspace, -2, 8);
583 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
584 CollisionCategories.Geom |
585 CollisionCategories.Land |
586 CollisionCategories.Water |
587 CollisionCategories.Phantom |
588 CollisionCategories.VolumeDtc
589 ));
590 d.GeomSetCollideBits(newspace, 0);
591
592 staticPrimspace[i, j] = newspace;
593 }
594 // let this now be real maximum values
595 spaceGridMaxX--;
596 spaceGridMaxY--;
597
598 // create 4 off world spaces (x<0,x>max,y<0,y>max)
599 staticPrimspaceOffRegion = new IntPtr[4];
600
601 for (i = 0; i < 4; i++)
602 {
603 newspace = d.HashSpaceCreate(StaticSpace);
604 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
605 waitForSpaceUnlock(newspace);
606 d.SpaceSetSublevel(newspace, 2);
607 d.HashSpaceSetLevels(newspace, -2, 8);
608 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
609 CollisionCategories.Geom |
610 CollisionCategories.Land |
611 CollisionCategories.Water |
612 CollisionCategories.Phantom |
613 CollisionCategories.VolumeDtc
614 ));
615 d.GeomSetCollideBits(newspace, 0);
616
617 staticPrimspaceOffRegion[i] = newspace;
618 }
619
620 m_lastframe = DateTime.UtcNow;
621 m_lastMeshExpire = m_lastframe;
622 }
623
624 internal void waitForSpaceUnlock(IntPtr space)
625 {
626 //if (space != IntPtr.Zero)
627 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
628 }
629
630 #region Collision Detection
631
632 // sets a global contact for a joint for contactgeom , and base contact description)
633
634 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
635 {
636 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
637 return IntPtr.Zero;
638
639 float erp = contactGeom.depth;
640 erp *= erpscale;
641 if (erp < minERP)
642 erp = minERP;
643 else if (erp > MaxERP)
644 erp = MaxERP;
645
646 float depth = contactGeom.depth * dscale;
647 if (depth > 0.5f)
648 depth = 0.5f;
649
650 d.Contact newcontact = new d.Contact();
651 newcontact.geom.depth = depth;
652 newcontact.geom.g1 = contactGeom.g1;
653 newcontact.geom.g2 = contactGeom.g2;
654 newcontact.geom.pos = contactGeom.pos;
655 newcontact.geom.normal = contactGeom.normal;
656 newcontact.geom.side1 = contactGeom.side1;
657 newcontact.geom.side2 = contactGeom.side2;
658
659 // this needs bounce also
660 newcontact.surface.mode = comumContactFlags;
661 newcontact.surface.mu = mu;
662 newcontact.surface.bounce = bounce;
663 newcontact.surface.soft_cfm = cfm;
664 newcontact.surface.soft_erp = erp;
665
666 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
667 Marshal.StructureToPtr(newcontact, contact, true);
668 return d.JointCreateContactPtr(world, contactgroup, contact);
669 }
670
671 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
672 {
673 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
674 return false;
675
676 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
677 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
678 return true;
679 }
680
681 /// <summary>
682 /// This is our near callback. A geometry is near a body
683 /// </summary>
684 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
685 /// <param name="g1">a geometry or space</param>
686 /// <param name="g2">another geometry or space</param>
687 ///
688
689 private void near(IntPtr space, IntPtr g1, IntPtr g2)
690 {
691 // no lock here! It's invoked from within Simulate(), which is thread-locked
692
693 if (m_global_contactcount >= maxContactsbeforedeath)
694 return;
695
696 // Test if we're colliding a geom with a space.
697 // If so we have to drill down into the space recursively
698
699 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
700 return;
701
702 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
703 {
704 // We'll be calling near recursivly if one
705 // of them is a space to find all of the
706 // contact points in the space
707 try
708 {
709 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
710 }
711 catch (AccessViolationException)
712 {
713 m_log.Warn("[PHYSICS]: Unable to collide test a space");
714 return;
715 }
716 //here one should check collisions of geoms inside a space
717 // but on each space we only should have geoms that not colide amoung each other
718 // so we don't dig inside spaces
719 return;
720 }
721
722 // get geom bodies to check if we already a joint contact
723 // guess this shouldn't happen now
724 IntPtr b1 = d.GeomGetBody(g1);
725 IntPtr b2 = d.GeomGetBody(g2);
726
727 // d.GeomClassID id = d.GeomGetClass(g1);
728
729 // Figure out how many contact points we have
730 int count = 0;
731 try
732 {
733 // Colliding Geom To Geom
734 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
735
736 if (g1 == g2)
737 return; // Can't collide with yourself
738
739 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
740 return;
741 /*
742 // debug
743 PhysicsActor dp2;
744 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
745 {
746 d.AABB aabb;
747 d.GeomGetAABB(g2, out aabb);
748 float x = aabb.MaxX - aabb.MinX;
749 float y = aabb.MaxY - aabb.MinY;
750 float z = aabb.MaxZ - aabb.MinZ;
751 if (x > 60.0f || y > 60.0f || z > 60.0f)
752 {
753 if (!actor_name_map.TryGetValue(g2, out dp2))
754 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
755 else
756 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
757 dp2.Name, dp2.Size, x, y, z,
758 dp2.Position.ToString(),
759 dp2.Orientation.ToString(),
760 dp2.Orientation.Length());
761 return;
762 }
763 }
764 //
765 */
766
767
768 if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
769 d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
770 {
771 int cflags;
772 unchecked
773 {
774 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
775 }
776 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
777 }
778 else
779 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
780 }
781 catch (SEHException)
782 {
783 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
784 // ode.drelease(world);
785 base.TriggerPhysicsBasedRestart();
786 }
787 catch (Exception e)
788 {
789 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
790 return;
791 }
792
793 // contacts done
794 if (count == 0)
795 return;
796
797 // try get physical actors
798 PhysicsActor p1;
799 PhysicsActor p2;
800
801 if (!actor_name_map.TryGetValue(g1, out p1))
802 {
803 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
804 return;
805 }
806
807 if (!actor_name_map.TryGetValue(g2, out p2))
808 {
809 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
810 return;
811 }
812
813 // update actors collision score
814 if (p1.CollisionScore >= float.MaxValue - count)
815 p1.CollisionScore = 0;
816 p1.CollisionScore += count;
817
818 if (p2.CollisionScore >= float.MaxValue - count)
819 p2.CollisionScore = 0;
820 p2.CollisionScore += count;
821
822 // get first contact
823 d.ContactGeom curContact = new d.ContactGeom();
824
825 if (!GetCurContactGeom(0, ref curContact))
826 return;
827
828 // do volume detection case
829 if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
830 {
831 ContactPoint maxDepthContact = new ContactPoint(
832 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
833 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
834 curContact.depth, false
835 );
836
837 collision_accounting_events(p1, p2, maxDepthContact);
838 return;
839 }
840
841 // big messy collision analises
842
843 float mu = 0;
844 float bounce = 0;
845 float cfm = 0.0001f;
846 float erpscale = 1.0f;
847 float dscale = 1.0f;
848 bool IgnoreNegSides = false;
849
850 ContactData contactdata1 = new ContactData(0, 0, false);
851 ContactData contactdata2 = new ContactData(0, 0, false);
852
853 bool dop1ava = false;
854 bool dop2ava = false;
855 bool ignore = false;
856
857 switch (p1.PhysicsActorType)
858 {
859 case (int)ActorTypes.Agent:
860 {
861 dop1ava = true;
862 switch (p2.PhysicsActorType)
863 {
864 case (int)ActorTypes.Agent:
865 p1.CollidingObj = true;
866 p2.CollidingObj = true;
867 break;
868
869 case (int)ActorTypes.Prim:
870 if (p2.Velocity.LengthSquared() > 0.0f)
871 p2.CollidingObj = true;
872 break;
873
874 default:
875 ignore = true; // avatar to terrain and water ignored
876 break;
877 }
878 break;
879 }
880
881 case (int)ActorTypes.Prim:
882 switch (p2.PhysicsActorType)
883 {
884 case (int)ActorTypes.Agent:
885
886 dop2ava = true;
887
888 if (p1.Velocity.LengthSquared() > 0.0f)
889 p1.CollidingObj = true;
890 break;
891
892 case (int)ActorTypes.Prim:
893 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
894 {
895 p1.CollidingObj = true;
896 p2.CollidingObj = true;
897 }
898 p1.getContactData(ref contactdata1);
899 p2.getContactData(ref contactdata2);
900 bounce = contactdata1.bounce * contactdata2.bounce;
901 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
902
903 cfm = p1.Mass;
904 if (cfm > p2.Mass)
905 cfm = p2.Mass;
906 dscale = 10 / cfm;
907 dscale = (float)Math.Sqrt(dscale);
908 if (dscale > 1.0f)
909 dscale = 1.0f;
910 erpscale = cfm * 0.01f;
911 cfm = 0.0001f / cfm;
912 if (cfm > 0.01f)
913 cfm = 0.01f;
914 else if (cfm < 0.00001f)
915 cfm = 0.00001f;
916
917 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
918 mu *= frictionMovementMult;
919
920 break;
921
922 case (int)ActorTypes.Ground:
923 p1.getContactData(ref contactdata1);
924 bounce = contactdata1.bounce * TerrainBounce;
925 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
926 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
927 mu *= frictionMovementMult;
928 p1.CollidingGround = true;
929
930 cfm = p1.Mass;
931 dscale = 10 / cfm;
932 dscale = (float)Math.Sqrt(dscale);
933 if (dscale > 1.0f)
934 dscale = 1.0f;
935 erpscale = cfm * 0.01f;
936 cfm = 0.0001f / cfm;
937 if (cfm > 0.01f)
938 cfm = 0.01f;
939 else if (cfm < 0.00001f)
940 cfm = 0.00001f;
941
942 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
943 {
944 if (curContact.side1 > 0)
945 IgnoreNegSides = true;
946 }
947 break;
948
949 case (int)ActorTypes.Water:
950 default:
951 ignore = true;
952 break;
953 }
954 break;
955
956 case (int)ActorTypes.Ground:
957 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
958 {
959 p2.CollidingGround = true;
960 p2.getContactData(ref contactdata2);
961 bounce = contactdata2.bounce * TerrainBounce;
962 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
963
964 cfm = p2.Mass;
965 dscale = 10 / cfm;
966 dscale = (float)Math.Sqrt(dscale);
967
968 if (dscale > 1.0f)
969 dscale = 1.0f;
970
971 erpscale = cfm * 0.01f;
972 cfm = 0.0001f / cfm;
973 if (cfm > 0.01f)
974 cfm = 0.01f;
975 else if (cfm < 0.00001f)
976 cfm = 0.00001f;
977
978 if (curContact.side1 > 0) // should be 2 ?
979 IgnoreNegSides = true;
980
981 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
982 mu *= frictionMovementMult;
983 }
984 else
985 ignore = true;
986 break;
987
988 case (int)ActorTypes.Water:
989 default:
990 break;
991 }
992
993 if (ignore)
994 return;
995
996
997 d.ContactGeom maxContact = curContact;
998 // if (IgnoreNegSides && curContact.side1 < 0)
999 // maxContact.depth = float.MinValue;
1000
1001 d.ContactGeom minContact = curContact;
1002 // if (IgnoreNegSides && curContact.side1 < 0)
1003 // minContact.depth = float.MaxValue;
1004
1005 IntPtr Joint;
1006 bool FeetCollision = false;
1007 int ncontacts = 0;
1008
1009
1010 int i = 0;
1011
1012 while (true)
1013 {
1014
1015// if (!(IgnoreNegSides && curContact.side1 < 0))
1016 {
1017 bool noskip = true;
1018 if (dop1ava)
1019 {
1020 if (!(((OdeCharacter)p1).Collide(g1, false, ref curContact, ref FeetCollision)))
1021
1022 noskip = false;
1023 }
1024 else if (dop2ava)
1025 {
1026 if (!(((OdeCharacter)p2).Collide(g2, true, ref curContact, ref FeetCollision)))
1027 noskip = false;
1028 }
1029
1030 if (noskip)
1031 {
1032 m_global_contactcount++;
1033 if (m_global_contactcount >= maxContactsbeforedeath)
1034 break;
1035
1036 ncontacts++;
1037
1038 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1039 if (Joint == IntPtr.Zero)
1040 break;
1041
1042 d.JointAttach(Joint, b1, b2);
1043
1044 if (curContact.depth > maxContact.depth)
1045 maxContact = curContact;
1046
1047 if (curContact.depth < minContact.depth)
1048 minContact = curContact;
1049 }
1050 }
1051
1052 if (++i >= count)
1053 break;
1054
1055 if (!GetCurContactGeom(i, ref curContact))
1056 break;
1057 }
1058
1059 if (ncontacts > 0)
1060 {
1061 ContactPoint maxDepthContact = new ContactPoint(
1062 new Vector3(maxContact.pos.X, maxContact.pos.Y, maxContact.pos.Z),
1063 new Vector3(minContact.normal.X, minContact.normal.Y, minContact.normal.Z),
1064 maxContact.depth, FeetCollision
1065 );
1066 collision_accounting_events(p1, p2, maxDepthContact);
1067 }
1068 }
1069
1070 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1071 {
1072 uint obj2LocalID = 0;
1073
1074 bool p1events = p1.SubscribedEvents();
1075 bool p2events = p2.SubscribedEvents();
1076
1077 if (p1.IsVolumeDtc)
1078 p2events = false;
1079 if (p2.IsVolumeDtc)
1080 p1events = false;
1081
1082 if (!p2events && !p1events)
1083 return;
1084
1085 Vector3 vel = Vector3.Zero;
1086 if (p2 != null && p2.IsPhysical)
1087 vel = p2.Velocity;
1088
1089 if (p1 != null && p1.IsPhysical)
1090 vel -= p1.Velocity;
1091
1092 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1093
1094 switch ((ActorTypes)p1.PhysicsActorType)
1095 {
1096 case ActorTypes.Agent:
1097 case ActorTypes.Prim:
1098 {
1099 switch ((ActorTypes)p2.PhysicsActorType)
1100 {
1101 case ActorTypes.Agent:
1102 case ActorTypes.Prim:
1103 if (p2events)
1104 {
1105 AddCollisionEventReporting(p2);
1106 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
1107 }
1108 obj2LocalID = p2.ParentActor.LocalID;
1109 break;
1110
1111 case ActorTypes.Ground:
1112 case ActorTypes.Unknown:
1113 default:
1114 obj2LocalID = 0;
1115 break;
1116 }
1117 if (p1events)
1118 {
1119 contact.SurfaceNormal = -contact.SurfaceNormal;
1120 AddCollisionEventReporting(p1);
1121 p1.AddCollisionEvent(obj2LocalID, contact);
1122 }
1123 break;
1124 }
1125 case ActorTypes.Ground:
1126 case ActorTypes.Unknown:
1127 default:
1128 {
1129 if (p2events && !p2.IsVolumeDtc)
1130 {
1131 AddCollisionEventReporting(p2);
1132 p2.AddCollisionEvent(0, contact);
1133 }
1134 break;
1135 }
1136 }
1137 }
1138
1139 /// <summary>
1140 /// This is our collision testing routine in ODE
1141 /// </summary>
1142 /// <param name="timeStep"></param>
1143 private void collision_optimized()
1144 {
1145 lock (_characters)
1146 {
1147 try
1148 {
1149 foreach (OdeCharacter chr in _characters)
1150 {
1151 if (chr == null || chr.Body == IntPtr.Zero)
1152 continue;
1153
1154 chr.IsColliding = false;
1155 // chr.CollidingGround = false; not done here
1156 chr.CollidingObj = false;
1157 // do colisions with static space
1158 d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
1159
1160 // chars with chars
1161 d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
1162 // no coll with gnd
1163 }
1164 }
1165 catch (AccessViolationException)
1166 {
1167 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1168 }
1169
1170 }
1171
1172 lock (_activeprims)
1173 {
1174 foreach (OdePrim aprim in _activeprims)
1175 {
1176 aprim.CollisionScore = 0;
1177 aprim.IsColliding = false;
1178 }
1179 }
1180
1181 // collide active prims with static enviroment
1182 lock (_activegroups)
1183 {
1184 try
1185 {
1186 foreach (OdePrim prm in _activegroups)
1187 {
1188 if (!prm.m_outbounds)
1189 {
1190 if (d.BodyIsEnabled(prm.Body))
1191 {
1192 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1193 d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1194 }
1195 }
1196 }
1197 }
1198 catch (AccessViolationException)
1199 {
1200 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1201 }
1202 }
1203 // colide active amoung them
1204 try
1205 {
1206 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1207 }
1208 catch (AccessViolationException)
1209 {
1210 m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space");
1211 }
1212 // and with chars
1213 try
1214 {
1215 d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
1216 }
1217 catch (AccessViolationException)
1218 {
1219 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1220 }
1221 // _perloopContact.Clear();
1222 }
1223
1224 #endregion
1225 /// <summary>
1226 /// Add actor to the list that should receive collision events in the simulate loop.
1227 /// </summary>
1228 /// <param name="obj"></param>
1229 public void AddCollisionEventReporting(PhysicsActor obj)
1230 {
1231 if (!_collisionEventPrim.Contains(obj))
1232 _collisionEventPrim.Add(obj);
1233 }
1234
1235 /// <summary>
1236 /// Remove actor from the list that should receive collision events in the simulate loop.
1237 /// </summary>
1238 /// <param name="obj"></param>
1239 public void RemoveCollisionEventReporting(PhysicsActor obj)
1240 {
1241 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1242 _collisionEventPrimRemove.Add(obj);
1243 }
1244
1245 public override float TimeDilation
1246 {
1247 get { return m_timeDilation; }
1248 }
1249
1250 public override bool SupportsNINJAJoints
1251 {
1252 get { return false; }
1253 }
1254
1255 #region Add/Remove Entities
1256
1257 public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
1258 {
1259 Vector3 pos;
1260 pos.X = position.X;
1261 pos.Y = position.Y;
1262 pos.Z = position.Z;
1263 OdeCharacter newAv = new OdeCharacter(localID,avName, this, pos, size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1264 newAv.Flying = isFlying;
1265 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1266
1267 return newAv;
1268 }
1269
1270 public void AddCharacter(OdeCharacter chr)
1271 {
1272 lock (_characters)
1273 {
1274 if (!_characters.Contains(chr))
1275 {
1276 _characters.Add(chr);
1277 if (chr.bad)
1278 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1279 }
1280 }
1281 }
1282
1283 public void RemoveCharacter(OdeCharacter chr)
1284 {
1285 lock (_characters)
1286 {
1287 if (_characters.Contains(chr))
1288 {
1289 _characters.Remove(chr);
1290 }
1291 }
1292 }
1293
1294 public void BadCharacter(OdeCharacter chr)
1295 {
1296 lock (_badCharacter)
1297 {
1298 if (!_badCharacter.Contains(chr))
1299 _badCharacter.Add(chr);
1300 }
1301 }
1302
1303 public override void RemoveAvatar(PhysicsActor actor)
1304 {
1305 //m_log.Debug("[PHYSICS]:ODELOCK");
1306 ((OdeCharacter) actor).Destroy();
1307 }
1308
1309
1310 public void addActivePrim(OdePrim activatePrim)
1311 {
1312 // adds active prim..
1313 lock (_activeprims)
1314 {
1315 if (!_activeprims.Contains(activatePrim))
1316 _activeprims.Add(activatePrim);
1317 }
1318 }
1319
1320 public void addActiveGroups(OdePrim activatePrim)
1321 {
1322 lock (_activegroups)
1323 {
1324 if (!_activegroups.Contains(activatePrim))
1325 _activegroups.Add(activatePrim);
1326 }
1327 }
1328
1329 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1330 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1331 {
1332 OdePrim newPrim;
1333 lock (OdeLock)
1334 {
1335 newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
1336 lock (_prims)
1337 _prims.Add(newPrim);
1338 }
1339 return newPrim;
1340 }
1341
1342 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1343 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1344 {
1345 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
1346 }
1347
1348
1349 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1350 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1351 {
1352 return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
1353 }
1354
1355 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1356 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1357 {
1358
1359 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1360 }
1361
1362 public void remActivePrim(OdePrim deactivatePrim)
1363 {
1364 lock (_activeprims)
1365 {
1366 _activeprims.Remove(deactivatePrim);
1367 }
1368 }
1369 public void remActiveGroup(OdePrim deactivatePrim)
1370 {
1371 lock (_activegroups)
1372 {
1373 _activegroups.Remove(deactivatePrim);
1374 }
1375 }
1376
1377 public override void RemovePrim(PhysicsActor prim)
1378 {
1379 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1380 // removed in the next physics simulate pass.
1381 if (prim is OdePrim)
1382 {
1383// lock (OdeLock)
1384 {
1385
1386 OdePrim p = (OdePrim)prim;
1387 p.setPrimForRemoval();
1388 }
1389 }
1390 }
1391 /// <summary>
1392 /// This is called from within simulate but outside the locked portion
1393 /// We need to do our own locking here
1394 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1395 /// Simulate() -- justincc).
1396 ///
1397 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1398 ///
1399 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1400 /// that the space was using.
1401 /// </summary>
1402 /// <param name="prim"></param>
1403 public void RemovePrimThreadLocked(OdePrim prim)
1404 {
1405 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1406 lock (prim)
1407 {
1408// RemoveCollisionEventReporting(prim);
1409 lock (_prims)
1410 _prims.Remove(prim);
1411 }
1412
1413 }
1414
1415 public bool havePrim(OdePrim prm)
1416 {
1417 lock (_prims)
1418 return _prims.Contains(prm);
1419 }
1420
1421 public bool haveActor(PhysicsActor actor)
1422 {
1423 if (actor is OdePrim)
1424 {
1425 lock (_prims)
1426 return _prims.Contains((OdePrim)actor);
1427 }
1428 else if (actor is OdeCharacter)
1429 {
1430 lock (_characters)
1431 return _characters.Contains((OdeCharacter)actor);
1432 }
1433 return false;
1434 }
1435
1436 #endregion
1437
1438 #region Space Separation Calculation
1439
1440 /// <summary>
1441 /// Called when a static prim moves or becomes static
1442 /// Places the prim in a space one the static sub-spaces grid
1443 /// </summary>
1444 /// <param name="geom">the pointer to the geom that moved</param>
1445 /// <param name="pos">the position that the geom moved to</param>
1446 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1447 /// <returns>a pointer to the new space it's in</returns>
1448 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1449 {
1450 // moves a prim into another static sub-space or from another space into a static sub-space
1451
1452 // Called ODEPrim so
1453 // it's already in locked space.
1454
1455 if (geom == IntPtr.Zero) // shouldn't happen
1456 return IntPtr.Zero;
1457
1458 // get the static sub-space for current position
1459 IntPtr newspace = calculateSpaceForGeom(pos);
1460
1461 if (newspace == currentspace) // if we are there all done
1462 return newspace;
1463
1464 // else remove it from its current space
1465 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1466 {
1467 if (d.GeomIsSpace(currentspace))
1468 {
1469 waitForSpaceUnlock(currentspace);
1470 d.SpaceRemove(currentspace, geom);
1471
1472 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1473 {
1474 d.SpaceDestroy(currentspace);
1475 }
1476 }
1477 else
1478 {
1479 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1480 " Geom:" + geom);
1481 }
1482 }
1483 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1484 {
1485 currentspace = d.GeomGetSpace(geom);
1486 if (currentspace != IntPtr.Zero)
1487 {
1488 if (d.GeomIsSpace(currentspace))
1489 {
1490 waitForSpaceUnlock(currentspace);
1491 d.SpaceRemove(currentspace, geom);
1492
1493 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1494 {
1495 d.SpaceDestroy(currentspace);
1496 }
1497
1498 }
1499 }
1500 }
1501
1502 // put the geom in the newspace
1503 waitForSpaceUnlock(newspace);
1504 d.SpaceAdd(newspace, geom);
1505
1506 // let caller know this newspace
1507 return newspace;
1508 }
1509
1510 /// <summary>
1511 /// Calculates the space the prim should be in by its position
1512 /// </summary>
1513 /// <param name="pos"></param>
1514 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1515 public IntPtr calculateSpaceForGeom(Vector3 pos)
1516 {
1517 int x, y;
1518
1519 if (pos.X < 0)
1520 return staticPrimspaceOffRegion[0];
1521
1522 if (pos.Y < 0)
1523 return staticPrimspaceOffRegion[2];
1524
1525 x = (int)(pos.X * spacesPerMeter);
1526 if (x > spaceGridMaxX)
1527 return staticPrimspaceOffRegion[1];
1528
1529 y = (int)(pos.Y * spacesPerMeter);
1530 if (y > spaceGridMaxY)
1531 return staticPrimspaceOffRegion[3];
1532
1533 return staticPrimspace[x, y];
1534 }
1535
1536 #endregion
1537
1538
1539 /// <summary>
1540 /// Called to queue a change to a actor
1541 /// to use in place of old taint mechanism so changes do have a time sequence
1542 /// </summary>
1543
1544 public void AddChange(PhysicsActor actor, changes what, Object arg)
1545 {
1546 ODEchangeitem item = new ODEchangeitem();
1547 item.actor = actor;
1548 item.what = what;
1549 item.arg = arg;
1550 ChangesQueue.Enqueue(item);
1551 }
1552
1553 /// <summary>
1554 /// Called after our prim properties are set Scale, position etc.
1555 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1556 /// This assures us that we have no race conditions
1557 /// </summary>
1558 /// <param name="prim"></param>
1559 public override void AddPhysicsActorTaint(PhysicsActor prim)
1560 {
1561 }
1562
1563 // does all pending changes generated during region load process
1564 public override void PrepareSimulation()
1565 {
1566 lock (OdeLock)
1567 {
1568 if (world == IntPtr.Zero)
1569 {
1570 ChangesQueue.Clear();
1571 return;
1572 }
1573
1574 ODEchangeitem item;
1575
1576 int donechanges = 0;
1577 if (ChangesQueue.Count > 0)
1578 {
1579 m_log.InfoFormat("[ODE] start processing pending actor operations");
1580 int tstart = Util.EnvironmentTickCount();
1581
1582 while (ChangesQueue.Dequeue(out item))
1583 {
1584 if (item.actor != null)
1585 {
1586 try
1587 {
1588 if (item.actor is OdeCharacter)
1589 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1590 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1591 RemovePrimThreadLocked((OdePrim)item.actor);
1592 }
1593 catch
1594 {
1595 m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
1596 item.actor.Name, item.what.ToString());
1597 }
1598 }
1599 donechanges++;
1600 }
1601 int time = Util.EnvironmentTickCountSubtract(tstart);
1602 m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time);
1603 }
1604 }
1605 }
1606
1607 /// <summary>
1608 /// This is our main simulate loop
1609 /// It's thread locked by a Mutex in the scene.
1610 /// It holds Collisions, it instructs ODE to step through the physical reactions
1611 /// It moves the objects around in memory
1612 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1613 /// </summary>
1614 /// <param name="timeStep"></param>
1615 /// <returns></returns>
1616 public override float Simulate(float timeStep)
1617 {
1618 DateTime now = DateTime.UtcNow;
1619 TimeSpan timedif = now - m_lastframe;
1620 timeStep = (float)timedif.TotalSeconds;
1621 m_lastframe = now;
1622
1623 // acumulate time so we can reduce error
1624 step_time += timeStep;
1625
1626 if (step_time < HalfOdeStep)
1627 return 0;
1628
1629 if (framecount < 0)
1630 framecount = 0;
1631
1632
1633 framecount++;
1634
1635 int curphysiteractions;
1636
1637 // if in trouble reduce step resolution
1638 if (step_time >= m_SkipFramesAtms)
1639 curphysiteractions = m_physicsiterations / 2;
1640 else
1641 curphysiteractions = m_physicsiterations;
1642
1643// checkThread();
1644 int nodeframes = 0;
1645
1646 lock (SimulationLock)
1647 lock(OdeLock)
1648 {
1649 if (world == IntPtr.Zero)
1650 {
1651 ChangesQueue.Clear();
1652 return 0;
1653 }
1654
1655 ODEchangeitem item;
1656
1657
1658
1659 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1660
1661 int loopstartMS = Util.EnvironmentTickCount();
1662 int looptimeMS = 0;
1663
1664
1665 while (step_time > HalfOdeStep)
1666 {
1667 try
1668 {
1669 // clear pointer/counter to contacts to pass into joints
1670 m_global_contactcount = 0;
1671
1672 if (ChangesQueue.Count > 0)
1673 {
1674 int changestartMS = Util.EnvironmentTickCount();
1675 int ttmp;
1676 while (ChangesQueue.Dequeue(out item))
1677 {
1678 if (item.actor != null)
1679 {
1680 try
1681 {
1682 if (item.actor is OdeCharacter)
1683 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1684 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1685 RemovePrimThreadLocked((OdePrim)item.actor);
1686 }
1687 catch
1688 {
1689 m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
1690 item.actor.Name, item.what.ToString());
1691 }
1692 }
1693 ttmp = Util.EnvironmentTickCountSubtract(changestartMS);
1694 if (ttmp > 20)
1695 break;
1696 }
1697 }
1698
1699 // Move characters
1700 lock (_characters)
1701 {
1702 List<OdeCharacter> defects = new List<OdeCharacter>();
1703 foreach (OdeCharacter actor in _characters)
1704 {
1705 if (actor != null)
1706 actor.Move(defects);
1707 }
1708 if (defects.Count != 0)
1709 {
1710 foreach (OdeCharacter defect in defects)
1711 {
1712 RemoveCharacter(defect);
1713 }
1714 defects.Clear();
1715 }
1716 }
1717
1718 // Move other active objects
1719 lock (_activegroups)
1720 {
1721 foreach (OdePrim aprim in _activegroups)
1722 {
1723 aprim.Move();
1724 }
1725 }
1726
1727 //if ((framecount % m_randomizeWater) == 0)
1728 // randomizeWater(waterlevel);
1729
1730 m_rayCastManager.ProcessQueuedRequests();
1731
1732 collision_optimized();
1733
1734 foreach (PhysicsActor obj in _collisionEventPrim)
1735 {
1736 if (obj == null)
1737 continue;
1738
1739 switch ((ActorTypes)obj.PhysicsActorType)
1740 {
1741 case ActorTypes.Agent:
1742 OdeCharacter cobj = (OdeCharacter)obj;
1743 cobj.AddCollisionFrameTime((int)(odetimestepMS));
1744 cobj.SendCollisions();
1745 break;
1746
1747 case ActorTypes.Prim:
1748 OdePrim pobj = (OdePrim)obj;
1749 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1750 if (!pobj.m_outbounds)
1751 {
1752 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1753 pobj.SendCollisions();
1754 }
1755 break;
1756 }
1757 }
1758
1759 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1760 _collisionEventPrim.Remove(obj);
1761
1762 _collisionEventPrimRemove.Clear();
1763
1764 // do a ode simulation step
1765 d.WorldQuickStep(world, ODE_STEPSIZE);
1766 d.JointGroupEmpty(contactgroup);
1767
1768 // update managed ideia of physical data and do updates to core
1769 /*
1770 lock (_characters)
1771 {
1772 foreach (OdeCharacter actor in _characters)
1773 {
1774 if (actor != null)
1775 {
1776 if (actor.bad)
1777 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1778
1779 actor.UpdatePositionAndVelocity();
1780 }
1781 }
1782 }
1783 */
1784
1785 lock (_activegroups)
1786 {
1787 {
1788 foreach (OdePrim actor in _activegroups)
1789 {
1790 if (actor.IsPhysical)
1791 {
1792 actor.UpdatePositionAndVelocity();
1793 }
1794 }
1795 }
1796 }
1797 }
1798 catch (Exception e)
1799 {
1800 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1801// ode.dunlock(world);
1802 }
1803
1804 step_time -= ODE_STEPSIZE;
1805 nodeframes++;
1806
1807 looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
1808 if (looptimeMS > 100)
1809 break;
1810 }
1811
1812 lock (_badCharacter)
1813 {
1814 if (_badCharacter.Count > 0)
1815 {
1816 foreach (OdeCharacter chr in _badCharacter)
1817 {
1818 RemoveCharacter(chr);
1819 }
1820
1821 _badCharacter.Clear();
1822 }
1823 }
1824
1825 timedif = now - m_lastMeshExpire;
1826
1827 if (timedif.Seconds > 10)
1828 {
1829 mesher.ExpireReleaseMeshs();
1830 m_lastMeshExpire = now;
1831 }
1832
1833// information block running in debug only
1834/*
1835 int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1836 int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1837 int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
1838
1839 int nactivegeoms = 0;
1840 int nactivespaces = 0;
1841
1842 int nstaticgeoms = 0;
1843 int nstaticspaces = 0;
1844 IntPtr sp;
1845
1846 for (int i = 0; i < ntopactivegeoms; i++)
1847 {
1848 sp = d.SpaceGetGeom(ActiveSpace, i);
1849 if (d.GeomIsSpace(sp))
1850 {
1851 nactivespaces++;
1852 nactivegeoms += d.SpaceGetNumGeoms(sp);
1853 }
1854 else
1855 nactivegeoms++;
1856 }
1857
1858 for (int i = 0; i < ntopstaticgeoms; i++)
1859 {
1860 sp = d.SpaceGetGeom(StaticSpace, i);
1861 if (d.GeomIsSpace(sp))
1862 {
1863 nstaticspaces++;
1864 nstaticgeoms += d.SpaceGetNumGeoms(sp);
1865 }
1866 else
1867 nstaticgeoms++;
1868 }
1869
1870 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1871
1872 int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
1873 int nbodies = d.NTotalBodies;
1874 int ngeoms = d.NTotalGeoms;
1875*/
1876 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1877 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1878 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1879 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1880 {
1881 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1882 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1883
1884 if (physics_logging_append_existing_logfile)
1885 {
1886 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1887 TextWriter fwriter = File.AppendText(fname);
1888 fwriter.WriteLine(header);
1889 fwriter.Close();
1890 }
1891
1892 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1893 }
1894
1895 // think time dilation as to do with dinamic step size that we dont' have
1896 // even so tell something to world
1897 if (looptimeMS < 100) // we did the requested loops
1898 m_timeDilation = 1.0f;
1899 else if (step_time > 0)
1900 {
1901 m_timeDilation = timeStep / step_time;
1902 if (m_timeDilation > 1)
1903 m_timeDilation = 1;
1904 if (step_time > m_SkipFramesAtms)
1905 step_time = 0;
1906 m_lastframe = DateTime.UtcNow; // skip also the time lost
1907 }
1908 }
1909
1910// return nodeframes * ODE_STEPSIZE; // return real simulated time
1911 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
1912 }
1913
1914 /// <summary>
1915 public override void GetResults()
1916 {
1917 }
1918
1919 public override bool IsThreaded
1920 {
1921 // for now we won't be multithreaded
1922 get { return (false); }
1923 }
1924
1925 public float GetTerrainHeightAtXY(float x, float y)
1926 {
1927
1928
1929 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1930 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1931
1932
1933 IntPtr heightFieldGeom = IntPtr.Zero;
1934
1935 // get region map
1936 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
1937 return 0f;
1938
1939 if (heightFieldGeom == IntPtr.Zero)
1940 return 0f;
1941
1942 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1943 return 0f;
1944
1945 // TerrainHeightField for ODE as offset 1m
1946 x += 1f - offsetX;
1947 y += 1f - offsetY;
1948
1949 // make position fit into array
1950 if (x < 0)
1951 x = 0;
1952 if (y < 0)
1953 y = 0;
1954
1955 // integer indexs
1956 int ix;
1957 int iy;
1958 // interpolators offset
1959 float dx;
1960 float dy;
1961
1962 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
1963
1964 if (OdeUbitLib)
1965 {
1966 if (x < regsize - 1)
1967 {
1968 ix = (int)x;
1969 dx = x - (float)ix;
1970 }
1971 else // out world use external height
1972 {
1973 ix = regsize - 2;
1974 dx = 0;
1975 }
1976 if (y < regsize - 1)
1977 {
1978 iy = (int)y;
1979 dy = y - (float)iy;
1980 }
1981 else
1982 {
1983 iy = regsize - 2;
1984 dy = 0;
1985 }
1986 }
1987
1988 else
1989 {
1990 // we still have square fixed size regions
1991 // also flip x and y because of how map is done for ODE fliped axis
1992 // so ix,iy,dx and dy are inter exchanged
1993 if (x < regsize - 1)
1994 {
1995 iy = (int)x;
1996 dy = x - (float)iy;
1997 }
1998 else // out world use external height
1999 {
2000 iy = regsize - 2;
2001 dy = 0;
2002 }
2003 if (y < regsize - 1)
2004 {
2005 ix = (int)y;
2006 dx = y - (float)ix;
2007 }
2008 else
2009 {
2010 ix = regsize - 2;
2011 dx = 0;
2012 }
2013 }
2014
2015 float h0;
2016 float h1;
2017 float h2;
2018
2019 iy *= regsize;
2020 iy += ix; // all indexes have iy + ix
2021
2022 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2023 /*
2024 if ((dx + dy) <= 1.0f)
2025 {
2026 h0 = ((float)heights[iy]); // 0,0 vertice
2027 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2028 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2029 }
2030 else
2031 {
2032 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2033 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2034 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2035 }
2036 */
2037 h0 = ((float)heights[iy]); // 0,0 vertice
2038
2039 if ((dy > dx))
2040 {
2041 iy += regsize;
2042 h2 = (float)heights[iy]; // 0,1 vertice
2043 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2044 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2045 }
2046 else
2047 {
2048 iy++;
2049 h2 = (float)heights[iy]; // vertice 1,0
2050 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2051 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2052 }
2053
2054 return h0 + h1 + h2;
2055 }
2056
2057
2058 public override void SetTerrain(float[] heightMap)
2059 {
2060 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2061 {
2062 if (m_parentScene is OdeScene)
2063 {
2064 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2065 }
2066 }
2067 else
2068 {
2069 SetTerrain(heightMap, m_worldOffset);
2070 }
2071 }
2072
2073 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2074 {
2075 SetTerrain(heightMap, pOffset);
2076 }
2077
2078 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2079 {
2080 if (OdeUbitLib)
2081 UbitSetTerrain(heightMap, pOffset);
2082 else
2083 OriSetTerrain(heightMap, pOffset);
2084 }
2085
2086 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2087 {
2088 // assumes 1m size grid and constante size square regions
2089 // needs to know about sims around in future
2090
2091 float[] _heightmap;
2092
2093 uint heightmapWidth = Constants.RegionSize + 2;
2094 uint heightmapHeight = Constants.RegionSize + 2;
2095
2096 uint heightmapWidthSamples = heightmapWidth + 1;
2097 uint heightmapHeightSamples = heightmapHeight + 1;
2098
2099 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2100
2101 const float scale = 1.0f;
2102 const float offset = 0.0f;
2103 const float thickness = 10f;
2104 const int wrap = 0;
2105
2106 uint regionsize = Constants.RegionSize;
2107
2108 float hfmin = float.MaxValue;
2109 float hfmax = float.MinValue;
2110 float val;
2111 uint xx;
2112 uint yy;
2113
2114 uint maxXXYY = regionsize - 1;
2115 // flipping map adding one margin all around so things don't fall in edges
2116
2117 uint xt = 0;
2118 xx = 0;
2119
2120 for (uint x = 0; x < heightmapWidthSamples; x++)
2121 {
2122 if (x > 1 && xx < maxXXYY)
2123 xx++;
2124 yy = 0;
2125 for (uint y = 0; y < heightmapHeightSamples; y++)
2126 {
2127 if (y > 1 && y < maxXXYY)
2128 yy += regionsize;
2129
2130 val = heightMap[yy + xx];
2131 if (val < 0.0f)
2132 val = 0.0f; // no neg terrain as in chode
2133 _heightmap[xt + y] = val;
2134
2135 if (hfmin > val)
2136 hfmin = val;
2137 if (hfmax < val)
2138 hfmax = val;
2139 }
2140 xt += heightmapHeightSamples;
2141 }
2142 lock (OdeLock)
2143 {
2144 IntPtr GroundGeom = IntPtr.Zero;
2145 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2146 {
2147 RegionTerrain.Remove(pOffset);
2148 if (GroundGeom != IntPtr.Zero)
2149 {
2150 actor_name_map.Remove(GroundGeom);
2151 d.GeomDestroy(GroundGeom);
2152
2153 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2154 {
2155 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2156 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2157 TerrainHeightFieldHeights.Remove(GroundGeom);
2158 }
2159 }
2160 }
2161 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2162
2163 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2164
2165 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2166 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2167 offset, thickness, wrap);
2168
2169 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2170
2171 GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
2172
2173 if (GroundGeom != IntPtr.Zero)
2174 {
2175 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2176 d.GeomSetCollideBits(GroundGeom, 0);
2177
2178 PhysicsActor pa = new NullPhysicsActor();
2179 pa.Name = "Terrain";
2180 pa.PhysicsActorType = (int)ActorTypes.Ground;
2181 actor_name_map[GroundGeom] = pa;
2182
2183// geom_name_map[GroundGeom] = "Terrain";
2184
2185 d.Matrix3 R = new d.Matrix3();
2186
2187 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2188 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2189
2190
2191 q1 = q1 * q2;
2192
2193 Vector3 v3;
2194 float angle;
2195 q1.GetAxisAngle(out v3, out angle);
2196
2197 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2198 d.GeomSetRotation(GroundGeom, ref R);
2199 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2200 RegionTerrain.Add(pOffset, GroundGeom);
2201 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2202 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2203 }
2204 }
2205 }
2206
2207 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2208 {
2209 // assumes 1m size grid and constante size square regions
2210 // needs to know about sims around in future
2211
2212 float[] _heightmap;
2213
2214 uint heightmapWidth = Constants.RegionSize + 2;
2215 uint heightmapHeight = Constants.RegionSize + 2;
2216
2217 uint heightmapWidthSamples = heightmapWidth + 1;
2218 uint heightmapHeightSamples = heightmapHeight + 1;
2219
2220 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2221
2222
2223 uint regionsize = Constants.RegionSize;
2224
2225 float hfmin = float.MaxValue;
2226// float hfmax = float.MinValue;
2227 float val;
2228
2229
2230 uint maxXXYY = regionsize - 1;
2231 // adding one margin all around so things don't fall in edges
2232
2233 uint xx;
2234 uint yy = 0;
2235 uint yt = 0;
2236
2237 for (uint y = 0; y < heightmapHeightSamples; y++)
2238 {
2239 if (y > 1 && y < maxXXYY)
2240 yy += regionsize;
2241 xx = 0;
2242 for (uint x = 0; x < heightmapWidthSamples; x++)
2243 {
2244 if (x > 1 && x < maxXXYY)
2245 xx++;
2246
2247 val = heightMap[yy + xx];
2248 if (val < 0.0f)
2249 val = 0.0f; // no neg terrain as in chode
2250 _heightmap[yt + x] = val;
2251
2252 if (hfmin > val)
2253 hfmin = val;
2254// if (hfmax < val)
2255// hfmax = val;
2256 }
2257 yt += heightmapWidthSamples;
2258 }
2259 lock (OdeLock)
2260 {
2261 IntPtr GroundGeom = IntPtr.Zero;
2262 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2263 {
2264 RegionTerrain.Remove(pOffset);
2265 if (GroundGeom != IntPtr.Zero)
2266 {
2267 actor_name_map.Remove(GroundGeom);
2268 d.GeomDestroy(GroundGeom);
2269
2270 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2271 {
2272 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2273 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2274 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2275 TerrainHeightFieldHeights.Remove(GroundGeom);
2276 }
2277 }
2278 }
2279 IntPtr HeightmapData = d.GeomUbitTerrainDataCreate();
2280
2281 const int wrap = 0;
2282 float thickness = hfmin;
2283 if (thickness < 0)
2284 thickness = 1;
2285
2286 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2287
2288 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2289 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2290 thickness, wrap);
2291
2292// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2293 GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1);
2294 if (GroundGeom != IntPtr.Zero)
2295 {
2296 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2297 d.GeomSetCollideBits(GroundGeom, 0);
2298
2299
2300 PhysicsActor pa = new NullPhysicsActor();
2301 pa.Name = "Terrain";
2302 pa.PhysicsActorType = (int)ActorTypes.Ground;
2303 actor_name_map[GroundGeom] = pa;
2304
2305// geom_name_map[GroundGeom] = "Terrain";
2306
2307 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2308 RegionTerrain.Add(pOffset, GroundGeom);
2309 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2310 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2311 }
2312 }
2313 }
2314
2315
2316 public override void DeleteTerrain()
2317 {
2318 }
2319
2320 public float GetWaterLevel()
2321 {
2322 return waterlevel;
2323 }
2324
2325 public override bool SupportsCombining()
2326 {
2327 return true;
2328 }
2329/*
2330 public override void UnCombine(PhysicsScene pScene)
2331 {
2332 IntPtr localGround = IntPtr.Zero;
2333// float[] localHeightfield;
2334 bool proceed = false;
2335 List<IntPtr> geomDestroyList = new List<IntPtr>();
2336
2337 lock (OdeLock)
2338 {
2339 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2340 {
2341 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2342 {
2343 if (geom == localGround)
2344 {
2345// localHeightfield = TerrainHeightFieldHeights[geom];
2346 proceed = true;
2347 }
2348 else
2349 {
2350 geomDestroyList.Add(geom);
2351 }
2352 }
2353
2354 if (proceed)
2355 {
2356 m_worldOffset = Vector3.Zero;
2357 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2358 m_parentScene = null;
2359
2360 foreach (IntPtr g in geomDestroyList)
2361 {
2362 // removingHeightField needs to be done or the garbage collector will
2363 // collect the terrain data before we tell ODE to destroy it causing
2364 // memory corruption
2365 if (TerrainHeightFieldHeights.ContainsKey(g))
2366 {
2367// float[] removingHeightField = TerrainHeightFieldHeights[g];
2368 TerrainHeightFieldHeights.Remove(g);
2369
2370 if (RegionTerrain.ContainsKey(g))
2371 {
2372 RegionTerrain.Remove(g);
2373 }
2374
2375 d.GeomDestroy(g);
2376 //removingHeightField = new float[0];
2377 }
2378 }
2379
2380 }
2381 else
2382 {
2383 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2384 }
2385 }
2386 }
2387 }
2388*/
2389 public override void SetWaterLevel(float baseheight)
2390 {
2391 waterlevel = baseheight;
2392// randomizeWater(waterlevel);
2393 }
2394/*
2395 public void randomizeWater(float baseheight)
2396 {
2397 const uint heightmapWidth = Constants.RegionSize + 2;
2398 const uint heightmapHeight = Constants.RegionSize + 2;
2399 const uint heightmapWidthSamples = heightmapWidth + 1;
2400 const uint heightmapHeightSamples = heightmapHeight + 1;
2401
2402 const float scale = 1.0f;
2403 const float offset = 0.0f;
2404 const int wrap = 0;
2405
2406 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
2407
2408 float maxheigh = float.MinValue;
2409 float minheigh = float.MaxValue;
2410 float val;
2411 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
2412 {
2413
2414 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
2415 _watermap[i] = val;
2416 if (maxheigh < val)
2417 maxheigh = val;
2418 if (minheigh > val)
2419 minheigh = val;
2420 }
2421
2422 float thickness = minheigh;
2423
2424 lock (OdeLock)
2425 {
2426 if (WaterGeom != IntPtr.Zero)
2427 {
2428 actor_name_map.Remove(WaterGeom);
2429 d.GeomDestroy(WaterGeom);
2430 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2431 WaterGeom = IntPtr.Zero;
2432 WaterHeightmapData = IntPtr.Zero;
2433 if(WaterMapHandler.IsAllocated)
2434 WaterMapHandler.Free();
2435 }
2436
2437 WaterHeightmapData = d.GeomHeightfieldDataCreate();
2438
2439 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2440
2441 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
2442 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2443 offset, thickness, wrap);
2444 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
2445 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
2446 if (WaterGeom != IntPtr.Zero)
2447 {
2448 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
2449 d.GeomSetCollideBits(WaterGeom, 0);
2450
2451
2452 PhysicsActor pa = new NullPhysicsActor();
2453 pa.Name = "Water";
2454 pa.PhysicsActorType = (int)ActorTypes.Water;
2455
2456 actor_name_map[WaterGeom] = pa;
2457// geom_name_map[WaterGeom] = "Water";
2458
2459 d.Matrix3 R = new d.Matrix3();
2460
2461 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2462 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2463
2464 q1 = q1 * q2;
2465 Vector3 v3;
2466 float angle;
2467 q1.GetAxisAngle(out v3, out angle);
2468
2469 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2470 d.GeomSetRotation(WaterGeom, ref R);
2471 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
2472 }
2473 }
2474 }
2475*/
2476 public override void Dispose()
2477 {
2478 if (m_meshWorker != null)
2479 m_meshWorker.Stop();
2480
2481 lock (OdeLock)
2482 {
2483 m_rayCastManager.Dispose();
2484 m_rayCastManager = null;
2485
2486 lock (_prims)
2487 {
2488 ChangesQueue.Clear();
2489 foreach (OdePrim prm in _prims)
2490 {
2491 prm.DoAChange(changes.Remove, null);
2492 _collisionEventPrim.Remove(prm);
2493 }
2494 _prims.Clear();
2495 }
2496
2497 OdeCharacter[] chtorem;
2498 lock (_characters)
2499 {
2500 chtorem = new OdeCharacter[_characters.Count];
2501 _characters.CopyTo(chtorem);
2502 }
2503
2504 ChangesQueue.Clear();
2505 foreach (OdeCharacter ch in chtorem)
2506 ch.DoAChange(changes.Remove, null);
2507
2508
2509 foreach (IntPtr GroundGeom in RegionTerrain.Values)
2510 {
2511 if (GroundGeom != IntPtr.Zero)
2512 d.GeomDestroy(GroundGeom);
2513 }
2514
2515
2516 RegionTerrain.Clear();
2517
2518 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2519 {
2520 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2521 {
2522 if (gch.IsAllocated)
2523 gch.Free();
2524 }
2525 }
2526
2527 TerrainHeightFieldHeightsHandlers.Clear();
2528 TerrainHeightFieldHeights.Clear();
2529/*
2530 if (WaterGeom != IntPtr.Zero)
2531 {
2532 d.GeomDestroy(WaterGeom);
2533 WaterGeom = IntPtr.Zero;
2534 if (WaterHeightmapData != IntPtr.Zero)
2535 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2536 WaterHeightmapData = IntPtr.Zero;
2537
2538 if (WaterMapHandler.IsAllocated)
2539 WaterMapHandler.Free();
2540 }
2541*/
2542 if (ContactgeomsArray != IntPtr.Zero)
2543 Marshal.FreeHGlobal(ContactgeomsArray);
2544 if (GlobalContactsArray != IntPtr.Zero)
2545 Marshal.FreeHGlobal(GlobalContactsArray);
2546
2547
2548 d.WorldDestroy(world);
2549 world = IntPtr.Zero;
2550 //d.CloseODE();
2551 }
2552 }
2553
2554 public override Dictionary<uint, float> GetTopColliders()
2555 {
2556 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2557 int cnt = 0;
2558 lock (_prims)
2559 {
2560 foreach (OdePrim prm in _prims)
2561 {
2562 if (prm.CollisionScore > 0)
2563 {
2564 returncolliders.Add(prm.LocalID, prm.CollisionScore);
2565 cnt++;
2566 prm.CollisionScore = 0f;
2567 if (cnt > 25)
2568 {
2569 break;
2570 }
2571 }
2572 }
2573 }
2574 return returncolliders;
2575 }
2576
2577 public override bool SupportsRayCast()
2578 {
2579 return true;
2580 }
2581
2582 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2583 {
2584 if (retMethod != null)
2585 {
2586 ODERayRequest req = new ODERayRequest();
2587 req.actor = null;
2588 req.callbackMethod = retMethod;
2589 req.length = length;
2590 req.Normal = direction;
2591 req.Origin = position;
2592 req.Count = 0;
2593 req.filter = RayFilterFlags.AllPrims;
2594
2595 m_rayCastManager.QueueRequest(req);
2596 }
2597 }
2598
2599 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2600 {
2601 if (retMethod != null)
2602 {
2603 ODERayRequest req = new ODERayRequest();
2604 req.actor = null;
2605 req.callbackMethod = retMethod;
2606 req.length = length;
2607 req.Normal = direction;
2608 req.Origin = position;
2609 req.Count = Count;
2610 req.filter = RayFilterFlags.AllPrims;
2611
2612 m_rayCastManager.QueueRequest(req);
2613 }
2614 }
2615
2616
2617 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2618 {
2619 List<ContactResult> ourresults = new List<ContactResult>();
2620 object SyncObject = new object();
2621
2622 RayCallback retMethod = delegate(List<ContactResult> results)
2623 {
2624 lock (SyncObject)
2625 {
2626 ourresults = results;
2627 Monitor.PulseAll(SyncObject);
2628 }
2629 };
2630
2631 ODERayRequest req = new ODERayRequest();
2632 req.actor = null;
2633 req.callbackMethod = retMethod;
2634 req.length = length;
2635 req.Normal = direction;
2636 req.Origin = position;
2637 req.Count = Count;
2638 req.filter = RayFilterFlags.AllPrims;
2639
2640 lock (SyncObject)
2641 {
2642 m_rayCastManager.QueueRequest(req);
2643 if (!Monitor.Wait(SyncObject, 500))
2644 return null;
2645 else
2646 return ourresults;
2647 }
2648 }
2649
2650 public override bool SupportsRaycastWorldFiltered()
2651 {
2652 return true;
2653 }
2654
2655 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2656 {
2657 object SyncObject = new object();
2658 List<ContactResult> ourresults = new List<ContactResult>();
2659
2660 RayCallback retMethod = delegate(List<ContactResult> results)
2661 {
2662 lock (SyncObject)
2663 {
2664 ourresults = results;
2665 Monitor.PulseAll(SyncObject);
2666 }
2667 };
2668
2669 ODERayRequest req = new ODERayRequest();
2670 req.actor = null;
2671 req.callbackMethod = retMethod;
2672 req.length = length;
2673 req.Normal = direction;
2674 req.Origin = position;
2675 req.Count = Count;
2676 req.filter = filter;
2677
2678 lock (SyncObject)
2679 {
2680 m_rayCastManager.QueueRequest(req);
2681 if (!Monitor.Wait(SyncObject, 500))
2682 return null;
2683 else
2684 return ourresults;
2685 }
2686 }
2687
2688 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
2689 {
2690 if (actor == null)
2691 return new List<ContactResult>();
2692
2693 IntPtr geom;
2694 if (actor is OdePrim)
2695 geom = ((OdePrim)actor).prim_geom;
2696 else if (actor is OdeCharacter)
2697 geom = ((OdePrim)actor).prim_geom;
2698 else
2699 return new List<ContactResult>();
2700
2701 if (geom == IntPtr.Zero)
2702 return new List<ContactResult>();
2703
2704 List<ContactResult> ourResults = null;
2705 object SyncObject = new object();
2706
2707 RayCallback retMethod = delegate(List<ContactResult> results)
2708 {
2709 lock (SyncObject)
2710 {
2711 ourResults = results;
2712 Monitor.PulseAll(SyncObject);
2713 }
2714 };
2715
2716 ODERayRequest req = new ODERayRequest();
2717 req.actor = actor;
2718 req.callbackMethod = retMethod;
2719 req.length = length;
2720 req.Normal = direction;
2721 req.Origin = position;
2722 req.Count = Count;
2723 req.filter = flags;
2724
2725 lock (SyncObject)
2726 {
2727 m_rayCastManager.QueueRequest(req);
2728 if (!Monitor.Wait(SyncObject, 500))
2729 return new List<ContactResult>();
2730 }
2731
2732 if (ourResults == null)
2733 return new List<ContactResult>();
2734 return ourResults;
2735 }
2736
2737 public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
2738 {
2739 List<ContactResult> ourResults = null;
2740 object SyncObject = new object();
2741
2742 ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
2743 {
2744 lock (SyncObject)
2745 {
2746 ourResults = results;
2747 Monitor.PulseAll(SyncObject);
2748 }
2749 };
2750
2751 ODERayRequest req = new ODERayRequest();
2752 req.actor = null;
2753 req.callbackMethod = retMethod;
2754 req.Normal = size;
2755 req.Origin = position;
2756 req.orientation = orientation;
2757 req.Count = Count;
2758 req.filter = flags;
2759
2760 lock (SyncObject)
2761 {
2762 m_rayCastManager.QueueRequest(req);
2763 if (!Monitor.Wait(SyncObject, 500))
2764 return new List<ContactResult>();
2765 }
2766
2767 if (ourResults == null)
2768 return new List<ContactResult>();
2769 return ourResults;
2770 }
2771
2772 public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
2773 {
2774 List<ContactResult> ourResults = null;
2775 object SyncObject = new object();
2776
2777 ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
2778 {
2779 ourResults = results;
2780 Monitor.PulseAll(SyncObject);
2781 };
2782
2783 ODERayRequest req = new ODERayRequest();
2784 req.actor = null;
2785 req.callbackMethod = retMethod;
2786 req.length = radius;
2787 req.Origin = position;
2788 req.Count = Count;
2789 req.filter = flags;
2790
2791
2792 lock (SyncObject)
2793 {
2794 m_rayCastManager.QueueRequest(req);
2795 if (!Monitor.Wait(SyncObject, 500))
2796 return new List<ContactResult>();
2797 }
2798
2799 if (ourResults == null)
2800 return new List<ContactResult>();
2801 return ourResults;
2802 }
2803
2804 public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
2805 {
2806 IntPtr geom = IntPtr.Zero;;
2807
2808 if (actor != null)
2809 {
2810 if (actor is OdePrim)
2811 geom = ((OdePrim)actor).prim_geom;
2812 else if (actor is OdeCharacter)
2813 geom = ((OdePrim)actor).prim_geom;
2814 }
2815
2816 List<ContactResult> ourResults = null;
2817 object SyncObject = new object();
2818
2819 ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
2820 {
2821 ourResults = results;
2822 Monitor.PulseAll(SyncObject);
2823 };
2824
2825 ODERayRequest req = new ODERayRequest();
2826 req.actor = null;
2827 req.callbackMethod = retMethod;
2828 req.length = plane.W;
2829 req.Normal.X = plane.X;
2830 req.Normal.Y = plane.Y;
2831 req.Normal.Z = plane.Z;
2832 req.Count = Count;
2833 req.filter = flags;
2834
2835 lock (SyncObject)
2836 {
2837 m_rayCastManager.QueueRequest(req);
2838 if (!Monitor.Wait(SyncObject, 500))
2839 return new List<ContactResult>();
2840 }
2841
2842 if (ourResults == null)
2843 return new List<ContactResult>();
2844 return ourResults;
2845 }
2846
2847 public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
2848 {
2849 Util.FireAndForget( delegate
2850 {
2851 ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
2852 if(sitAvatar != null)
2853 sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
2854 });
2855 return 1;
2856 }
2857
2858 }
2859}