aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics')
-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.cs2
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsActor.cs75
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsScene.cs69
-rw-r--r--OpenSim/Region/Physics/Manager/VehicleConstants.cs45
-rw-r--r--OpenSim/Region/Physics/Manager/ZeroMesher.cs5
-rw-r--r--OpenSim/Region/Physics/Meshing/Mesh.cs72
-rw-r--r--OpenSim/Region/Physics/Meshing/Meshmerizer.cs28
-rw-r--r--OpenSim/Region/Physics/Meshing/SculptMap.cs62
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs6
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs11
-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.cs401
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs1026
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs2284
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/SculptMap.cs197
-rw-r--r--OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs646
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1470
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs1083
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs4074
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs603
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs2005
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs90
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2835
34 files changed, 27991 insertions, 39 deletions
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..5b743e8
--- /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,convex);
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, convex);
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 3a9ca1b..c32cf38 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -36,6 +36,7 @@ namespace OpenSim.Region.Physics.Manager
36 { 36 {
37 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); 37 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod);
38 IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); 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,bool convex);
39 } 40 }
40 41
41 // Values for level of detail to be passed to the mesher. 42 // Values for level of detail to be passed to the mesher.
@@ -65,5 +66,6 @@ namespace OpenSim.Region.Physics.Manager
65 void releasePinned(); 66 void releasePinned();
66 void Append(IMesh newMesh); 67 void Append(IMesh newMesh);
67 void TransformLinear(float[,] matrix, float[] offset); 68 void TransformLinear(float[,] matrix, float[] offset);
69 Vector3 GetCentroid();
68 } 70 }
69} 71}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 0587054..aaeae86 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -59,15 +59,30 @@ namespace OpenSim.Region.Physics.Manager
59 public Vector3 Position; 59 public Vector3 Position;
60 public Vector3 SurfaceNormal; 60 public Vector3 SurfaceNormal;
61 public float PenetrationDepth; 61 public float PenetrationDepth;
62 public float RelativeSpeed;
62 63
63 public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth) 64 public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth)
64 { 65 {
65 Position = position; 66 Position = position;
66 SurfaceNormal = surfaceNormal; 67 SurfaceNormal = surfaceNormal;
67 PenetrationDepth = penetrationDepth; 68 PenetrationDepth = penetrationDepth;
69 RelativeSpeed = 0f; // for now let this one be set explicity
68 } 70 }
69 } 71 }
70 72
73 public struct ContactData
74 {
75 public float mu;
76 public float bounce;
77 public bool softcolide;
78
79 public ContactData(float _mu, float _bounce, bool _softcolide)
80 {
81 mu = _mu;
82 bounce = _bounce;
83 softcolide = _softcolide;
84 }
85 }
71 /// <summary> 86 /// <summary>
72 /// Used to pass collision information to OnCollisionUpdate listeners. 87 /// Used to pass collision information to OnCollisionUpdate listeners.
73 /// </summary> 88 /// </summary>
@@ -135,6 +150,8 @@ namespace OpenSim.Region.Physics.Manager
135 /// </summary> 150 /// </summary>
136 public event CollisionUpdate OnCollisionUpdate; 151 public event CollisionUpdate OnCollisionUpdate;
137 152
153 public virtual void SetVehicle(object vdata) { }
154
138 public event OutOfBounds OnOutOfBounds; 155 public event OutOfBounds OnOutOfBounds;
139#pragma warning restore 67 156#pragma warning restore 67
140 157
@@ -142,11 +159,29 @@ namespace OpenSim.Region.Physics.Manager
142 { 159 {
143 get { return new NullPhysicsActor(); } 160 get { return new NullPhysicsActor(); }
144 } 161 }
162
163 public virtual bool Building { get; set; }
164
165 public virtual void getContactData(ref ContactData cdata)
166 {
167 cdata.mu = 0;
168 cdata.bounce = 0;
169 }
145 170
146 public abstract bool Stopped { get; } 171 public abstract bool Stopped { get; }
147 172
148 public abstract Vector3 Size { get; set; } 173 public abstract Vector3 Size { get; set; }
149 174
175 public virtual bool Phantom { get; set; }
176
177 public virtual bool IsVolumeDtc
178 {
179 get { return false; }
180 set { return; }
181 }
182
183 public virtual byte PhysicsShapeType { get; set; }
184
150 public abstract PrimitiveBaseShape Shape { set; } 185 public abstract PrimitiveBaseShape Shape { set; }
151 186
152 uint m_baseLocalID; 187 uint m_baseLocalID;
@@ -195,6 +230,11 @@ namespace OpenSim.Region.Physics.Manager
195 } 230 }
196 } 231 }
197 232
233 public virtual byte[] Serialize(bool PhysIsRunning)
234 {
235 return new byte[0];
236 }
237
198 public virtual void RaiseOutOfBounds(Vector3 pos) 238 public virtual void RaiseOutOfBounds(Vector3 pos)
199 { 239 {
200 // Make a temporary copy of the event to avoid possibility of 240 // Make a temporary copy of the event to avoid possibility of
@@ -222,6 +262,11 @@ namespace OpenSim.Region.Physics.Manager
222 { 262 {
223 } 263 }
224 264
265 public virtual float Density { get; set; }
266 public virtual float GravModifier { get; set; }
267 public virtual float Friction { get; set; }
268 public virtual float Bounce { get; set; }
269
225 /// <summary> 270 /// <summary>
226 /// Position of this actor. 271 /// Position of this actor.
227 /// </summary> 272 /// </summary>
@@ -249,6 +294,34 @@ namespace OpenSim.Region.Physics.Manager
249 public abstract Vector3 GeometricCenter { get; } 294 public abstract Vector3 GeometricCenter { get; }
250 public abstract Vector3 CenterOfMass { get; } 295 public abstract Vector3 CenterOfMass { get; }
251 296
297 public virtual Vector3 OOBsize
298 {
299 get
300 {
301 Vector3 s=Size;
302 s.X *=0.5f;
303 s.Y *=0.5f;
304 s.Z *=0.5f;
305 return s;
306 }
307 }
308
309 public virtual Vector3 OOBoffset
310 {
311 get
312 {
313 return Vector3.Zero;
314 }
315 }
316
317 public virtual float OOBRadiusSQ
318 {
319 get
320 {
321 return Size.LengthSquared() * 0.25f; // ((0.5^2)
322 }
323 }
324
252 /// <summary> 325 /// <summary>
253 /// Velocity of this actor. 326 /// Velocity of this actor.
254 /// </summary> 327 /// </summary>
@@ -384,7 +457,6 @@ namespace OpenSim.Region.Physics.Manager
384 457
385 public override void VehicleFloatParam(int param, float value) 458 public override void VehicleFloatParam(int param, float value)
386 { 459 {
387
388 } 460 }
389 461
390 public override void VehicleVectorParam(int param, Vector3 value) 462 public override void VehicleVectorParam(int param, Vector3 value)
@@ -554,5 +626,6 @@ namespace OpenSim.Region.Physics.Manager
554 { 626 {
555 return false; 627 return false;
556 } 628 }
629
557 } 630 }
558} 631}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index b32cd30..cfede55 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -43,6 +43,34 @@ namespace OpenSim.Region.Physics.Manager
43 public delegate void JointDeactivated(PhysicsJoint joint); 43 public delegate void JointDeactivated(PhysicsJoint joint);
44 public delegate void JointErrorMessage(PhysicsJoint joint, string message); // this refers to an "error message due to a problem", not "amount of joint constraint violation" 44 public delegate void JointErrorMessage(PhysicsJoint joint, string message); // this refers to an "error message due to a problem", not "amount of joint constraint violation"
45 45
46 public enum RayFilterFlags:ushort
47 {
48 // the flags
49 water = 0x01,
50 land = 0x02,
51 agent = 0x04,
52 nonphysical = 0x08,
53 physical = 0x10,
54 phantom = 0x20,
55 volumedtc = 0x40,
56
57 // ray cast colision control (may only work for meshs)
58 BackFaceCull = 0x4000,
59 ClosestHit = 0x8000,
60
61 // some combinations
62 LSLPhanton = phantom | volumedtc,
63 PrimsNonPhantom = nonphysical | physical,
64 PrimsNonPhantomAgents = nonphysical | physical | agent,
65
66 AllPrims = nonphysical | phantom | volumedtc | physical,
67 AllButLand = agent | nonphysical | physical | phantom | volumedtc,
68
69 ClosestAndBackCull = ClosestHit | BackFaceCull,
70
71 All = 0x3f
72 }
73
46 /// <summary> 74 /// <summary>
47 /// Contact result from a raycast. 75 /// Contact result from a raycast.
48 /// </summary> 76 /// </summary>
@@ -54,6 +82,8 @@ namespace OpenSim.Region.Physics.Manager
54 public Vector3 Normal; 82 public Vector3 Normal;
55 } 83 }
56 84
85
86
57 public abstract class PhysicsScene 87 public abstract class PhysicsScene
58 { 88 {
59// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 89// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
@@ -125,6 +155,25 @@ namespace OpenSim.Region.Physics.Manager
125 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 155 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
126 Vector3 size, Quaternion rotation, bool isPhysical, uint localid); 156 Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
127 157
158 public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
159 uint localid, byte[] sdata)
160 {
161 return null;
162 }
163
164 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
165 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
166 {
167 return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
168 }
169
170
171 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
172 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
173 {
174 return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
175 }
176
128 public virtual float TimeDilation 177 public virtual float TimeDilation
129 { 178 {
130 get { return 1.0f; } 179 get { return 1.0f; }
@@ -236,7 +285,7 @@ namespace OpenSim.Region.Physics.Manager
236 } 285 }
237 286
238 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} 287 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
239 288 public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {}
240 public virtual void UnCombine(PhysicsScene pScene) {} 289 public virtual void UnCombine(PhysicsScene pScene) {}
241 290
242 /// <summary> 291 /// <summary>
@@ -274,5 +323,23 @@ namespace OpenSim.Region.Physics.Manager
274 { 323 {
275 return new List<ContactResult>(); 324 return new List<ContactResult>();
276 } 325 }
326
327 public virtual object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
328 {
329 return null;
330 }
331
332 public virtual bool SuportsRaycastWorldFiltered()
333 {
334 return false;
335 }
336
337 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){}
338 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { }
339 public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
340 {
341 return new List<ContactResult>();
342 }
343
277 } 344 }
278} 345}
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 ba19db6..8a3b50b 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); 67 return CreateMesh(primName, primShape, size, lod, false);
68 } 68 }
69 69
70 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
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 // Remove the reference to the encoded JPEG2000 data so it can be GCed 77 // Remove the reference to the encoded JPEG2000 data so it can be GCed
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
index f781ff9..c715642 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,57 @@ 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
83 public void CalcNormals() 151 public void CalcNormals()
84 { 152 {
85 int iTriangles = m_triangles.Count; 153 int iTriangles = m_triangles.Count;
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index 3bd15ce..825b858 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing
74#endif 74#endif
75 75
76 private bool cacheSculptMaps = true; 76 private bool cacheSculptMaps = true;
77 private bool cacheSculptAlphaMaps = true;
78
77 private string decodedSculptMapPath = null; 79 private string decodedSculptMapPath = null;
78 private bool useMeshiesPhysicsMesh = false; 80 private bool useMeshiesPhysicsMesh = false;
79 81
@@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing
87 IConfig mesh_config = config.Configs["Mesh"]; 89 IConfig mesh_config = config.Configs["Mesh"];
88 90
89 decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); 91 decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache");
92
90 cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); 93 cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps);
94
95 if (Environment.OSVersion.Platform == PlatformID.Unix)
96 {
97 cacheSculptAlphaMaps = false;
98 }
99 else
100 cacheSculptAlphaMaps = cacheSculptMaps;
101
91 if(mesh_config != null) 102 if(mesh_config != null)
92 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); 103 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
93 104
@@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing
268 { 279 {
269 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) 280 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces))
270 return null; 281 return null;
282 // Remove the reference to any JPEG2000 sculpt data so it can be GCed
283 // don't loose it
284 // primShape.SculptData = Utils.EmptyBytes;
271 } 285 }
286// primShape.SculptDataLoaded = true;
272 } 287 }
273 else 288 else
274 { 289 {
275 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) 290 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces))
276 return null; 291 return null;
277 } 292 }
278 293 // keep compatible
279 // Remove the reference to any JPEG2000 sculpt data so it can be GCed
280 primShape.SculptData = Utils.EmptyBytes; 294 primShape.SculptData = Utils.EmptyBytes;
281 295
282 int numCoords = coords.Count; 296 int numCoords = coords.Count;
@@ -321,7 +335,7 @@ namespace OpenSim.Region.Physics.Meshing
321 335
322 if (primShape.SculptData.Length <= 0) 336 if (primShape.SculptData.Length <= 0)
323 { 337 {
324 m_log.ErrorFormat("[MESH]: asset data for {0} is zero length", primName); 338 m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName);
325 return false; 339 return false;
326 } 340 }
327 341
@@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing
482 496
483 //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); 497 //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData);
484 498
485 if (cacheSculptMaps) 499 if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0)))
500 // don't cache images with alpha channel in linux since mono can't load them correctly)
486 { 501 {
487 try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } 502 try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); }
488 catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } 503 catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); }
@@ -702,6 +717,11 @@ namespace OpenSim.Region.Physics.Meshing
702 return CreateMesh(primName, primShape, size, lod, false); 717 return CreateMesh(primName, primShape, size, lod, false);
703 } 718 }
704 719
720 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
721 {
722 return CreateMesh(primName, primShape, size, lod, false);
723 }
724
705 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) 725 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
706 { 726 {
707#if SPAM 727#if SPAM
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 0716214..a41c856 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -601,8 +601,8 @@ namespace OpenSim.Region.Physics.OdePlugin
601 break; 601 break;
602 602
603 case HollowShape.Circle: 603 case HollowShape.Circle:
604 // Hollow shape is a perfect cyllinder in respect to the cube's scale 604 // Hollow shape is a perfect cylinder in respect to the cube's scale
605 // Cyllinder hollow volume calculation 605 // Cylinder hollow volume calculation
606 606
607 hollowVolume *= 0.1963495f * 3.07920140172638f; 607 hollowVolume *= 0.1963495f * 3.07920140172638f;
608 break; 608 break;
@@ -2827,7 +2827,7 @@ Console.WriteLine(" JointCreateFixed");
2827 } 2827 }
2828 public override bool PIDActive { set { m_usePID = value; } } 2828 public override bool PIDActive { set { m_usePID = value; } }
2829 public override float PIDTau { set { m_PIDTau = value; } } 2829 public override float PIDTau { set { m_PIDTau = value; } }
2830 2830
2831 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 2831 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2832 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 2832 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2833 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 2833 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
index 8d7d3b3..7e3ec63 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
@@ -137,8 +137,15 @@ namespace OpenSim.Region.Physics.OdePlugin
137 ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); 137 ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
138 for (int i = 0; i < reqs.Length; i++) 138 for (int i = 0; i < reqs.Length; i++)
139 { 139 {
140 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 140 try
141 RayCast(reqs[i]); // if there isn't anyone to send results 141 {
142 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
143 RayCast(reqs[i]); // if there isn't anyone to send results
144 }
145 catch
146 {
147 //Fail silently
148 }
142 } 149 }
143 150
144 m_PendingRequests.Clear(); 151 m_PendingRequests.Clear();
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..c715642
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/Mesh.cs
@@ -0,0 +1,401 @@
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;
35
36namespace OpenSim.Region.Physics.Meshing
37{
38 public class Mesh : IMesh
39 {
40 private Dictionary<Vertex, int> m_vertices;
41 private List<Triangle> m_triangles;
42 GCHandle m_pinnedVertexes;
43 GCHandle m_pinnedIndex;
44 IntPtr m_verticesPtr = IntPtr.Zero;
45 int m_vertexCount = 0;
46 IntPtr m_indicesPtr = IntPtr.Zero;
47 int m_indexCount = 0;
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 }
70
71 public Mesh()
72 {
73 vertexcomp vcomp = new vertexcomp();
74
75 m_vertices = new Dictionary<Vertex, int>(vcomp);
76 m_triangles = new List<Triangle>();
77 _centroid = Vector3.Zero;
78 _centroidDiv = 0;
79 }
80
81 public Mesh Clone()
82 {
83 Mesh result = new Mesh();
84
85 foreach (Triangle t in m_triangles)
86 {
87 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
88 }
89 result._centroid = _centroid;
90 result._centroidDiv = _centroidDiv;
91 return result;
92 }
93
94 public void Add(Triangle triangle)
95 {
96 if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
97 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
98 // If a vertex of the triangle is not yet in the vertices list,
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
116 if (!m_vertices.ContainsKey(triangle.v1))
117 {
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 }
124 if (!m_vertices.ContainsKey(triangle.v2))
125 {
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 }
132 if (!m_vertices.ContainsKey(triangle.v3))
133 {
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 }
140 m_triangles.Add(triangle);
141 }
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 public void CalcNormals()
152 {
153 int iTriangles = m_triangles.Count;
154
155 this.m_normals = new float[iTriangles * 3];
156
157 int i = 0;
158 foreach (Triangle t in m_triangles)
159 {
160 float ux, uy, uz;
161 float vx, vy, vz;
162 float wx, wy, wz;
163
164 ux = t.v1.X;
165 uy = t.v1.Y;
166 uz = t.v1.Z;
167
168 vx = t.v2.X;
169 vy = t.v2.Y;
170 vz = t.v2.Z;
171
172 wx = t.v3.X;
173 wy = t.v3.Y;
174 wz = t.v3.Z;
175
176
177 // Vectors for edges
178 float e1x, e1y, e1z;
179 float e2x, e2y, e2z;
180
181 e1x = ux - vx;
182 e1y = uy - vy;
183 e1z = uz - vz;
184
185 e2x = ux - wx;
186 e2y = uy - wy;
187 e2z = uz - wz;
188
189
190 // Cross product for normal
191 float nx, ny, nz;
192 nx = e1y * e2z - e1z * e2y;
193 ny = e1z * e2x - e1x * e2z;
194 nz = e1x * e2y - e1y * e2x;
195
196 // Length
197 float l = (float)Math.Sqrt(nx * nx + ny * ny + nz * nz);
198 float lReciprocal = 1.0f / l;
199
200 // Normalized "normal"
201 //nx /= l;
202 //ny /= l;
203 //nz /= l;
204
205 m_normals[i] = nx * lReciprocal;
206 m_normals[i + 1] = ny * lReciprocal;
207 m_normals[i + 2] = nz * lReciprocal;
208
209 i += 3;
210 }
211 }
212
213 public List<Vector3> getVertexList()
214 {
215 List<Vector3> result = new List<Vector3>();
216 foreach (Vertex v in m_vertices.Keys)
217 {
218 result.Add(new Vector3(v.X, v.Y, v.Z));
219 }
220 return result;
221 }
222
223 private float[] getVertexListAsFloat()
224 {
225 if (m_vertices == null)
226 throw new NotSupportedException();
227 float[] result = new float[m_vertices.Count * 3];
228 foreach (KeyValuePair<Vertex, int> kvp in m_vertices)
229 {
230 Vertex v = kvp.Key;
231 int i = kvp.Value;
232 result[3 * i + 0] = v.X;
233 result[3 * i + 1] = v.Y;
234 result[3 * i + 2] = v.Z;
235 }
236 return result;
237 }
238
239 public float[] getVertexListAsFloatLocked()
240 {
241 if (m_pinnedVertexes.IsAllocated)
242 return (float[])(m_pinnedVertexes.Target);
243
244 float[] result = getVertexListAsFloat();
245 m_pinnedVertexes = GCHandle.Alloc(result, GCHandleType.Pinned);
246 // Inform the garbage collector of this unmanaged allocation so it can schedule
247 // the next GC round more intelligently
248 GC.AddMemoryPressure(Buffer.ByteLength(result));
249
250 return result;
251 }
252
253 public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount)
254 {
255 // A vertex is 3 floats
256 vertexStride = 3 * sizeof(float);
257
258 // If there isn't an unmanaged array allocated yet, do it now
259 if (m_verticesPtr == IntPtr.Zero)
260 {
261 float[] vertexList = getVertexListAsFloat();
262 // Each vertex is 3 elements (floats)
263 m_vertexCount = vertexList.Length / 3;
264 int byteCount = m_vertexCount * vertexStride;
265 m_verticesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount);
266 System.Runtime.InteropServices.Marshal.Copy(vertexList, 0, m_verticesPtr, m_vertexCount * 3);
267 }
268 vertices = m_verticesPtr;
269 vertexCount = m_vertexCount;
270 }
271
272 public int[] getIndexListAsInt()
273 {
274 if (m_triangles == null)
275 throw new NotSupportedException();
276 int[] result = new int[m_triangles.Count * 3];
277 for (int i = 0; i < m_triangles.Count; i++)
278 {
279 Triangle t = m_triangles[i];
280 result[3 * i + 0] = m_vertices[t.v1];
281 result[3 * i + 1] = m_vertices[t.v2];
282 result[3 * i + 2] = m_vertices[t.v3];
283 }
284 return result;
285 }
286
287 /// <summary>
288 /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA
289 /// </summary>
290 /// <returns></returns>
291 public int[] getIndexListAsIntLocked()
292 {
293 if (m_pinnedIndex.IsAllocated)
294 return (int[])(m_pinnedIndex.Target);
295
296 int[] result = getIndexListAsInt();
297 m_pinnedIndex = GCHandle.Alloc(result, GCHandleType.Pinned);
298 // Inform the garbage collector of this unmanaged allocation so it can schedule
299 // the next GC round more intelligently
300 GC.AddMemoryPressure(Buffer.ByteLength(result));
301
302 return result;
303 }
304
305 public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount)
306 {
307 // If there isn't an unmanaged array allocated yet, do it now
308 if (m_indicesPtr == IntPtr.Zero)
309 {
310 int[] indexList = getIndexListAsInt();
311 m_indexCount = indexList.Length;
312 int byteCount = m_indexCount * sizeof(int);
313 m_indicesPtr = System.Runtime.InteropServices.Marshal.AllocHGlobal(byteCount);
314 System.Runtime.InteropServices.Marshal.Copy(indexList, 0, m_indicesPtr, m_indexCount);
315 }
316 // A triangle is 3 ints (indices)
317 triStride = 3 * sizeof(int);
318 indices = m_indicesPtr;
319 indexCount = m_indexCount;
320 }
321
322 public void releasePinned()
323 {
324 if (m_pinnedVertexes.IsAllocated)
325 m_pinnedVertexes.Free();
326 if (m_pinnedIndex.IsAllocated)
327 m_pinnedIndex.Free();
328 if (m_verticesPtr != IntPtr.Zero)
329 {
330 System.Runtime.InteropServices.Marshal.FreeHGlobal(m_verticesPtr);
331 m_verticesPtr = IntPtr.Zero;
332 }
333 if (m_indicesPtr != IntPtr.Zero)
334 {
335 System.Runtime.InteropServices.Marshal.FreeHGlobal(m_indicesPtr);
336 m_indicesPtr = IntPtr.Zero;
337 }
338 }
339
340 /// <summary>
341 /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions
342 /// </summary>
343 public void releaseSourceMeshData()
344 {
345 m_triangles = null;
346 m_vertices = null;
347 }
348
349 public void Append(IMesh newMesh)
350 {
351 if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
352 throw new NotSupportedException("Attempt to Append to a pinned Mesh");
353
354 if (!(newMesh is Mesh))
355 return;
356
357 foreach (Triangle t in ((Mesh)newMesh).m_triangles)
358 Add(t);
359 }
360
361 // Do a linear transformation of mesh.
362 public void TransformLinear(float[,] matrix, float[] offset)
363 {
364 if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero)
365 throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh");
366
367 foreach (Vertex v in m_vertices.Keys)
368 {
369 if (v == null)
370 continue;
371 float x, y, z;
372 x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0];
373 y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1];
374 z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2];
375 v.X = x + offset[0];
376 v.Y = y + offset[1];
377 v.Z = z + offset[2];
378 }
379 }
380
381 public void DumpRaw(String path, String name, String title)
382 {
383 if (path == null)
384 return;
385 String fileName = name + "_" + title + ".raw";
386 String completePath = System.IO.Path.Combine(path, fileName);
387 StreamWriter sw = new StreamWriter(completePath);
388 foreach (Triangle t in m_triangles)
389 {
390 String s = t.ToStringRaw();
391 sw.WriteLine(s);
392 }
393 sw.Close();
394 }
395
396 public void TrimExcess()
397 {
398 m_triangles.TrimExcess();
399 }
400 }
401}
diff --git a/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
new file mode 100644
index 0000000..f002bba
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/Meshmerizer.cs
@@ -0,0 +1,1026 @@
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;
45
46namespace OpenSim.Region.Physics.Meshing
47{
48 public class MeshmerizerPlugin : IMeshingPlugin
49 {
50 public MeshmerizerPlugin()
51 {
52 }
53
54 public string GetName()
55 {
56 return "UbitMeshmerizer";
57 }
58
59 public IMesher GetMesher(IConfigSource config)
60 {
61 return new Meshmerizer(config);
62 }
63 }
64
65 public class Meshmerizer : IMesher
66 {
67 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
68
69 // Setting baseDir to a path will enable the dumping of raw files
70 // raw files can be imported by blender so a visual inspection of the results can be done
71#if SPAM
72 const string baseDir = "rawFiles";
73#else
74 private const string baseDir = null; //"rawFiles";
75#endif
76
77 private bool cacheSculptMaps = true;
78 private bool cacheSculptAlphaMaps = true;
79
80 private string decodedSculptMapPath = null;
81 private bool useMeshiesPhysicsMesh = false;
82
83 private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh
84
85 private Dictionary<ulong, Mesh> m_uniqueMeshes = new Dictionary<ulong, Mesh>();
86
87 public Meshmerizer(IConfigSource config)
88 {
89 IConfig start_config = config.Configs["Startup"];
90 IConfig mesh_config = config.Configs["Mesh"];
91
92 decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache");
93
94 cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps);
95
96 if (Environment.OSVersion.Platform == PlatformID.Unix)
97 {
98 cacheSculptAlphaMaps = false;
99 }
100 else
101 cacheSculptAlphaMaps = cacheSculptMaps;
102
103 if(mesh_config != null)
104 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
105
106 try
107 {
108 if (!Directory.Exists(decodedSculptMapPath))
109 Directory.CreateDirectory(decodedSculptMapPath);
110 }
111 catch (Exception e)
112 {
113 m_log.WarnFormat("[SCULPT]: Unable to create {0} directory: ", decodedSculptMapPath, e.Message);
114 }
115 }
116
117 /// <summary>
118 /// creates a simple box mesh of the specified size. This mesh is of very low vertex count and may
119 /// be useful as a backup proxy when level of detail is not needed or when more complex meshes fail
120 /// for some reason
121 /// </summary>
122 /// <param name="minX"></param>
123 /// <param name="maxX"></param>
124 /// <param name="minY"></param>
125 /// <param name="maxY"></param>
126 /// <param name="minZ"></param>
127 /// <param name="maxZ"></param>
128 /// <returns></returns>
129 private static Mesh CreateSimpleBoxMesh(float minX, float maxX, float minY, float maxY, float minZ, float maxZ)
130 {
131 Mesh box = new Mesh();
132 List<Vertex> vertices = new List<Vertex>();
133 // bottom
134
135 vertices.Add(new Vertex(minX, maxY, minZ));
136 vertices.Add(new Vertex(maxX, maxY, minZ));
137 vertices.Add(new Vertex(maxX, minY, minZ));
138 vertices.Add(new Vertex(minX, minY, minZ));
139
140 box.Add(new Triangle(vertices[0], vertices[1], vertices[2]));
141 box.Add(new Triangle(vertices[0], vertices[2], vertices[3]));
142
143 // top
144
145 vertices.Add(new Vertex(maxX, maxY, maxZ));
146 vertices.Add(new Vertex(minX, maxY, maxZ));
147 vertices.Add(new Vertex(minX, minY, maxZ));
148 vertices.Add(new Vertex(maxX, minY, maxZ));
149
150 box.Add(new Triangle(vertices[4], vertices[5], vertices[6]));
151 box.Add(new Triangle(vertices[4], vertices[6], vertices[7]));
152
153 // sides
154
155 box.Add(new Triangle(vertices[5], vertices[0], vertices[3]));
156 box.Add(new Triangle(vertices[5], vertices[3], vertices[6]));
157
158 box.Add(new Triangle(vertices[1], vertices[0], vertices[5]));
159 box.Add(new Triangle(vertices[1], vertices[5], vertices[4]));
160
161 box.Add(new Triangle(vertices[7], vertices[1], vertices[4]));
162 box.Add(new Triangle(vertices[7], vertices[2], vertices[1]));
163
164 box.Add(new Triangle(vertices[3], vertices[2], vertices[7]));
165 box.Add(new Triangle(vertices[3], vertices[7], vertices[6]));
166
167 return box;
168 }
169
170 /// <summary>
171 /// Creates a simple bounding box mesh for a complex input mesh
172 /// </summary>
173 /// <param name="meshIn"></param>
174 /// <returns></returns>
175 private static Mesh CreateBoundingBoxMesh(Mesh meshIn)
176 {
177 float minX = float.MaxValue;
178 float maxX = float.MinValue;
179 float minY = float.MaxValue;
180 float maxY = float.MinValue;
181 float minZ = float.MaxValue;
182 float maxZ = float.MinValue;
183
184 foreach (Vector3 v in meshIn.getVertexList())
185 {
186 if (v.X < minX) minX = v.X;
187 if (v.Y < minY) minY = v.Y;
188 if (v.Z < minZ) minZ = v.Z;
189
190 if (v.X > maxX) maxX = v.X;
191 if (v.Y > maxY) maxY = v.Y;
192 if (v.Z > maxZ) maxZ = v.Z;
193 }
194
195 return CreateSimpleBoxMesh(minX, maxX, minY, maxY, minZ, maxZ);
196 }
197
198 private void ReportPrimError(string message, string primName, PrimMesh primMesh)
199 {
200 m_log.Error(message);
201 m_log.Error("\nPrim Name: " + primName);
202 m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString());
203 }
204
205 /// <summary>
206 /// Add a submesh to an existing list of coords and faces.
207 /// </summary>
208 /// <param name="subMeshData"></param>
209 /// <param name="size">Size of entire object</param>
210 /// <param name="coords"></param>
211 /// <param name="faces"></param>
212 private void AddSubMesh(OSDMap subMeshData, Vector3 size, List<Coord> coords, List<Face> faces)
213 {
214 // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap));
215
216 // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level
217 // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no
218 // geometry for this submesh.
219 if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"]))
220 return;
221
222 OpenMetaverse.Vector3 posMax;
223 OpenMetaverse.Vector3 posMin;
224 if (subMeshData.ContainsKey("PositionDomain"))
225 {
226 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3();
227 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3();
228 }
229 else
230 {
231 posMax = new Vector3(0.5f, 0.5f, 0.5f);
232 posMin = new Vector3(-0.5f, -0.5f, -0.5f);
233 }
234
235 ushort faceIndexOffset = (ushort)coords.Count;
236
237 byte[] posBytes = subMeshData["Position"].AsBinary();
238 for (int i = 0; i < posBytes.Length; i += 6)
239 {
240 ushort uX = Utils.BytesToUInt16(posBytes, i);
241 ushort uY = Utils.BytesToUInt16(posBytes, i + 2);
242 ushort uZ = Utils.BytesToUInt16(posBytes, i + 4);
243
244 Coord c = new Coord(
245 Utils.UInt16ToFloat(uX, posMin.X, posMax.X) * size.X,
246 Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y) * size.Y,
247 Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z) * size.Z);
248
249 coords.Add(c);
250 }
251
252 byte[] triangleBytes = subMeshData["TriangleList"].AsBinary();
253 for (int i = 0; i < triangleBytes.Length; i += 6)
254 {
255 ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset);
256 ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset);
257 ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset);
258 Face f = new Face(v1, v2, v3);
259 faces.Add(f);
260 }
261 }
262
263 /// <summary>
264 /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type.
265 /// </summary>
266 /// <param name="primName"></param>
267 /// <param name="primShape"></param>
268 /// <param name="size"></param>
269 /// <param name="lod"></param>
270 /// <returns></returns>
271 private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool convex)
272 {
273// m_log.DebugFormat(
274// "[MESH]: Creating physics proxy for {0}, shape {1}",
275// primName, (OpenMetaverse.SculptType)primShape.SculptType);
276
277 List<Coord> coords;
278 List<Face> faces;
279
280 if (primShape.SculptEntry)
281 {
282 if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh)
283 {
284 if (!useMeshiesPhysicsMesh)
285 return null;
286
287 if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, size, out coords, out faces, convex))
288 return null;
289 }
290 else
291 {
292 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces))
293 return null;
294 }
295 }
296 else
297 {
298 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces))
299 return null;
300 }
301
302 primShape.SculptData = Utils.EmptyBytes;
303
304 int numCoords = coords.Count;
305 int numFaces = faces.Count;
306
307 Mesh mesh = new Mesh();
308 // Add the corresponding triangles to the mesh
309 for (int i = 0; i < numFaces; i++)
310 {
311 Face f = faces[i];
312 mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z,
313 coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z,
314 coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z));
315 }
316
317 return mesh;
318 }
319
320 /// <summary>
321 /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim.
322 /// </summary>
323 /// <param name="primName"></param>
324 /// <param name="primShape"></param>
325 /// <param name="size"></param>
326 /// <param name="coords">Coords are added to this list by the method.</param>
327 /// <param name="faces">Faces are added to this list by the method.</param>
328 /// <returns>true if coords and faces were successfully generated, false if not</returns>
329 private bool GenerateCoordsAndFacesFromPrimMeshData(
330 string primName, PrimitiveBaseShape primShape, Vector3 size, out List<Coord> coords, out List<Face> faces, bool convex)
331 {
332// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName);
333
334 bool usemesh = false;
335
336 coords = new List<Coord>();
337 faces = new List<Face>();
338 OSD meshOsd = null;
339
340 if (primShape.SculptData.Length <= 0)
341 {
342 m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName);
343 return false;
344 }
345
346 long start = 0;
347 using (MemoryStream data = new MemoryStream(primShape.SculptData))
348 {
349 try
350 {
351 OSD osd = OSDParser.DeserializeLLSDBinary(data);
352 if (osd is OSDMap)
353 meshOsd = (OSDMap)osd;
354 else
355 {
356 m_log.Warn("[Mesh}: unable to cast mesh asset to OSDMap");
357 return false;
358 }
359 }
360 catch (Exception e)
361 {
362 m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString());
363 }
364
365 start = data.Position;
366 }
367
368 if (meshOsd is OSDMap)
369 {
370 OSDMap physicsParms = null;
371 OSDMap map = (OSDMap)meshOsd;
372
373 if (!convex)
374 {
375 if (map.ContainsKey("physics_shape"))
376 physicsParms = (OSDMap)map["physics_shape"]; // old asset format
377 else if (map.ContainsKey("physics_mesh"))
378 physicsParms = (OSDMap)map["physics_mesh"]; // new asset format
379
380 if (physicsParms != null)
381 usemesh = true;
382 }
383
384 if(!usemesh && (map.ContainsKey("physics_convex")))
385 physicsParms = (OSDMap)map["physics_convex"];
386
387
388 if (physicsParms == null)
389 {
390 m_log.Warn("[MESH]: unknown mesh type");
391 return false;
392 }
393
394 int physOffset = physicsParms["offset"].AsInteger() + (int)start;
395 int physSize = physicsParms["size"].AsInteger();
396
397 if (physOffset < 0 || physSize == 0)
398 return false; // no mesh data in asset
399
400 OSD decodedMeshOsd = new OSD();
401 byte[] meshBytes = new byte[physSize];
402 System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize);
403// byte[] decompressed = new byte[physSize * 5];
404 try
405 {
406 using (MemoryStream inMs = new MemoryStream(meshBytes))
407 {
408 using (MemoryStream outMs = new MemoryStream())
409 {
410 using (ZOutputStream zOut = new ZOutputStream(outMs))
411 {
412 byte[] readBuffer = new byte[2048];
413 int readLen = 0;
414 while ((readLen = inMs.Read(readBuffer, 0, readBuffer.Length)) > 0)
415 {
416 zOut.Write(readBuffer, 0, readLen);
417 }
418 zOut.Flush();
419 outMs.Seek(0, SeekOrigin.Begin);
420
421 byte[] decompressedBuf = outMs.GetBuffer();
422
423 decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf);
424 }
425 }
426 }
427 }
428 catch (Exception e)
429 {
430 m_log.Error("[MESH]: exception decoding physical mesh: " + e.ToString());
431 return false;
432 }
433
434 if (usemesh)
435 {
436 OSDArray decodedMeshOsdArray = null;
437
438 // physics_shape is an array of OSDMaps, one for each submesh
439 if (decodedMeshOsd is OSDArray)
440 {
441 // Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd));
442
443 decodedMeshOsdArray = (OSDArray)decodedMeshOsd;
444 foreach (OSD subMeshOsd in decodedMeshOsdArray)
445 {
446 if (subMeshOsd is OSDMap)
447 AddSubMesh(subMeshOsd as OSDMap, size, coords, faces);
448 }
449 }
450 }
451 else
452 {
453 OSDMap cmap = (OSDMap)decodedMeshOsd;
454 if (cmap == null)
455 return false;
456
457 byte[] data;
458
459 List<float3> vs = new List<float3>();
460 PHullResult hullr = new PHullResult();
461 float3 f3;
462 Coord c;
463 Face f;
464 Vector3 range;
465 Vector3 min;
466
467 const float invMaxU16 = 1.0f / 65535f;
468 int t1;
469 int t2;
470 int t3;
471 int i;
472 int nverts;
473 int nindexs;
474
475 if (cmap.ContainsKey("Max"))
476 range = cmap["Max"].AsVector3();
477 else
478 range = new Vector3(0.5f, 0.5f, 0.5f);
479
480 if (cmap.ContainsKey("Min"))
481 min = cmap["Min"].AsVector3();
482 else
483 min = new Vector3(-0.5f, -0.5f, -0.5f);
484
485 range = range - min;
486 range *= invMaxU16;
487
488 if (!convex && cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions"))
489 {
490 List<int> hsizes = new List<int>();
491 int totalpoints = 0;
492 data = cmap["HullList"].AsBinary();
493 for (i = 0; i < data.Length; i++)
494 {
495 t1 = data[i];
496 if (t1 == 0)
497 t1 = 256;
498 totalpoints += t1;
499 hsizes.Add(t1);
500 }
501
502 data = cmap["Positions"].AsBinary();
503 int ptr = 0;
504 int vertsoffset = 0;
505
506 if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point
507 {
508 foreach (int hullsize in hsizes)
509 {
510 for (i = 0; i < hullsize; i++ )
511 {
512 t1 = data[ptr++];
513 t1 += data[ptr++] << 8;
514 t2 = data[ptr++];
515 t2 += data[ptr++] << 8;
516 t3 = data[ptr++];
517 t3 += data[ptr++] << 8;
518
519 f3 = new float3((t1 * range.X + min.X) * size.X,
520 (t2 * range.Y + min.Y) * size.Y,
521 (t3 * range.Z + min.Z) * size.Z);
522 vs.Add(f3);
523 }
524
525 if(hullsize <3)
526 {
527 vs.Clear();
528 continue;
529 }
530
531 if (hullsize <5)
532 {
533 foreach (float3 point in vs)
534 {
535 c.X = point.x;
536 c.Y = point.y;
537 c.Z = point.z;
538 coords.Add(c);
539 }
540 f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2);
541 faces.Add(f);
542
543 if (hullsize == 4)
544 {
545 // not sure about orientation..
546 f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3);
547 faces.Add(f);
548 f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1);
549 faces.Add(f);
550 f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1);
551 faces.Add(f);
552 }
553 vertsoffset += vs.Count;
554 vs.Clear();
555 continue;
556 }
557
558 if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f))
559 {
560 vs.Clear();
561 continue;
562 }
563
564 nverts = hullr.Vertices.Count;
565 nindexs = hullr.Indices.Count;
566
567 if (nindexs % 3 != 0)
568 {
569 vs.Clear();
570 continue;
571 }
572
573 for (i = 0; i < nverts; i++)
574 {
575 c.X = hullr.Vertices[i].x;
576 c.Y = hullr.Vertices[i].y;
577 c.Z = hullr.Vertices[i].z;
578 coords.Add(c);
579 }
580
581 for (i = 0; i < nindexs; i += 3)
582 {
583 t1 = hullr.Indices[i];
584 if (t1 > nverts)
585 break;
586 t2 = hullr.Indices[i + 1];
587 if (t2 > nverts)
588 break;
589 t3 = hullr.Indices[i + 2];
590 if (t3 > nverts)
591 break;
592 f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3);
593 faces.Add(f);
594 }
595 vertsoffset += nverts;
596 vs.Clear();
597 }
598 }
599 if (coords.Count > 0 && faces.Count > 0)
600 return true;
601 }
602
603 vs.Clear();
604
605 if (cmap.ContainsKey("BoundingVerts"))
606 {
607 data = cmap["BoundingVerts"].AsBinary();
608
609 for (i = 0; i < data.Length; )
610 {
611 t1 = data[i++];
612 t1 += data[i++] << 8;
613 t2 = data[i++];
614 t2 += data[i++] << 8;
615 t3 = data[i++];
616 t3 += data[i++] << 8;
617
618 f3 = new float3((t1 * range.X + min.X) * size.X,
619 (t2 * range.Y + min.Y) * size.Y,
620 (t3 * range.Z + min.Z) * size.Z);
621 vs.Add(f3);
622 }
623
624 if (vs.Count < 3)
625 {
626 vs.Clear();
627 return false;
628 }
629
630 if (vs.Count < 5)
631 {
632 foreach (float3 point in vs)
633 {
634 c.X = point.x;
635 c.Y = point.y;
636 c.Z = point.z;
637 coords.Add(c);
638 }
639 f = new Face(0, 1, 2);
640 faces.Add(f);
641
642 if (vs.Count == 4)
643 {
644 f = new Face(0, 2, 3);
645 faces.Add(f);
646 f = new Face(0, 3, 1);
647 faces.Add(f);
648 f = new Face( 3, 2, 1);
649 faces.Add(f);
650 }
651 vs.Clear();
652 return true;
653 }
654
655 if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f))
656 return false;
657
658 nverts = hullr.Vertices.Count;
659 nindexs = hullr.Indices.Count;
660
661 if (nindexs % 3 != 0)
662 return false;
663
664 for (i = 0; i < nverts; i++)
665 {
666 c.X = hullr.Vertices[i].x;
667 c.Y = hullr.Vertices[i].y;
668 c.Z = hullr.Vertices[i].z;
669 coords.Add(c);
670 }
671 for (i = 0; i < nindexs; i += 3)
672 {
673 t1 = hullr.Indices[i];
674 if (t1 > nverts)
675 break;
676 t2 = hullr.Indices[i + 1];
677 if (t2 > nverts)
678 break;
679 t3 = hullr.Indices[i + 2];
680 if (t3 > nverts)
681 break;
682 f = new Face(t1, t2, t3);
683 faces.Add(f);
684 }
685
686 if (coords.Count > 0 && faces.Count > 0)
687 return true;
688 }
689 else
690 return false;
691 }
692 }
693
694 return true;
695 }
696
697 /// <summary>
698 /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim.
699 /// </summary>
700 /// <param name="primName"></param>
701 /// <param name="primShape"></param>
702 /// <param name="size"></param>
703 /// <param name="lod"></param>
704 /// <param name="coords">Coords are added to this list by the method.</param>
705 /// <param name="faces">Faces are added to this list by the method.</param>
706 /// <returns>true if coords and faces were successfully generated, false if not</returns>
707 private bool GenerateCoordsAndFacesFromPrimSculptData(
708 string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List<Coord> coords, out List<Face> faces)
709 {
710 coords = new List<Coord>();
711 faces = new List<Face>();
712 PrimMesher.SculptMesh sculptMesh;
713 Image idata = null;
714 string decodedSculptFileName = "";
715
716 if (cacheSculptMaps && primShape.SculptTexture != UUID.Zero)
717 {
718 decodedSculptFileName = System.IO.Path.Combine(decodedSculptMapPath, "smap_" + primShape.SculptTexture.ToString());
719 try
720 {
721 if (File.Exists(decodedSculptFileName))
722 {
723 idata = Image.FromFile(decodedSculptFileName);
724 }
725 }
726 catch (Exception e)
727 {
728 m_log.Error("[SCULPT]: unable to load cached sculpt map " + decodedSculptFileName + " " + e.Message);
729
730 }
731 //if (idata != null)
732 // m_log.Debug("[SCULPT]: loaded cached map asset for map ID: " + primShape.SculptTexture.ToString());
733 }
734
735 if (idata == null)
736 {
737 if (primShape.SculptData == null || primShape.SculptData.Length == 0)
738 return false;
739
740 try
741 {
742 OpenMetaverse.Imaging.ManagedImage unusedData;
743 OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata);
744
745 if (idata == null)
746 {
747 // In some cases it seems that the decode can return a null bitmap without throwing
748 // an exception
749 m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName);
750
751 return false;
752 }
753
754 unusedData = null;
755
756 //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData);
757
758 if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0)))
759 // don't cache images with alpha channel in linux since mono can't load them correctly)
760 {
761 try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); }
762 catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); }
763 }
764 }
765 catch (DllNotFoundException)
766 {
767 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!");
768 return false;
769 }
770 catch (IndexOutOfRangeException)
771 {
772 m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed");
773 return false;
774 }
775 catch (Exception ex)
776 {
777 m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message);
778 return false;
779 }
780 }
781
782 PrimMesher.SculptMesh.SculptType sculptType;
783 switch ((OpenMetaverse.SculptType)primShape.SculptType)
784 {
785 case OpenMetaverse.SculptType.Cylinder:
786 sculptType = PrimMesher.SculptMesh.SculptType.cylinder;
787 break;
788 case OpenMetaverse.SculptType.Plane:
789 sculptType = PrimMesher.SculptMesh.SculptType.plane;
790 break;
791 case OpenMetaverse.SculptType.Torus:
792 sculptType = PrimMesher.SculptMesh.SculptType.torus;
793 break;
794 case OpenMetaverse.SculptType.Sphere:
795 sculptType = PrimMesher.SculptMesh.SculptType.sphere;
796 break;
797 default:
798 sculptType = PrimMesher.SculptMesh.SculptType.plane;
799 break;
800 }
801
802 bool mirror = ((primShape.SculptType & 128) != 0);
803 bool invert = ((primShape.SculptType & 64) != 0);
804
805 sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, false, mirror, invert);
806
807 idata.Dispose();
808
809 sculptMesh.DumpRaw(baseDir, primName, "primMesh");
810
811 sculptMesh.Scale(size.X, size.Y, size.Z);
812
813 coords = sculptMesh.coords;
814 faces = sculptMesh.faces;
815
816 return true;
817 }
818
819 /// <summary>
820 /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim.
821 /// </summary>
822 /// <param name="primName"></param>
823 /// <param name="primShape"></param>
824 /// <param name="size"></param>
825 /// <param name="coords">Coords are added to this list by the method.</param>
826 /// <param name="faces">Faces are added to this list by the method.</param>
827 /// <returns>true if coords and faces were successfully generated, false if not</returns>
828 private bool GenerateCoordsAndFacesFromPrimShapeData(
829 string primName, PrimitiveBaseShape primShape, Vector3 size, float lod, out List<Coord> coords, out List<Face> faces)
830 {
831 PrimMesh primMesh;
832 coords = new List<Coord>();
833 faces = new List<Face>();
834
835 float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f;
836 float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f;
837 float pathBegin = (float)primShape.PathBegin * 2.0e-5f;
838 float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f;
839 float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f;
840 float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f;
841
842 float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f;
843 float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f;
844 float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f;
845 if (profileHollow > 0.95f)
846 profileHollow = 0.95f;
847
848 int sides = 4;
849 LevelOfDetail iLOD = (LevelOfDetail)lod;
850 if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
851 sides = 3;
852 else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
853 {
854 switch (iLOD)
855 {
856 case LevelOfDetail.High: sides = 24; break;
857 case LevelOfDetail.Medium: sides = 12; break;
858 case LevelOfDetail.Low: sides = 6; break;
859 case LevelOfDetail.VeryLow: sides = 3; break;
860 default: sides = 24; break;
861 }
862 }
863 else if ((primShape.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
864 { // half circle, prim is a sphere
865 switch (iLOD)
866 {
867 case LevelOfDetail.High: sides = 24; break;
868 case LevelOfDetail.Medium: sides = 12; break;
869 case LevelOfDetail.Low: sides = 6; break;
870 case LevelOfDetail.VeryLow: sides = 3; break;
871 default: sides = 24; break;
872 }
873
874 profileBegin = 0.5f * profileBegin + 0.5f;
875 profileEnd = 0.5f * profileEnd + 0.5f;
876 }
877
878 int hollowSides = sides;
879 if (primShape.HollowShape == HollowShape.Circle)
880 {
881 switch (iLOD)
882 {
883 case LevelOfDetail.High: hollowSides = 24; break;
884 case LevelOfDetail.Medium: hollowSides = 12; break;
885 case LevelOfDetail.Low: hollowSides = 6; break;
886 case LevelOfDetail.VeryLow: hollowSides = 3; break;
887 default: hollowSides = 24; break;
888 }
889 }
890 else if (primShape.HollowShape == HollowShape.Square)
891 hollowSides = 4;
892 else if (primShape.HollowShape == HollowShape.Triangle)
893 hollowSides = 3;
894
895 primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides);
896
897 if (primMesh.errorMessage != null)
898 if (primMesh.errorMessage.Length > 0)
899 m_log.Error("[ERROR] " + primMesh.errorMessage);
900
901 primMesh.topShearX = pathShearX;
902 primMesh.topShearY = pathShearY;
903 primMesh.pathCutBegin = pathBegin;
904 primMesh.pathCutEnd = pathEnd;
905
906 if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible)
907 {
908 primMesh.twistBegin = primShape.PathTwistBegin * 18 / 10;
909 primMesh.twistEnd = primShape.PathTwist * 18 / 10;
910 primMesh.taperX = pathScaleX;
911 primMesh.taperY = pathScaleY;
912
913 if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f)
914 {
915 ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh);
916 if (profileBegin < 0.0f) profileBegin = 0.0f;
917 if (profileEnd > 1.0f) profileEnd = 1.0f;
918 }
919#if SPAM
920 m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString());
921#endif
922 try
923 {
924 primMesh.ExtrudeLinear();
925 }
926 catch (Exception ex)
927 {
928 ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
929 return false;
930 }
931 }
932 else
933 {
934 primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f;
935 primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f;
936 primMesh.radius = 0.01f * primShape.PathRadiusOffset;
937 primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions;
938 primMesh.skew = 0.01f * primShape.PathSkew;
939 primMesh.twistBegin = primShape.PathTwistBegin * 36 / 10;
940 primMesh.twistEnd = primShape.PathTwist * 36 / 10;
941 primMesh.taperX = primShape.PathTaperX * 0.01f;
942 primMesh.taperY = primShape.PathTaperY * 0.01f;
943
944 if (profileBegin < 0.0f || profileBegin >= profileEnd || profileEnd > 1.0f)
945 {
946 ReportPrimError("*** CORRUPT PRIM!! ***", primName, primMesh);
947 if (profileBegin < 0.0f) profileBegin = 0.0f;
948 if (profileEnd > 1.0f) profileEnd = 1.0f;
949 }
950#if SPAM
951 m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString());
952#endif
953 try
954 {
955 primMesh.ExtrudeCircular();
956 }
957 catch (Exception ex)
958 {
959 ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh);
960 return false;
961 }
962 }
963
964 primMesh.DumpRaw(baseDir, primName, "primMesh");
965
966 primMesh.Scale(size.X, size.Y, size.Z);
967
968 coords = primMesh.coords;
969 faces = primMesh.faces;
970
971 return true;
972 }
973
974 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
975 {
976 return CreateMesh(primName, primShape, size, lod, false,false);
977 }
978
979 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
980 {
981 return CreateMesh(primName, primShape, size, lod, false,false);
982 }
983
984 public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex)
985 {
986#if SPAM
987 m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
988#endif
989
990 Mesh mesh = null;
991 ulong key = 0;
992
993 // If this mesh has been created already, return it instead of creating another copy
994 // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory
995 key = primShape.GetMeshKey(size, lod, convex);
996 if (m_uniqueMeshes.TryGetValue(key, out mesh))
997 return mesh;
998
999 if (size.X < 0.01f) size.X = 0.01f;
1000 if (size.Y < 0.01f) size.Y = 0.01f;
1001 if (size.Z < 0.01f) size.Z = 0.01f;
1002
1003 mesh = CreateMeshFromPrimMesher(primName, primShape, size, lod,convex);
1004
1005 if (mesh != null)
1006 {
1007 if ((!isPhysical) && size.X < minSizeForComplexMesh && size.Y < minSizeForComplexMesh && size.Z < minSizeForComplexMesh)
1008 {
1009#if SPAM
1010 m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " +
1011 minSizeForComplexMesh.ToString() + " - creating simple bounding box");
1012#endif
1013 mesh = CreateBoundingBoxMesh(mesh);
1014 mesh.DumpRaw(baseDir, primName, "Z extruded");
1015 }
1016
1017 // trim the vertex and triangle lists to free up memory
1018 mesh.TrimExcess();
1019
1020 m_uniqueMeshes.Add(key, mesh);
1021 }
1022
1023 return mesh;
1024 }
1025 }
1026}
diff --git a/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs
new file mode 100644
index 0000000..53022ad
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/PrimMesher.cs
@@ -0,0 +1,2284 @@
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
241 public struct Face
242 {
243 public int primFace;
244
245 // vertices
246 public int v1;
247 public int v2;
248 public int v3;
249
250 //normals
251 public int n1;
252 public int n2;
253 public int n3;
254
255 // uvs
256 public int uv1;
257 public int uv2;
258 public int uv3;
259
260 public Face(int v1, int v2, int v3)
261 {
262 primFace = 0;
263
264 this.v1 = v1;
265 this.v2 = v2;
266 this.v3 = v3;
267
268 this.n1 = 0;
269 this.n2 = 0;
270 this.n3 = 0;
271
272 this.uv1 = 0;
273 this.uv2 = 0;
274 this.uv3 = 0;
275
276 }
277
278 public Face(int v1, int v2, int v3, int n1, int n2, int n3)
279 {
280 primFace = 0;
281
282 this.v1 = v1;
283 this.v2 = v2;
284 this.v3 = v3;
285
286 this.n1 = n1;
287 this.n2 = n2;
288 this.n3 = n3;
289
290 this.uv1 = 0;
291 this.uv2 = 0;
292 this.uv3 = 0;
293 }
294
295 public Coord SurfaceNormal(List<Coord> coordList)
296 {
297 Coord c1 = coordList[this.v1];
298 Coord c2 = coordList[this.v2];
299 Coord c3 = coordList[this.v3];
300
301 Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
302 Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
303
304 return Coord.Cross(edge1, edge2).Normalize();
305 }
306 }
307
308 public struct ViewerFace
309 {
310 public int primFaceNumber;
311
312 public Coord v1;
313 public Coord v2;
314 public Coord v3;
315
316 public int coordIndex1;
317 public int coordIndex2;
318 public int coordIndex3;
319
320 public Coord n1;
321 public Coord n2;
322 public Coord n3;
323
324 public UVCoord uv1;
325 public UVCoord uv2;
326 public UVCoord uv3;
327
328 public ViewerFace(int primFaceNumber)
329 {
330 this.primFaceNumber = primFaceNumber;
331
332 this.v1 = new Coord();
333 this.v2 = new Coord();
334 this.v3 = new Coord();
335
336 this.coordIndex1 = this.coordIndex2 = this.coordIndex3 = -1; // -1 means not assigned yet
337
338 this.n1 = new Coord();
339 this.n2 = new Coord();
340 this.n3 = new Coord();
341
342 this.uv1 = new UVCoord();
343 this.uv2 = new UVCoord();
344 this.uv3 = new UVCoord();
345 }
346
347 public void Scale(float x, float y, float z)
348 {
349 this.v1.X *= x;
350 this.v1.Y *= y;
351 this.v1.Z *= z;
352
353 this.v2.X *= x;
354 this.v2.Y *= y;
355 this.v2.Z *= z;
356
357 this.v3.X *= x;
358 this.v3.Y *= y;
359 this.v3.Z *= z;
360 }
361
362 public void AddPos(float x, float y, float z)
363 {
364 this.v1.X += x;
365 this.v2.X += x;
366 this.v3.X += x;
367
368 this.v1.Y += y;
369 this.v2.Y += y;
370 this.v3.Y += y;
371
372 this.v1.Z += z;
373 this.v2.Z += z;
374 this.v3.Z += z;
375 }
376
377 public void AddRot(Quat q)
378 {
379 this.v1 *= q;
380 this.v2 *= q;
381 this.v3 *= q;
382
383 this.n1 *= q;
384 this.n2 *= q;
385 this.n3 *= q;
386 }
387
388 public void CalcSurfaceNormal()
389 {
390
391 Coord edge1 = new Coord(this.v2.X - this.v1.X, this.v2.Y - this.v1.Y, this.v2.Z - this.v1.Z);
392 Coord edge2 = new Coord(this.v3.X - this.v1.X, this.v3.Y - this.v1.Y, this.v3.Z - this.v1.Z);
393
394 this.n1 = this.n2 = this.n3 = Coord.Cross(edge1, edge2).Normalize();
395 }
396 }
397
398 internal struct Angle
399 {
400 internal float angle;
401 internal float X;
402 internal float Y;
403
404 internal Angle(float angle, float x, float y)
405 {
406 this.angle = angle;
407 this.X = x;
408 this.Y = y;
409 }
410 }
411
412 internal class AngleList
413 {
414 private float iX, iY; // intersection point
415
416 private static Angle[] angles3 =
417 {
418 new Angle(0.0f, 1.0f, 0.0f),
419 new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
420 new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
421 new Angle(1.0f, 1.0f, 0.0f)
422 };
423
424 private static Coord[] normals3 =
425 {
426 new Coord(0.25f, 0.4330127019f, 0.0f).Normalize(),
427 new Coord(-0.5f, 0.0f, 0.0f).Normalize(),
428 new Coord(0.25f, -0.4330127019f, 0.0f).Normalize(),
429 new Coord(0.25f, 0.4330127019f, 0.0f).Normalize()
430 };
431
432 private static Angle[] angles4 =
433 {
434 new Angle(0.0f, 1.0f, 0.0f),
435 new Angle(0.25f, 0.0f, 1.0f),
436 new Angle(0.5f, -1.0f, 0.0f),
437 new Angle(0.75f, 0.0f, -1.0f),
438 new Angle(1.0f, 1.0f, 0.0f)
439 };
440
441 private static Coord[] normals4 =
442 {
443 new Coord(0.5f, 0.5f, 0.0f).Normalize(),
444 new Coord(-0.5f, 0.5f, 0.0f).Normalize(),
445 new Coord(-0.5f, -0.5f, 0.0f).Normalize(),
446 new Coord(0.5f, -0.5f, 0.0f).Normalize(),
447 new Coord(0.5f, 0.5f, 0.0f).Normalize()
448 };
449
450 private static Angle[] angles24 =
451 {
452 new Angle(0.0f, 1.0f, 0.0f),
453 new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f),
454 new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f),
455 new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f),
456 new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f),
457 new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f),
458 new Angle(0.25f, 0.0f, 1.0f),
459 new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f),
460 new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f),
461 new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f),
462 new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f),
463 new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f),
464 new Angle(0.5f, -1.0f, 0.0f),
465 new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f),
466 new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f),
467 new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f),
468 new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f),
469 new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f),
470 new Angle(0.75f, 0.0f, -1.0f),
471 new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f),
472 new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f),
473 new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f),
474 new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f),
475 new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f),
476 new Angle(1.0f, 1.0f, 0.0f)
477 };
478
479 private Angle interpolatePoints(float newPoint, Angle p1, Angle p2)
480 {
481 float m = (newPoint - p1.angle) / (p2.angle - p1.angle);
482 return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y));
483 }
484
485 private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
486 { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
487 double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
488 double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3);
489
490 if (denom != 0.0)
491 {
492 double ua = uaNumerator / denom;
493 iX = (float)(x1 + ua * (x2 - x1));
494 iY = (float)(y1 + ua * (y2 - y1));
495 }
496 }
497
498 internal List<Angle> angles;
499 internal List<Coord> normals;
500
501 internal void makeAngles(int sides, float startAngle, float stopAngle)
502 {
503 angles = new List<Angle>();
504 normals = new List<Coord>();
505
506 double twoPi = System.Math.PI * 2.0;
507 float twoPiInv = 1.0f / (float)twoPi;
508
509 if (sides < 1)
510 throw new Exception("number of sides not greater than zero");
511 if (stopAngle <= startAngle)
512 throw new Exception("stopAngle not greater than startAngle");
513
514 if ((sides == 3 || sides == 4 || sides == 24))
515 {
516 startAngle *= twoPiInv;
517 stopAngle *= twoPiInv;
518
519 Angle[] sourceAngles;
520 if (sides == 3)
521 sourceAngles = angles3;
522 else if (sides == 4)
523 sourceAngles = angles4;
524 else sourceAngles = angles24;
525
526 int startAngleIndex = (int)(startAngle * sides);
527 int endAngleIndex = sourceAngles.Length - 1;
528 if (stopAngle < 1.0f)
529 endAngleIndex = (int)(stopAngle * sides) + 1;
530 if (endAngleIndex == startAngleIndex)
531 endAngleIndex++;
532
533 for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++)
534 {
535 angles.Add(sourceAngles[angleIndex]);
536 if (sides == 3)
537 normals.Add(normals3[angleIndex]);
538 else if (sides == 4)
539 normals.Add(normals4[angleIndex]);
540 }
541
542 if (startAngle > 0.0f)
543 angles[0] = interpolatePoints(startAngle, angles[0], angles[1]);
544
545 if (stopAngle < 1.0f)
546 {
547 int lastAngleIndex = angles.Count - 1;
548 angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]);
549 }
550 }
551 else
552 {
553 double stepSize = twoPi / sides;
554
555 int startStep = (int)(startAngle / stepSize);
556 double angle = stepSize * startStep;
557 int step = startStep;
558 double stopAngleTest = stopAngle;
559 if (stopAngle < twoPi)
560 {
561 stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1);
562 if (stopAngleTest < stopAngle)
563 stopAngleTest += stepSize;
564 if (stopAngleTest > twoPi)
565 stopAngleTest = twoPi;
566 }
567
568 while (angle <= stopAngleTest)
569 {
570 Angle newAngle;
571 newAngle.angle = (float)angle;
572 newAngle.X = (float)System.Math.Cos(angle);
573 newAngle.Y = (float)System.Math.Sin(angle);
574 angles.Add(newAngle);
575 step += 1;
576 angle = stepSize * step;
577 }
578
579 if (startAngle > angles[0].angle)
580 {
581 Angle newAngle;
582 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));
583 newAngle.angle = startAngle;
584 newAngle.X = iX;
585 newAngle.Y = iY;
586 angles[0] = newAngle;
587 }
588
589 int index = angles.Count - 1;
590 if (stopAngle < angles[index].angle)
591 {
592 Angle newAngle;
593 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));
594 newAngle.angle = stopAngle;
595 newAngle.X = iX;
596 newAngle.Y = iY;
597 angles[index] = newAngle;
598 }
599 }
600 }
601 }
602
603 /// <summary>
604 /// generates a profile for extrusion
605 /// </summary>
606 internal class Profile
607 {
608 private const float twoPi = 2.0f * (float)Math.PI;
609
610 internal string errorMessage = null;
611
612 internal List<Coord> coords;
613 internal List<Face> faces;
614 internal List<Coord> vertexNormals;
615 internal List<float> us;
616 internal List<UVCoord> faceUVs;
617 internal List<int> faceNumbers;
618
619 // use these for making individual meshes for each prim face
620 internal List<int> outerCoordIndices = null;
621 internal List<int> hollowCoordIndices = null;
622 internal List<int> cut1CoordIndices = null;
623 internal List<int> cut2CoordIndices = null;
624
625 internal Coord faceNormal = new Coord(0.0f, 0.0f, 1.0f);
626 internal Coord cutNormal1 = new Coord();
627 internal Coord cutNormal2 = new Coord();
628
629 internal int numOuterVerts = 0;
630 internal int numHollowVerts = 0;
631
632 internal int outerFaceNumber = -1;
633 internal int hollowFaceNumber = -1;
634
635 internal bool calcVertexNormals = false;
636 internal int bottomFaceNumber = 0;
637 internal int numPrimFaces = 0;
638
639 internal Profile()
640 {
641 this.coords = new List<Coord>();
642 this.faces = new List<Face>();
643 this.vertexNormals = new List<Coord>();
644 this.us = new List<float>();
645 this.faceUVs = new List<UVCoord>();
646 this.faceNumbers = new List<int>();
647 }
648
649 internal Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool createFaces, bool calcVertexNormals)
650 {
651 this.calcVertexNormals = calcVertexNormals;
652 this.coords = new List<Coord>();
653 this.faces = new List<Face>();
654 this.vertexNormals = new List<Coord>();
655 this.us = new List<float>();
656 this.faceUVs = new List<UVCoord>();
657 this.faceNumbers = new List<int>();
658
659 Coord center = new Coord(0.0f, 0.0f, 0.0f);
660 //bool hasCenter = false;
661
662 List<Coord> hollowCoords = new List<Coord>();
663 List<Coord> hollowNormals = new List<Coord>();
664 List<float> hollowUs = new List<float>();
665
666 if (calcVertexNormals)
667 {
668 this.outerCoordIndices = new List<int>();
669 this.hollowCoordIndices = new List<int>();
670 this.cut1CoordIndices = new List<int>();
671 this.cut2CoordIndices = new List<int>();
672 }
673
674 bool hasHollow = (hollow > 0.0f);
675
676 bool hasProfileCut = (profileStart > 0.0f || profileEnd < 1.0f);
677
678 AngleList angles = new AngleList();
679 AngleList hollowAngles = new AngleList();
680
681 float xScale = 0.5f;
682 float yScale = 0.5f;
683 if (sides == 4) // corners of a square are sqrt(2) from center
684 {
685 xScale = 0.707f;
686 yScale = 0.707f;
687 }
688
689 float startAngle = profileStart * twoPi;
690 float stopAngle = profileEnd * twoPi;
691
692 try { angles.makeAngles(sides, startAngle, stopAngle); }
693 catch (Exception ex)
694 {
695
696 errorMessage = "makeAngles failed: Exception: " + ex.ToString()
697 + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
698
699 return;
700 }
701
702 this.numOuterVerts = angles.angles.Count;
703
704 // flag to create as few triangles as possible for 3 or 4 side profile
705 bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut);
706
707 if (hasHollow)
708 {
709 if (sides == hollowSides)
710 hollowAngles = angles;
711 else
712 {
713 try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle); }
714 catch (Exception ex)
715 {
716 errorMessage = "makeAngles failed: Exception: " + ex.ToString()
717 + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString();
718
719 return;
720 }
721 }
722 this.numHollowVerts = hollowAngles.angles.Count;
723 }
724 else if (!simpleFace)
725 {
726 this.coords.Add(center);
727 //hasCenter = true;
728 if (this.calcVertexNormals)
729 this.vertexNormals.Add(new Coord(0.0f, 0.0f, 1.0f));
730 this.us.Add(0.0f);
731 }
732
733 float z = 0.0f;
734
735 Angle angle;
736 Coord newVert = new Coord();
737 if (hasHollow && hollowSides != sides)
738 {
739 int numHollowAngles = hollowAngles.angles.Count;
740 for (int i = 0; i < numHollowAngles; i++)
741 {
742 angle = hollowAngles.angles[i];
743 newVert.X = hollow * xScale * angle.X;
744 newVert.Y = hollow * yScale * angle.Y;
745 newVert.Z = z;
746
747 hollowCoords.Add(newVert);
748 if (this.calcVertexNormals)
749 {
750 if (hollowSides < 5)
751 hollowNormals.Add(hollowAngles.normals[i].Invert());
752 else
753 hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f));
754
755 hollowUs.Add(angle.angle * hollow);
756 }
757 }
758 }
759
760 int index = 0;
761 int numAngles = angles.angles.Count;
762
763 for (int i = 0; i < numAngles; i++)
764 {
765 angle = angles.angles[i];
766 newVert.X = angle.X * xScale;
767 newVert.Y = angle.Y * yScale;
768 newVert.Z = z;
769 this.coords.Add(newVert);
770 if (this.calcVertexNormals)
771 {
772 this.outerCoordIndices.Add(this.coords.Count - 1);
773
774 if (sides < 5)
775 {
776 this.vertexNormals.Add(angles.normals[i]);
777 float u = angle.angle;
778 this.us.Add(u);
779 }
780 else
781 {
782 this.vertexNormals.Add(new Coord(angle.X, angle.Y, 0.0f));
783 this.us.Add(angle.angle);
784 }
785 }
786
787 if (hasHollow)
788 {
789 if (hollowSides == sides)
790 {
791 newVert.X *= hollow;
792 newVert.Y *= hollow;
793 newVert.Z = z;
794 hollowCoords.Add(newVert);
795 if (this.calcVertexNormals)
796 {
797 if (sides < 5)
798 {
799 hollowNormals.Add(angles.normals[i].Invert());
800 }
801
802 else
803 hollowNormals.Add(new Coord(-angle.X, -angle.Y, 0.0f));
804
805 hollowUs.Add(angle.angle * hollow);
806 }
807 }
808 }
809 else if (!simpleFace && createFaces && angle.angle > 0.0001f)
810 {
811 Face newFace = new Face();
812 newFace.v1 = 0;
813 newFace.v2 = index;
814 newFace.v3 = index + 1;
815
816 this.faces.Add(newFace);
817 }
818 index += 1;
819 }
820
821 if (hasHollow)
822 {
823 hollowCoords.Reverse();
824 if (this.calcVertexNormals)
825 {
826 hollowNormals.Reverse();
827 hollowUs.Reverse();
828 }
829
830 if (createFaces)
831 {
832 //int numOuterVerts = this.coords.Count;
833 //numOuterVerts = this.coords.Count;
834 //int numHollowVerts = hollowCoords.Count;
835 int numTotalVerts = this.numOuterVerts + this.numHollowVerts;
836
837 if (this.numOuterVerts == this.numHollowVerts)
838 {
839 Face newFace = new Face();
840
841 for (int coordIndex = 0; coordIndex < this.numOuterVerts - 1; coordIndex++)
842 {
843 newFace.v1 = coordIndex;
844 newFace.v2 = coordIndex + 1;
845 newFace.v3 = numTotalVerts - coordIndex - 1;
846 this.faces.Add(newFace);
847
848 newFace.v1 = coordIndex + 1;
849 newFace.v2 = numTotalVerts - coordIndex - 2;
850 newFace.v3 = numTotalVerts - coordIndex - 1;
851 this.faces.Add(newFace);
852 }
853 }
854 else
855 {
856 if (this.numOuterVerts < this.numHollowVerts)
857 {
858 Face newFace = new Face();
859 int j = 0; // j is the index for outer vertices
860 int maxJ = this.numOuterVerts - 1;
861 for (int i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices
862 {
863 if (j < maxJ)
864 if (angles.angles[j + 1].angle - hollowAngles.angles[i].angle < hollowAngles.angles[i].angle - angles.angles[j].angle + 0.000001f)
865 {
866 newFace.v1 = numTotalVerts - i - 1;
867 newFace.v2 = j;
868 newFace.v3 = j + 1;
869
870 this.faces.Add(newFace);
871 j += 1;
872 }
873
874 newFace.v1 = j;
875 newFace.v2 = numTotalVerts - i - 2;
876 newFace.v3 = numTotalVerts - i - 1;
877
878 this.faces.Add(newFace);
879 }
880 }
881 else // numHollowVerts < numOuterVerts
882 {
883 Face newFace = new Face();
884 int j = 0; // j is the index for inner vertices
885 int maxJ = this.numHollowVerts - 1;
886 for (int i = 0; i < this.numOuterVerts; i++)
887 {
888 if (j < maxJ)
889 if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f)
890 {
891 newFace.v1 = i;
892 newFace.v2 = numTotalVerts - j - 2;
893 newFace.v3 = numTotalVerts - j - 1;
894
895 this.faces.Add(newFace);
896 j += 1;
897 }
898
899 newFace.v1 = numTotalVerts - j - 1;
900 newFace.v2 = i;
901 newFace.v3 = i + 1;
902
903 this.faces.Add(newFace);
904 }
905 }
906 }
907 }
908
909 if (calcVertexNormals)
910 {
911 foreach (Coord hc in hollowCoords)
912 {
913 this.coords.Add(hc);
914 hollowCoordIndices.Add(this.coords.Count - 1);
915 }
916 }
917 else
918 this.coords.AddRange(hollowCoords);
919
920 if (this.calcVertexNormals)
921 {
922 this.vertexNormals.AddRange(hollowNormals);
923 this.us.AddRange(hollowUs);
924
925 }
926 }
927
928 if (simpleFace && createFaces)
929 {
930 if (sides == 3)
931 this.faces.Add(new Face(0, 1, 2));
932 else if (sides == 4)
933 {
934 this.faces.Add(new Face(0, 1, 2));
935 this.faces.Add(new Face(0, 2, 3));
936 }
937 }
938
939 if (calcVertexNormals && hasProfileCut)
940 {
941 int lastOuterVertIndex = this.numOuterVerts - 1;
942
943 if (hasHollow)
944 {
945 this.cut1CoordIndices.Add(0);
946 this.cut1CoordIndices.Add(this.coords.Count - 1);
947
948 this.cut2CoordIndices.Add(lastOuterVertIndex + 1);
949 this.cut2CoordIndices.Add(lastOuterVertIndex);
950
951 this.cutNormal1.X = this.coords[0].Y - this.coords[this.coords.Count - 1].Y;
952 this.cutNormal1.Y = -(this.coords[0].X - this.coords[this.coords.Count - 1].X);
953
954 this.cutNormal2.X = this.coords[lastOuterVertIndex + 1].Y - this.coords[lastOuterVertIndex].Y;
955 this.cutNormal2.Y = -(this.coords[lastOuterVertIndex + 1].X - this.coords[lastOuterVertIndex].X);
956 }
957
958 else
959 {
960 this.cut1CoordIndices.Add(0);
961 this.cut1CoordIndices.Add(1);
962
963 this.cut2CoordIndices.Add(lastOuterVertIndex);
964 this.cut2CoordIndices.Add(0);
965
966 this.cutNormal1.X = this.vertexNormals[1].Y;
967 this.cutNormal1.Y = -this.vertexNormals[1].X;
968
969 this.cutNormal2.X = -this.vertexNormals[this.vertexNormals.Count - 2].Y;
970 this.cutNormal2.Y = this.vertexNormals[this.vertexNormals.Count - 2].X;
971
972 }
973 this.cutNormal1.Normalize();
974 this.cutNormal2.Normalize();
975 }
976
977 this.MakeFaceUVs();
978
979 hollowCoords = null;
980 hollowNormals = null;
981 hollowUs = null;
982
983 if (calcVertexNormals)
984 { // calculate prim face numbers
985
986 // face number order is top, outer, hollow, bottom, start cut, end cut
987 // I know it's ugly but so is the whole concept of prim face numbers
988
989 int faceNum = 1; // start with outer faces
990 this.outerFaceNumber = faceNum;
991
992 int startVert = hasProfileCut && !hasHollow ? 1 : 0;
993 if (startVert > 0)
994 this.faceNumbers.Add(-1);
995 for (int i = 0; i < this.numOuterVerts - 1; i++)
996 //this.faceNumbers.Add(sides < 5 ? faceNum++ : faceNum);
997 this.faceNumbers.Add(sides < 5 && i < sides ? faceNum++ : faceNum);
998
999 //if (!hasHollow && !hasProfileCut)
1000 // this.bottomFaceNumber = faceNum++;
1001
1002 this.faceNumbers.Add(hasProfileCut ? -1 : faceNum++);
1003
1004 if (sides > 4 && (hasHollow || hasProfileCut))
1005 faceNum++;
1006
1007 if (sides < 5 && (hasHollow || hasProfileCut) && this.numOuterVerts < sides)
1008 faceNum++;
1009
1010 if (hasHollow)
1011 {
1012 for (int i = 0; i < this.numHollowVerts; i++)
1013 this.faceNumbers.Add(faceNum);
1014
1015 this.hollowFaceNumber = faceNum++;
1016 }
1017 //if (hasProfileCut || hasHollow)
1018 // this.bottomFaceNumber = faceNum++;
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 internal void MakeFaceUVs()
1034 {
1035 this.faceUVs = new List<UVCoord>();
1036 foreach (Coord c in this.coords)
1037 this.faceUVs.Add(new UVCoord(0.5f + c.X, 0.5f - c.Y));
1038 }
1039
1040 internal Profile Copy()
1041 {
1042 return this.Copy(true);
1043 }
1044
1045 internal 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 internal void AddPos(Coord v)
1075 {
1076 this.AddPos(v.X, v.Y, v.Z);
1077 }
1078
1079 internal 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 internal 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 internal 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 internal 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 internal 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 internal 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 internal 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 (pathType == PathType.Linear || pathType == PathType.Flexible)
1265 {
1266 int step = 0;
1267
1268 float length = this.pathCutEnd - this.pathCutBegin;
1269 float twistTotal = twistEnd - twistBegin;
1270 float twistTotalAbs = Math.Abs(twistTotal);
1271 if (twistTotalAbs > 0.01f)
1272 steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number
1273
1274 float start = -0.5f;
1275 float stepSize = length / (float)steps;
1276 float percentOfPathMultiplier = stepSize;
1277 float xOffset = 0.0f;
1278 float yOffset = 0.0f;
1279 float zOffset = start;
1280 float xOffsetStepIncrement = this.topShearX / steps;
1281 float yOffsetStepIncrement = this.topShearY / steps;
1282
1283 float percentOfPath = this.pathCutBegin;
1284 zOffset += percentOfPath;
1285
1286 // sanity checks
1287
1288 bool done = false;
1289
1290 while (!done)
1291 {
1292 PathNode newNode = new PathNode();
1293
1294 newNode.xScale = 1.0f;
1295 if (this.taperX == 0.0f)
1296 newNode.xScale = 1.0f;
1297 else if (this.taperX > 0.0f)
1298 newNode.xScale = 1.0f - percentOfPath * this.taperX;
1299 else newNode.xScale = 1.0f + (1.0f - percentOfPath) * this.taperX;
1300
1301 newNode.yScale = 1.0f;
1302 if (this.taperY == 0.0f)
1303 newNode.yScale = 1.0f;
1304 else if (this.taperY > 0.0f)
1305 newNode.yScale = 1.0f - percentOfPath * this.taperY;
1306 else newNode.yScale = 1.0f + (1.0f - percentOfPath) * this.taperY;
1307
1308 float twist = twistBegin + twistTotal * percentOfPath;
1309
1310 newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
1311 newNode.position = new Coord(xOffset, yOffset, zOffset);
1312 newNode.percentOfPath = percentOfPath;
1313
1314 pathNodes.Add(newNode);
1315
1316 if (step < steps)
1317 {
1318 step += 1;
1319 percentOfPath += percentOfPathMultiplier;
1320 xOffset += xOffsetStepIncrement;
1321 yOffset += yOffsetStepIncrement;
1322 zOffset += stepSize;
1323 if (percentOfPath > this.pathCutEnd)
1324 done = true;
1325 }
1326 else done = true;
1327 }
1328 } // end of linear path code
1329
1330 else // pathType == Circular
1331 {
1332 float twistTotal = twistEnd - twistBegin;
1333
1334 // if the profile has a lot of twist, add more layers otherwise the layers may overlap
1335 // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't
1336 // accurately match the viewer
1337 float twistTotalAbs = Math.Abs(twistTotal);
1338 if (twistTotalAbs > 0.01f)
1339 {
1340 if (twistTotalAbs > Math.PI * 1.5f)
1341 steps *= 2;
1342 if (twistTotalAbs > Math.PI * 3.0f)
1343 steps *= 2;
1344 }
1345
1346 float yPathScale = this.holeSizeY * 0.5f;
1347 float pathLength = this.pathCutEnd - this.pathCutBegin;
1348 float totalSkew = this.skew * 2.0f * pathLength;
1349 float skewStart = this.pathCutBegin * 2.0f * this.skew - this.skew;
1350 float xOffsetTopShearXFactor = this.topShearX * (0.25f + 0.5f * (0.5f - this.holeSizeY));
1351 float yShearCompensation = 1.0f + Math.Abs(this.topShearY) * 0.25f;
1352
1353 // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end
1354 // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used
1355 // to calculate the sine for generating the path radius appears to approximate it's effects there
1356 // too, but there are some subtle differences in the radius which are noticeable as the prim size
1357 // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on
1358 // the meshes generated with this technique appear nearly identical in shape to the same prims when
1359 // displayed by the viewer.
1360
1361 float startAngle = (twoPi * this.pathCutBegin * this.revolutions) - this.topShearY * 0.9f;
1362 float endAngle = (twoPi * this.pathCutEnd * this.revolutions) - this.topShearY * 0.9f;
1363 float stepSize = twoPi / this.stepsPerRevolution;
1364
1365 int step = (int)(startAngle / stepSize);
1366 float angle = startAngle;
1367
1368 bool done = false;
1369 while (!done) // loop through the length of the path and add the layers
1370 {
1371 PathNode newNode = new PathNode();
1372
1373 float xProfileScale = (1.0f - Math.Abs(this.skew)) * this.holeSizeX;
1374 float yProfileScale = this.holeSizeY;
1375
1376 float percentOfPath = angle / (twoPi * this.revolutions);
1377 float percentOfAngles = (angle - startAngle) / (endAngle - startAngle);
1378
1379 if (this.taperX > 0.01f)
1380 xProfileScale *= 1.0f - percentOfPath * this.taperX;
1381 else if (this.taperX < -0.01f)
1382 xProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperX;
1383
1384 if (this.taperY > 0.01f)
1385 yProfileScale *= 1.0f - percentOfPath * this.taperY;
1386 else if (this.taperY < -0.01f)
1387 yProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperY;
1388
1389 newNode.xScale = xProfileScale;
1390 newNode.yScale = yProfileScale;
1391
1392 float radiusScale = 1.0f;
1393 if (this.radius > 0.001f)
1394 radiusScale = 1.0f - this.radius * percentOfPath;
1395 else if (this.radius < 0.001f)
1396 radiusScale = 1.0f + this.radius * (1.0f - percentOfPath);
1397
1398 float twist = twistBegin + twistTotal * percentOfPath;
1399
1400 float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles);
1401 xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor;
1402
1403 float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale;
1404
1405 float zOffset = (float)Math.Sin(angle + this.topShearY) * (0.5f - yPathScale) * radiusScale;
1406
1407 newNode.position = new Coord(xOffset, yOffset, zOffset);
1408
1409 // now orient the rotation of the profile layer relative to it's position on the path
1410 // adding taperY to the angle used to generate the quat appears to approximate the viewer
1411
1412 newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + this.topShearY);
1413
1414 // next apply twist rotation to the profile layer
1415 if (twistTotal != 0.0f || twistBegin != 0.0f)
1416 newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist);
1417
1418 newNode.percentOfPath = percentOfPath;
1419
1420 pathNodes.Add(newNode);
1421
1422 // calculate terms for next iteration
1423 // calculate the angle for the next iteration of the loop
1424
1425 if (angle >= endAngle - 0.01)
1426 done = true;
1427 else
1428 {
1429 step += 1;
1430 angle = stepSize * step;
1431 if (angle > endAngle)
1432 angle = endAngle;
1433 }
1434 }
1435 }
1436 }
1437 }
1438
1439 public class PrimMesh
1440 {
1441 public string errorMessage = "";
1442 private const float twoPi = 2.0f * (float)Math.PI;
1443
1444 public List<Coord> coords;
1445 public List<Coord> normals;
1446 public List<Face> faces;
1447
1448 public List<ViewerFace> viewerFaces;
1449
1450 private int sides = 4;
1451 private int hollowSides = 4;
1452 private float profileStart = 0.0f;
1453 private float profileEnd = 1.0f;
1454 private float hollow = 0.0f;
1455 public int twistBegin = 0;
1456 public int twistEnd = 0;
1457 public float topShearX = 0.0f;
1458 public float topShearY = 0.0f;
1459 public float pathCutBegin = 0.0f;
1460 public float pathCutEnd = 1.0f;
1461 public float dimpleBegin = 0.0f;
1462 public float dimpleEnd = 1.0f;
1463 public float skew = 0.0f;
1464 public float holeSizeX = 1.0f; // called pathScaleX in pbs
1465 public float holeSizeY = 0.25f;
1466 public float taperX = 0.0f;
1467 public float taperY = 0.0f;
1468 public float radius = 0.0f;
1469 public float revolutions = 1.0f;
1470 public int stepsPerRevolution = 24;
1471
1472 private int profileOuterFaceNumber = -1;
1473 private int profileHollowFaceNumber = -1;
1474
1475 private bool hasProfileCut = false;
1476 private bool hasHollow = false;
1477 public bool calcVertexNormals = false;
1478 private bool normalsProcessed = false;
1479 public bool viewerMode = false;
1480 public bool sphereMode = false;
1481
1482 public int numPrimFaces = 0;
1483
1484 /// <summary>
1485 /// Human readable string representation of the parameters used to create a mesh.
1486 /// </summary>
1487 /// <returns></returns>
1488 public string ParamsToDisplayString()
1489 {
1490 string s = "";
1491 s += "sides..................: " + this.sides.ToString();
1492 s += "\nhollowSides..........: " + this.hollowSides.ToString();
1493 s += "\nprofileStart.........: " + this.profileStart.ToString();
1494 s += "\nprofileEnd...........: " + this.profileEnd.ToString();
1495 s += "\nhollow...............: " + this.hollow.ToString();
1496 s += "\ntwistBegin...........: " + this.twistBegin.ToString();
1497 s += "\ntwistEnd.............: " + this.twistEnd.ToString();
1498 s += "\ntopShearX............: " + this.topShearX.ToString();
1499 s += "\ntopShearY............: " + this.topShearY.ToString();
1500 s += "\npathCutBegin.........: " + this.pathCutBegin.ToString();
1501 s += "\npathCutEnd...........: " + this.pathCutEnd.ToString();
1502 s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString();
1503 s += "\ndimpleEnd............: " + this.dimpleEnd.ToString();
1504 s += "\nskew.................: " + this.skew.ToString();
1505 s += "\nholeSizeX............: " + this.holeSizeX.ToString();
1506 s += "\nholeSizeY............: " + this.holeSizeY.ToString();
1507 s += "\ntaperX...............: " + this.taperX.ToString();
1508 s += "\ntaperY...............: " + this.taperY.ToString();
1509 s += "\nradius...............: " + this.radius.ToString();
1510 s += "\nrevolutions..........: " + this.revolutions.ToString();
1511 s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString();
1512 s += "\nsphereMode...........: " + this.sphereMode.ToString();
1513 s += "\nhasProfileCut........: " + this.hasProfileCut.ToString();
1514 s += "\nhasHollow............: " + this.hasHollow.ToString();
1515 s += "\nviewerMode...........: " + this.viewerMode.ToString();
1516
1517 return s;
1518 }
1519
1520 public int ProfileOuterFaceNumber
1521 {
1522 get { return profileOuterFaceNumber; }
1523 }
1524
1525 public int ProfileHollowFaceNumber
1526 {
1527 get { return profileHollowFaceNumber; }
1528 }
1529
1530 public bool HasProfileCut
1531 {
1532 get { return hasProfileCut; }
1533 }
1534
1535 public bool HasHollow
1536 {
1537 get { return hasHollow; }
1538 }
1539
1540
1541 /// <summary>
1542 /// Constructs a PrimMesh object and creates the profile for extrusion.
1543 /// </summary>
1544 /// <param name="sides"></param>
1545 /// <param name="profileStart"></param>
1546 /// <param name="profileEnd"></param>
1547 /// <param name="hollow"></param>
1548 /// <param name="hollowSides"></param>
1549 public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides)
1550 {
1551 this.coords = new List<Coord>();
1552 this.faces = new List<Face>();
1553
1554 this.sides = sides;
1555 this.profileStart = profileStart;
1556 this.profileEnd = profileEnd;
1557 this.hollow = hollow;
1558 this.hollowSides = hollowSides;
1559
1560 if (sides < 3)
1561 this.sides = 3;
1562 if (hollowSides < 3)
1563 this.hollowSides = 3;
1564 if (profileStart < 0.0f)
1565 this.profileStart = 0.0f;
1566 if (profileEnd > 1.0f)
1567 this.profileEnd = 1.0f;
1568 if (profileEnd < 0.02f)
1569 this.profileEnd = 0.02f;
1570 if (profileStart >= profileEnd)
1571 this.profileStart = profileEnd - 0.02f;
1572 if (hollow > 0.99f)
1573 this.hollow = 0.99f;
1574 if (hollow < 0.0f)
1575 this.hollow = 0.0f;
1576
1577 //if (sphereMode)
1578 // this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f;
1579 //else
1580 // //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f);
1581 // this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f;
1582 //this.hasHollow = (this.hollow > 0.001f);
1583 }
1584
1585 /// <summary>
1586 /// Extrudes a profile along a path.
1587 /// </summary>
1588 public void Extrude(PathType pathType)
1589 {
1590 bool needEndFaces = false;
1591
1592 this.coords = new List<Coord>();
1593 this.faces = new List<Face>();
1594
1595 if (this.viewerMode)
1596 {
1597 this.viewerFaces = new List<ViewerFace>();
1598 this.calcVertexNormals = true;
1599 }
1600
1601 if (this.calcVertexNormals)
1602 this.normals = new List<Coord>();
1603
1604 int steps = 1;
1605
1606 float length = this.pathCutEnd - this.pathCutBegin;
1607 normalsProcessed = false;
1608
1609 if (this.viewerMode && this.sides == 3)
1610 {
1611 // prisms don't taper well so add some vertical resolution
1612 // other prims may benefit from this but just do prisms for now
1613 if (Math.Abs(this.taperX) > 0.01 || Math.Abs(this.taperY) > 0.01)
1614 steps = (int)(steps * 4.5 * length);
1615 }
1616
1617 if (sphereMode)
1618 this.hasProfileCut = this.profileEnd - this.profileStart < 0.4999f;
1619 else
1620 //this.hasProfileCut = (this.profileStart > 0.0f || this.profileEnd < 1.0f);
1621 this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f;
1622 this.hasHollow = (this.hollow > 0.001f);
1623
1624 float twistBegin = this.twistBegin / 360.0f * twoPi;
1625 float twistEnd = this.twistEnd / 360.0f * twoPi;
1626 float twistTotal = twistEnd - twistBegin;
1627 float twistTotalAbs = Math.Abs(twistTotal);
1628 if (twistTotalAbs > 0.01f)
1629 steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number
1630
1631 float hollow = this.hollow;
1632
1633 // sanity checks
1634 float initialProfileRot = 0.0f;
1635 if (pathType == PathType.Circular)
1636 {
1637 if (this.sides == 3)
1638 {
1639 initialProfileRot = (float)Math.PI;
1640 if (this.hollowSides == 4)
1641 {
1642 if (hollow > 0.7f)
1643 hollow = 0.7f;
1644 hollow *= 0.707f;
1645 }
1646 else hollow *= 0.5f;
1647 }
1648 else if (this.sides == 4)
1649 {
1650 initialProfileRot = 0.25f * (float)Math.PI;
1651 if (this.hollowSides != 4)
1652 hollow *= 0.707f;
1653 }
1654 else if (this.sides > 4)
1655 {
1656 initialProfileRot = (float)Math.PI;
1657 if (this.hollowSides == 4)
1658 {
1659 if (hollow > 0.7f)
1660 hollow = 0.7f;
1661 hollow /= 0.7f;
1662 }
1663 }
1664 }
1665 else
1666 {
1667 if (this.sides == 3)
1668 {
1669 if (this.hollowSides == 4)
1670 {
1671 if (hollow > 0.7f)
1672 hollow = 0.7f;
1673 hollow *= 0.707f;
1674 }
1675 else hollow *= 0.5f;
1676 }
1677 else if (this.sides == 4)
1678 {
1679 initialProfileRot = 1.25f * (float)Math.PI;
1680 if (this.hollowSides != 4)
1681 hollow *= 0.707f;
1682 }
1683 else if (this.sides == 24 && this.hollowSides == 4)
1684 hollow *= 1.414f;
1685 }
1686
1687 Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, true, calcVertexNormals);
1688 this.errorMessage = profile.errorMessage;
1689
1690 this.numPrimFaces = profile.numPrimFaces;
1691
1692 //profileOuterFaceNumber = profile.faceNumbers[0];
1693 //if (!needEndFaces)
1694 // profileOuterFaceNumber--;
1695 //profileOuterFaceNumber = needEndFaces ? 1 : 0;
1696
1697
1698 //if (hasHollow)
1699 //{
1700 // if (needEndFaces)
1701 // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts + 1];
1702 // else
1703 // profileHollowFaceNumber = profile.faceNumbers[profile.numOuterVerts] - 1;
1704 //}
1705
1706
1707 profileOuterFaceNumber = profile.outerFaceNumber;
1708 if (!needEndFaces)
1709 profileOuterFaceNumber--;
1710
1711 if (hasHollow)
1712 {
1713 profileHollowFaceNumber = profile.hollowFaceNumber;
1714 if (!needEndFaces)
1715 profileHollowFaceNumber--;
1716 }
1717
1718 int cut1Vert = -1;
1719 int cut2Vert = -1;
1720 if (hasProfileCut)
1721 {
1722 cut1Vert = hasHollow ? profile.coords.Count - 1 : 0;
1723 cut2Vert = hasHollow ? profile.numOuterVerts - 1 : profile.numOuterVerts;
1724 }
1725
1726 if (initialProfileRot != 0.0f)
1727 {
1728 profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot));
1729 if (viewerMode)
1730 profile.MakeFaceUVs();
1731 }
1732
1733 Coord lastCutNormal1 = new Coord();
1734 Coord lastCutNormal2 = new Coord();
1735 float lastV = 1.0f;
1736
1737 Path path = new Path();
1738 path.twistBegin = twistBegin;
1739 path.twistEnd = twistEnd;
1740 path.topShearX = topShearX;
1741 path.topShearY = topShearY;
1742 path.pathCutBegin = pathCutBegin;
1743 path.pathCutEnd = pathCutEnd;
1744 path.dimpleBegin = dimpleBegin;
1745 path.dimpleEnd = dimpleEnd;
1746 path.skew = skew;
1747 path.holeSizeX = holeSizeX;
1748 path.holeSizeY = holeSizeY;
1749 path.taperX = taperX;
1750 path.taperY = taperY;
1751 path.radius = radius;
1752 path.revolutions = revolutions;
1753 path.stepsPerRevolution = stepsPerRevolution;
1754
1755 path.Create(pathType, steps);
1756
1757
1758 if (pathType == PathType.Circular)
1759 {
1760 needEndFaces = false;
1761 if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f)
1762 needEndFaces = true;
1763 else if (this.taperX != 0.0f || this.taperY != 0.0f)
1764 needEndFaces = true;
1765 else if (this.skew != 0.0f)
1766 needEndFaces = true;
1767 else if (twistTotal != 0.0f)
1768 needEndFaces = true;
1769 else if (this.radius != 0.0f)
1770 needEndFaces = true;
1771 }
1772 else needEndFaces = true;
1773
1774 for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
1775 {
1776 PathNode node = path.pathNodes[nodeIndex];
1777 Profile newLayer = profile.Copy();
1778 newLayer.Scale(node.xScale, node.yScale);
1779
1780 newLayer.AddRot(node.rotation);
1781 newLayer.AddPos(node.position);
1782
1783 if (needEndFaces && nodeIndex == 0)
1784 {
1785 newLayer.FlipNormals();
1786
1787 // add the top faces to the viewerFaces list here
1788 if (this.viewerMode)
1789 {
1790 Coord faceNormal = newLayer.faceNormal;
1791 ViewerFace newViewerFace = new ViewerFace(profile.bottomFaceNumber);
1792 int numFaces = newLayer.faces.Count;
1793 List<Face> faces = newLayer.faces;
1794
1795 for (int i = 0; i < numFaces; i++)
1796 {
1797 Face face = faces[i];
1798 newViewerFace.v1 = newLayer.coords[face.v1];
1799 newViewerFace.v2 = newLayer.coords[face.v2];
1800 newViewerFace.v3 = newLayer.coords[face.v3];
1801
1802 newViewerFace.coordIndex1 = face.v1;
1803 newViewerFace.coordIndex2 = face.v2;
1804 newViewerFace.coordIndex3 = face.v3;
1805
1806 newViewerFace.n1 = faceNormal;
1807 newViewerFace.n2 = faceNormal;
1808 newViewerFace.n3 = faceNormal;
1809
1810 newViewerFace.uv1 = newLayer.faceUVs[face.v1];
1811 newViewerFace.uv2 = newLayer.faceUVs[face.v2];
1812 newViewerFace.uv3 = newLayer.faceUVs[face.v3];
1813
1814 this.viewerFaces.Add(newViewerFace);
1815 }
1816 }
1817 } // if (nodeIndex == 0)
1818
1819 // append this layer
1820
1821 int coordsLen = this.coords.Count;
1822 newLayer.AddValue2FaceVertexIndices(coordsLen);
1823
1824 this.coords.AddRange(newLayer.coords);
1825
1826 if (this.calcVertexNormals)
1827 {
1828 newLayer.AddValue2FaceNormalIndices(this.normals.Count);
1829 this.normals.AddRange(newLayer.vertexNormals);
1830 }
1831
1832 if (node.percentOfPath < this.pathCutBegin + 0.01f || node.percentOfPath > this.pathCutEnd - 0.01f)
1833 this.faces.AddRange(newLayer.faces);
1834
1835 // fill faces between layers
1836
1837 int numVerts = newLayer.coords.Count;
1838 Face newFace = new Face();
1839
1840 if (nodeIndex > 0)
1841 {
1842 int startVert = coordsLen + 1;
1843 int endVert = this.coords.Count;
1844
1845 if (sides < 5 || this.hasProfileCut || this.hasHollow)
1846 startVert--;
1847
1848 for (int i = startVert; i < endVert; i++)
1849 {
1850 int iNext = i + 1;
1851 if (i == endVert - 1)
1852 iNext = startVert;
1853
1854 int whichVert = i - startVert;
1855
1856 newFace.v1 = i;
1857 newFace.v2 = i - numVerts;
1858 newFace.v3 = iNext - numVerts;
1859 this.faces.Add(newFace);
1860
1861 newFace.v2 = iNext - numVerts;
1862 newFace.v3 = iNext;
1863 this.faces.Add(newFace);
1864
1865 if (this.viewerMode)
1866 {
1867 // add the side faces to the list of viewerFaces here
1868
1869 int primFaceNum = profile.faceNumbers[whichVert];
1870 if (!needEndFaces)
1871 primFaceNum -= 1;
1872
1873 ViewerFace newViewerFace1 = new ViewerFace(primFaceNum);
1874 ViewerFace newViewerFace2 = new ViewerFace(primFaceNum);
1875
1876 float u1 = newLayer.us[whichVert];
1877 float u2 = 1.0f;
1878 if (whichVert < newLayer.us.Count - 1)
1879 u2 = newLayer.us[whichVert + 1];
1880
1881 if (whichVert == cut1Vert || whichVert == cut2Vert)
1882 {
1883 u1 = 0.0f;
1884 u2 = 1.0f;
1885 }
1886 else if (sides < 5)
1887 {
1888 if (whichVert < profile.numOuterVerts)
1889 { // boxes and prisms have one texture face per side of the prim, so the U values have to be scaled
1890 // to reflect the entire texture width
1891 u1 *= sides;
1892 u2 *= sides;
1893 u2 -= (int)u1;
1894 u1 -= (int)u1;
1895 if (u2 < 0.1f)
1896 u2 = 1.0f;
1897 //this.profileOuterFaceNumber = primFaceNum;
1898 }
1899 else if (whichVert > profile.coords.Count - profile.numHollowVerts - 1)
1900 {
1901 u1 *= 2.0f;
1902 u2 *= 2.0f;
1903 //this.profileHollowFaceNumber = primFaceNum;
1904 }
1905 }
1906
1907 newViewerFace1.uv1.U = u1;
1908 newViewerFace1.uv2.U = u1;
1909 newViewerFace1.uv3.U = u2;
1910
1911 newViewerFace1.uv1.V = 1.0f - node.percentOfPath;
1912 newViewerFace1.uv2.V = lastV;
1913 newViewerFace1.uv3.V = lastV;
1914
1915 newViewerFace2.uv1.U = u1;
1916 newViewerFace2.uv2.U = u2;
1917 newViewerFace2.uv3.U = u2;
1918
1919 newViewerFace2.uv1.V = 1.0f - node.percentOfPath;
1920 newViewerFace2.uv2.V = lastV;
1921 newViewerFace2.uv3.V = 1.0f - node.percentOfPath;
1922
1923 newViewerFace1.v1 = this.coords[i];
1924 newViewerFace1.v2 = this.coords[i - numVerts];
1925 newViewerFace1.v3 = this.coords[iNext - numVerts];
1926
1927 newViewerFace2.v1 = this.coords[i];
1928 newViewerFace2.v2 = this.coords[iNext - numVerts];
1929 newViewerFace2.v3 = this.coords[iNext];
1930
1931 newViewerFace1.coordIndex1 = i;
1932 newViewerFace1.coordIndex2 = i - numVerts;
1933 newViewerFace1.coordIndex3 = iNext - numVerts;
1934
1935 newViewerFace2.coordIndex1 = i;
1936 newViewerFace2.coordIndex2 = iNext - numVerts;
1937 newViewerFace2.coordIndex3 = iNext;
1938
1939 // profile cut faces
1940 if (whichVert == cut1Vert)
1941 {
1942 newViewerFace1.n1 = newLayer.cutNormal1;
1943 newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal1;
1944
1945 newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal1;
1946 newViewerFace2.n2 = lastCutNormal1;
1947 }
1948 else if (whichVert == cut2Vert)
1949 {
1950 newViewerFace1.n1 = newLayer.cutNormal2;
1951 newViewerFace1.n2 = newViewerFace1.n3 = lastCutNormal2;
1952
1953 newViewerFace2.n1 = newViewerFace2.n3 = newLayer.cutNormal2;
1954 newViewerFace2.n2 = lastCutNormal2;
1955 }
1956
1957 else // outer and hollow faces
1958 {
1959 if ((sides < 5 && whichVert < newLayer.numOuterVerts) || (hollowSides < 5 && whichVert >= newLayer.numOuterVerts))
1960 { // looks terrible when path is twisted... need vertex normals here
1961 newViewerFace1.CalcSurfaceNormal();
1962 newViewerFace2.CalcSurfaceNormal();
1963 }
1964 else
1965 {
1966 newViewerFace1.n1 = this.normals[i];
1967 newViewerFace1.n2 = this.normals[i - numVerts];
1968 newViewerFace1.n3 = this.normals[iNext - numVerts];
1969
1970 newViewerFace2.n1 = this.normals[i];
1971 newViewerFace2.n2 = this.normals[iNext - numVerts];
1972 newViewerFace2.n3 = this.normals[iNext];
1973 }
1974 }
1975
1976 this.viewerFaces.Add(newViewerFace1);
1977 this.viewerFaces.Add(newViewerFace2);
1978
1979 }
1980 }
1981 }
1982
1983 lastCutNormal1 = newLayer.cutNormal1;
1984 lastCutNormal2 = newLayer.cutNormal2;
1985 lastV = 1.0f - node.percentOfPath;
1986
1987 if (needEndFaces && nodeIndex == path.pathNodes.Count - 1 && viewerMode)
1988 {
1989 // add the top faces to the viewerFaces list here
1990 Coord faceNormal = newLayer.faceNormal;
1991 ViewerFace newViewerFace = new ViewerFace();
1992 newViewerFace.primFaceNumber = 0;
1993 int numFaces = newLayer.faces.Count;
1994 List<Face> faces = newLayer.faces;
1995
1996 for (int i = 0; i < numFaces; i++)
1997 {
1998 Face face = faces[i];
1999 newViewerFace.v1 = newLayer.coords[face.v1 - coordsLen];
2000 newViewerFace.v2 = newLayer.coords[face.v2 - coordsLen];
2001 newViewerFace.v3 = newLayer.coords[face.v3 - coordsLen];
2002
2003 newViewerFace.coordIndex1 = face.v1 - coordsLen;
2004 newViewerFace.coordIndex2 = face.v2 - coordsLen;
2005 newViewerFace.coordIndex3 = face.v3 - coordsLen;
2006
2007 newViewerFace.n1 = faceNormal;
2008 newViewerFace.n2 = faceNormal;
2009 newViewerFace.n3 = faceNormal;
2010
2011 newViewerFace.uv1 = newLayer.faceUVs[face.v1 - coordsLen];
2012 newViewerFace.uv2 = newLayer.faceUVs[face.v2 - coordsLen];
2013 newViewerFace.uv3 = newLayer.faceUVs[face.v3 - coordsLen];
2014
2015 this.viewerFaces.Add(newViewerFace);
2016 }
2017 }
2018
2019
2020 } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++)
2021
2022 }
2023
2024
2025 /// <summary>
2026 /// DEPRICATED - use Extrude(PathType.Linear) instead
2027 /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism.
2028 /// </summary>
2029 ///
2030 public void ExtrudeLinear()
2031 {
2032 this.Extrude(PathType.Linear);
2033 }
2034
2035
2036 /// <summary>
2037 /// DEPRICATED - use Extrude(PathType.Circular) instead
2038 /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring.
2039 /// </summary>
2040 ///
2041 public void ExtrudeCircular()
2042 {
2043 this.Extrude(PathType.Circular);
2044 }
2045
2046
2047 private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3)
2048 {
2049 Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z);
2050 Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z);
2051
2052 Coord normal = Coord.Cross(edge1, edge2);
2053
2054 normal.Normalize();
2055
2056 return normal;
2057 }
2058
2059 private Coord SurfaceNormal(Face face)
2060 {
2061 return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]);
2062 }
2063
2064 /// <summary>
2065 /// Calculate the surface normal for a face in the list of faces
2066 /// </summary>
2067 /// <param name="faceIndex"></param>
2068 /// <returns></returns>
2069 public Coord SurfaceNormal(int faceIndex)
2070 {
2071 int numFaces = this.faces.Count;
2072 if (faceIndex < 0 || faceIndex >= numFaces)
2073 throw new Exception("faceIndex out of range");
2074
2075 return SurfaceNormal(this.faces[faceIndex]);
2076 }
2077
2078 /// <summary>
2079 /// Duplicates a PrimMesh object. All object properties are copied by value, including lists.
2080 /// </summary>
2081 /// <returns></returns>
2082 public PrimMesh Copy()
2083 {
2084 PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides);
2085 copy.twistBegin = this.twistBegin;
2086 copy.twistEnd = this.twistEnd;
2087 copy.topShearX = this.topShearX;
2088 copy.topShearY = this.topShearY;
2089 copy.pathCutBegin = this.pathCutBegin;
2090 copy.pathCutEnd = this.pathCutEnd;
2091 copy.dimpleBegin = this.dimpleBegin;
2092 copy.dimpleEnd = this.dimpleEnd;
2093 copy.skew = this.skew;
2094 copy.holeSizeX = this.holeSizeX;
2095 copy.holeSizeY = this.holeSizeY;
2096 copy.taperX = this.taperX;
2097 copy.taperY = this.taperY;
2098 copy.radius = this.radius;
2099 copy.revolutions = this.revolutions;
2100 copy.stepsPerRevolution = this.stepsPerRevolution;
2101 copy.calcVertexNormals = this.calcVertexNormals;
2102 copy.normalsProcessed = this.normalsProcessed;
2103 copy.viewerMode = this.viewerMode;
2104 copy.numPrimFaces = this.numPrimFaces;
2105 copy.errorMessage = this.errorMessage;
2106
2107 copy.coords = new List<Coord>(this.coords);
2108 copy.faces = new List<Face>(this.faces);
2109 copy.viewerFaces = new List<ViewerFace>(this.viewerFaces);
2110 copy.normals = new List<Coord>(this.normals);
2111
2112 return copy;
2113 }
2114
2115 /// <summary>
2116 /// Calculate surface normals for all of the faces in the list of faces in this mesh
2117 /// </summary>
2118 public void CalcNormals()
2119 {
2120 if (normalsProcessed)
2121 return;
2122
2123 normalsProcessed = true;
2124
2125 int numFaces = faces.Count;
2126
2127 if (!this.calcVertexNormals)
2128 this.normals = new List<Coord>();
2129
2130 for (int i = 0; i < numFaces; i++)
2131 {
2132 Face face = faces[i];
2133
2134 this.normals.Add(SurfaceNormal(i).Normalize());
2135
2136 int normIndex = normals.Count - 1;
2137 face.n1 = normIndex;
2138 face.n2 = normIndex;
2139 face.n3 = normIndex;
2140
2141 this.faces[i] = face;
2142 }
2143 }
2144
2145 /// <summary>
2146 /// Adds a value to each XYZ vertex coordinate in the mesh
2147 /// </summary>
2148 /// <param name="x"></param>
2149 /// <param name="y"></param>
2150 /// <param name="z"></param>
2151 public void AddPos(float x, float y, float z)
2152 {
2153 int i;
2154 int numVerts = this.coords.Count;
2155 Coord vert;
2156
2157 for (i = 0; i < numVerts; i++)
2158 {
2159 vert = this.coords[i];
2160 vert.X += x;
2161 vert.Y += y;
2162 vert.Z += z;
2163 this.coords[i] = vert;
2164 }
2165
2166 if (this.viewerFaces != null)
2167 {
2168 int numViewerFaces = this.viewerFaces.Count;
2169
2170 for (i = 0; i < numViewerFaces; i++)
2171 {
2172 ViewerFace v = this.viewerFaces[i];
2173 v.AddPos(x, y, z);
2174 this.viewerFaces[i] = v;
2175 }
2176 }
2177 }
2178
2179 /// <summary>
2180 /// Rotates the mesh
2181 /// </summary>
2182 /// <param name="q"></param>
2183 public void AddRot(Quat q)
2184 {
2185 int i;
2186 int numVerts = this.coords.Count;
2187
2188 for (i = 0; i < numVerts; i++)
2189 this.coords[i] *= q;
2190
2191 if (this.normals != null)
2192 {
2193 int numNormals = this.normals.Count;
2194 for (i = 0; i < numNormals; i++)
2195 this.normals[i] *= q;
2196 }
2197
2198 if (this.viewerFaces != null)
2199 {
2200 int numViewerFaces = this.viewerFaces.Count;
2201
2202 for (i = 0; i < numViewerFaces; i++)
2203 {
2204 ViewerFace v = this.viewerFaces[i];
2205 v.v1 *= q;
2206 v.v2 *= q;
2207 v.v3 *= q;
2208
2209 v.n1 *= q;
2210 v.n2 *= q;
2211 v.n3 *= q;
2212 this.viewerFaces[i] = v;
2213 }
2214 }
2215 }
2216
2217#if VERTEX_INDEXER
2218 public VertexIndexer GetVertexIndexer()
2219 {
2220 if (this.viewerMode && this.viewerFaces.Count > 0)
2221 return new VertexIndexer(this);
2222 return null;
2223 }
2224#endif
2225
2226 /// <summary>
2227 /// Scales the mesh
2228 /// </summary>
2229 /// <param name="x"></param>
2230 /// <param name="y"></param>
2231 /// <param name="z"></param>
2232 public void Scale(float x, float y, float z)
2233 {
2234 int i;
2235 int numVerts = this.coords.Count;
2236 //Coord vert;
2237
2238 Coord m = new Coord(x, y, z);
2239 for (i = 0; i < numVerts; i++)
2240 this.coords[i] *= m;
2241
2242 if (this.viewerFaces != null)
2243 {
2244 int numViewerFaces = this.viewerFaces.Count;
2245 for (i = 0; i < numViewerFaces; i++)
2246 {
2247 ViewerFace v = this.viewerFaces[i];
2248 v.v1 *= m;
2249 v.v2 *= m;
2250 v.v3 *= m;
2251 this.viewerFaces[i] = v;
2252 }
2253
2254 }
2255
2256 }
2257
2258 /// <summary>
2259 /// Dumps the mesh to a Blender compatible "Raw" format file
2260 /// </summary>
2261 /// <param name="path"></param>
2262 /// <param name="name"></param>
2263 /// <param name="title"></param>
2264 public void DumpRaw(String path, String name, String title)
2265 {
2266 if (path == null)
2267 return;
2268 String fileName = name + "_" + title + ".raw";
2269 String completePath = System.IO.Path.Combine(path, fileName);
2270 StreamWriter sw = new StreamWriter(completePath);
2271
2272 for (int i = 0; i < this.faces.Count; i++)
2273 {
2274 string s = this.coords[this.faces[i].v1].ToString();
2275 s += " " + this.coords[this.faces[i].v2].ToString();
2276 s += " " + this.coords[this.faces[i].v3].ToString();
2277
2278 sw.WriteLine(s);
2279 }
2280
2281 sw.Close();
2282 }
2283 }
2284}
diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs
new file mode 100644
index 0000000..b3d9cb6
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/SculptMap.cs
@@ -0,0 +1,197 @@
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
28// to build without references to System.Drawing, comment this out
29#define SYSTEM_DRAWING
30
31using System;
32using System.Collections.Generic;
33using System.Text;
34
35#if SYSTEM_DRAWING
36using System.Drawing;
37using System.Drawing.Imaging;
38
39namespace PrimMesher
40{
41 public class SculptMap
42 {
43 public int width;
44 public int height;
45 public byte[] redBytes;
46 public byte[] greenBytes;
47 public byte[] blueBytes;
48
49 public SculptMap()
50 {
51 }
52
53 public SculptMap(Bitmap bm, int lod)
54 {
55 int bmW = bm.Width;
56 int bmH = bm.Height;
57
58 if (bmW == 0 || bmH == 0)
59 throw new Exception("SculptMap: bitmap has no data");
60
61 int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image
62
63 bool smallMap = bmW * bmH <= numLodPixels;
64 bool needsScaling = false;
65
66 width = bmW;
67 height = bmH;
68 while (width * height > numLodPixels * 4)
69 {
70 width >>= 1;
71 height >>= 1;
72 needsScaling = true;
73 }
74
75 try
76 {
77 if (needsScaling)
78 bm = ScaleImage(bm, width, height);
79 }
80
81 catch (Exception e)
82 {
83 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
84 }
85
86 if (width * height > numLodPixels)
87 {
88 width >>= 1;
89 height >>= 1;
90 }
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
99 try
100 {
101 for (int y = 0; y <= height; y++)
102 {
103 for (int x = 0; x <= width; x++)
104 {
105 Color c;
106
107 if (smallMap)
108 c = bm.GetPixel(x < width ? x : x - 1,
109 y < height ? y : y - 1);
110 else
111 c = bm.GetPixel(x < width ? x * 2 : x * 2 - 1,
112 y < height ? y * 2 : y * 2 - 1);
113
114 redBytes[byteNdx] = c.R;
115 greenBytes[byteNdx] = c.G;
116 blueBytes[byteNdx] = c.B;
117
118 ++byteNdx;
119 }
120 }
121 }
122 catch (Exception e)
123 {
124 throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString());
125 }
126
127 width++;
128 height++;
129 }
130
131 public List<List<Coord>> ToRows(bool mirror)
132 {
133 int numRows = height;
134 int numCols = width;
135
136 List<List<Coord>> rows = new List<List<Coord>>(numRows);
137
138 float pixScale = 1.0f / 255;
139
140 int rowNdx, colNdx;
141 int smNdx = 0;
142
143
144 for (rowNdx = 0; rowNdx < numRows; rowNdx++)
145 {
146 List<Coord> row = new List<Coord>(numCols);
147 for (colNdx = 0; colNdx < numCols; colNdx++)
148 {
149
150 if (mirror)
151 row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
152 else
153 row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
154
155 ++smNdx;
156 }
157 rows.Add(row);
158 }
159 return rows;
160 }
161
162 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
163 {
164
165 Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
166
167 Color c;
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 }
185
186 sx += xscale;
187 }
188 sy += yscale;
189 }
190 srcImage.Dispose();
191 return scaledImage;
192 }
193
194 }
195
196 }
197#endif
diff --git a/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs
new file mode 100644
index 0000000..4a7f3ad
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitMeshing/SculptMesh.cs
@@ -0,0 +1,646 @@
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
28// to build without references to System.Drawing, comment this out
29#define SYSTEM_DRAWING
30
31using System;
32using System.Collections.Generic;
33using System.Text;
34using System.IO;
35
36#if SYSTEM_DRAWING
37using System.Drawing;
38using System.Drawing.Imaging;
39#endif
40
41namespace PrimMesher
42{
43
44 public class SculptMesh
45 {
46 public List<Coord> coords;
47 public List<Face> faces;
48
49 public List<ViewerFace> viewerFaces;
50 public List<Coord> normals;
51 public List<UVCoord> uvs;
52
53 public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 };
54
55#if SYSTEM_DRAWING
56
57 public SculptMesh SculptMeshFromFile(string fileName, SculptType sculptType, int lod, bool viewerMode)
58 {
59 Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName);
60 SculptMesh sculptMesh = new SculptMesh(bitmap, sculptType, lod, viewerMode);
61 bitmap.Dispose();
62 return sculptMesh;
63 }
64
65
66 public SculptMesh(string fileName, int sculptType, int lod, int viewerMode, int mirror, int invert)
67 {
68 Bitmap bitmap = (Bitmap)Bitmap.FromFile(fileName);
69 _SculptMesh(bitmap, (SculptType)sculptType, lod, viewerMode != 0, mirror != 0, invert != 0);
70 bitmap.Dispose();
71 }
72#endif
73
74 /// <summary>
75 /// ** Experimental ** May disappear from future versions ** not recommeneded for use in applications
76 /// Construct a sculpt mesh from a 2D array of floats
77 /// </summary>
78 /// <param name="zMap"></param>
79 /// <param name="xBegin"></param>
80 /// <param name="xEnd"></param>
81 /// <param name="yBegin"></param>
82 /// <param name="yEnd"></param>
83 /// <param name="viewerMode"></param>
84 public SculptMesh(float[,] zMap, float xBegin, float xEnd, float yBegin, float yEnd, bool viewerMode)
85 {
86 float xStep, yStep;
87 float uStep, vStep;
88
89 int numYElements = zMap.GetLength(0);
90 int numXElements = zMap.GetLength(1);
91
92 try
93 {
94 xStep = (xEnd - xBegin) / (float)(numXElements - 1);
95 yStep = (yEnd - yBegin) / (float)(numYElements - 1);
96
97 uStep = 1.0f / (numXElements - 1);
98 vStep = 1.0f / (numYElements - 1);
99 }
100 catch (DivideByZeroException)
101 {
102 return;
103 }
104
105 coords = new List<Coord>();
106 faces = new List<Face>();
107 normals = new List<Coord>();
108 uvs = new List<UVCoord>();
109
110 viewerFaces = new List<ViewerFace>();
111
112 int p1, p2, p3, p4;
113
114 int x, y;
115 int xStart = 0, yStart = 0;
116
117 for (y = yStart; y < numYElements; y++)
118 {
119 int rowOffset = y * numXElements;
120
121 for (x = xStart; x < numXElements; x++)
122 {
123 /*
124 * p1-----p2
125 * | \ f2 |
126 * | \ |
127 * | f1 \|
128 * p3-----p4
129 */
130
131 p4 = rowOffset + x;
132 p3 = p4 - 1;
133
134 p2 = p4 - numXElements;
135 p1 = p3 - numXElements;
136
137 Coord c = new Coord(xBegin + x * xStep, yBegin + y * yStep, zMap[y, x]);
138 this.coords.Add(c);
139 if (viewerMode)
140 {
141 this.normals.Add(new Coord());
142 this.uvs.Add(new UVCoord(uStep * x, 1.0f - vStep * y));
143 }
144
145 if (y > 0 && x > 0)
146 {
147 Face f1, f2;
148
149 if (viewerMode)
150 {
151 f1 = new Face(p1, p4, p3, p1, p4, p3);
152 f1.uv1 = p1;
153 f1.uv2 = p4;
154 f1.uv3 = p3;
155
156 f2 = new Face(p1, p2, p4, p1, p2, p4);
157 f2.uv1 = p1;
158 f2.uv2 = p2;
159 f2.uv3 = p4;
160 }
161 else
162 {
163 f1 = new Face(p1, p4, p3);
164 f2 = new Face(p1, p2, p4);
165 }
166
167 this.faces.Add(f1);
168 this.faces.Add(f2);
169 }
170 }
171 }
172
173 if (viewerMode)
174 calcVertexNormals(SculptType.plane, numXElements, numYElements);
175 }
176
177#if SYSTEM_DRAWING
178 public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode)
179 {
180 _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, false, false);
181 }
182
183 public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert)
184 {
185 _SculptMesh(sculptBitmap, sculptType, lod, viewerMode, mirror, invert);
186 }
187#endif
188
189 public SculptMesh(List<List<Coord>> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert)
190 {
191 _SculptMesh(rows, sculptType, viewerMode, mirror, invert);
192 }
193
194#if SYSTEM_DRAWING
195 /// <summary>
196 /// converts a bitmap to a list of lists of coords, while scaling the image.
197 /// the scaling is done in floating point so as to allow for reduced vertex position
198 /// quantization as the position will be averaged between pixel values. this routine will
199 /// likely fail if the bitmap width and height are not powers of 2.
200 /// </summary>
201 /// <param name="bitmap"></param>
202 /// <param name="scale"></param>
203 /// <param name="mirror"></param>
204 /// <returns></returns>
205 private List<List<Coord>> bitmap2Coords(Bitmap bitmap, int scale, bool mirror)
206 {
207 int numRows = bitmap.Height / scale;
208 int numCols = bitmap.Width / scale;
209 List<List<Coord>> rows = new List<List<Coord>>(numRows);
210
211 float pixScale = 1.0f / (scale * scale);
212 pixScale /= 255;
213
214 int imageX, imageY = 0;
215
216 int rowNdx, colNdx;
217
218 for (rowNdx = 0; rowNdx < numRows; rowNdx++)
219 {
220 List<Coord> row = new List<Coord>(numCols);
221 for (colNdx = 0; colNdx < numCols; colNdx++)
222 {
223 imageX = colNdx * scale;
224 int imageYStart = rowNdx * scale;
225 int imageYEnd = imageYStart + scale;
226 int imageXEnd = imageX + scale;
227 float rSum = 0.0f;
228 float gSum = 0.0f;
229 float bSum = 0.0f;
230 for (; imageX < imageXEnd; imageX++)
231 {
232 for (imageY = imageYStart; imageY < imageYEnd; imageY++)
233 {
234 Color c = bitmap.GetPixel(imageX, imageY);
235 if (c.A != 255)
236 {
237 bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B));
238 c = bitmap.GetPixel(imageX, imageY);
239 }
240 rSum += c.R;
241 gSum += c.G;
242 bSum += c.B;
243 }
244 }
245 if (mirror)
246 row.Add(new Coord(-(rSum * pixScale - 0.5f), gSum * pixScale - 0.5f, bSum * pixScale - 0.5f));
247 else
248 row.Add(new Coord(rSum * pixScale - 0.5f, gSum * pixScale - 0.5f, bSum * pixScale - 0.5f));
249
250 }
251 rows.Add(row);
252 }
253 return rows;
254 }
255
256 private List<List<Coord>> bitmap2CoordsSampled(Bitmap bitmap, int scale, bool mirror)
257 {
258 int numRows = bitmap.Height / scale;
259 int numCols = bitmap.Width / scale;
260 List<List<Coord>> rows = new List<List<Coord>>(numRows);
261
262 float pixScale = 1.0f / 256.0f;
263
264 int imageX, imageY = 0;
265
266 int rowNdx, colNdx;
267
268 for (rowNdx = 0; rowNdx <= numRows; rowNdx++)
269 {
270 List<Coord> row = new List<Coord>(numCols);
271 imageY = rowNdx * scale;
272 if (rowNdx == numRows) imageY--;
273 for (colNdx = 0; colNdx <= numCols; colNdx++)
274 {
275 imageX = colNdx * scale;
276 if (colNdx == numCols) imageX--;
277
278 Color c = bitmap.GetPixel(imageX, imageY);
279 if (c.A != 255)
280 {
281 bitmap.SetPixel(imageX, imageY, Color.FromArgb(255, c.R, c.G, c.B));
282 c = bitmap.GetPixel(imageX, imageY);
283 }
284
285 if (mirror)
286 row.Add(new Coord(-(c.R * pixScale - 0.5f), c.G * pixScale - 0.5f, c.B * pixScale - 0.5f));
287 else
288 row.Add(new Coord(c.R * pixScale - 0.5f, c.G * pixScale - 0.5f, c.B * pixScale - 0.5f));
289
290 }
291 rows.Add(row);
292 }
293 return rows;
294 }
295
296
297 void _SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool viewerMode, bool mirror, bool invert)
298 {
299 _SculptMesh(new SculptMap(sculptBitmap, lod).ToRows(mirror), sculptType, viewerMode, mirror, invert);
300 }
301#endif
302
303 void _SculptMesh(List<List<Coord>> rows, SculptType sculptType, bool viewerMode, bool mirror, bool invert)
304 {
305 coords = new List<Coord>();
306 faces = new List<Face>();
307 normals = new List<Coord>();
308 uvs = new List<UVCoord>();
309
310 sculptType = (SculptType)(((int)sculptType) & 0x07);
311
312 if (mirror)
313 invert = !invert;
314
315 viewerFaces = new List<ViewerFace>();
316
317 int width = rows[0].Count;
318
319 int p1, p2, p3, p4;
320
321 int imageX, imageY;
322
323 if (sculptType != SculptType.plane)
324 {
325 if (rows.Count % 2 == 0)
326 {
327 for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++)
328 rows[rowNdx].Add(rows[rowNdx][0]);
329 }
330 else
331 {
332 int lastIndex = rows[0].Count - 1;
333
334 for (int i = 0; i < rows.Count; i++)
335 rows[i][0] = rows[i][lastIndex];
336 }
337 }
338
339 Coord topPole = rows[0][width / 2];
340 Coord bottomPole = rows[rows.Count - 1][width / 2];
341
342 if (sculptType == SculptType.sphere)
343 {
344 if (rows.Count % 2 == 0)
345 {
346 int count = rows[0].Count;
347 List<Coord> topPoleRow = new List<Coord>(count);
348 List<Coord> bottomPoleRow = new List<Coord>(count);
349
350 for (int i = 0; i < count; i++)
351 {
352 topPoleRow.Add(topPole);
353 bottomPoleRow.Add(bottomPole);
354 }
355 rows.Insert(0, topPoleRow);
356 rows.Add(bottomPoleRow);
357 }
358 else
359 {
360 int count = rows[0].Count;
361
362 List<Coord> topPoleRow = rows[0];
363 List<Coord> bottomPoleRow = rows[rows.Count - 1];
364
365 for (int i = 0; i < count; i++)
366 {
367 topPoleRow[i] = topPole;
368 bottomPoleRow[i] = bottomPole;
369 }
370 }
371 }
372
373 if (sculptType == SculptType.torus)
374 rows.Add(rows[0]);
375
376 int coordsDown = rows.Count;
377 int coordsAcross = rows[0].Count;
378// int lastColumn = coordsAcross - 1;
379
380 float widthUnit = 1.0f / (coordsAcross - 1);
381 float heightUnit = 1.0f / (coordsDown - 1);
382
383 for (imageY = 0; imageY < coordsDown; imageY++)
384 {
385 int rowOffset = imageY * coordsAcross;
386
387 for (imageX = 0; imageX < coordsAcross; imageX++)
388 {
389 /*
390 * p1-----p2
391 * | \ f2 |
392 * | \ |
393 * | f1 \|
394 * p3-----p4
395 */
396
397 p4 = rowOffset + imageX;
398 p3 = p4 - 1;
399
400 p2 = p4 - coordsAcross;
401 p1 = p3 - coordsAcross;
402
403 this.coords.Add(rows[imageY][imageX]);
404 if (viewerMode)
405 {
406 this.normals.Add(new Coord());
407 this.uvs.Add(new UVCoord(widthUnit * imageX, heightUnit * imageY));
408 }
409
410 if (imageY > 0 && imageX > 0)
411 {
412 Face f1, f2;
413
414 if (viewerMode)
415 {
416 if (invert)
417 {
418 f1 = new Face(p1, p4, p3, p1, p4, p3);
419 f1.uv1 = p1;
420 f1.uv2 = p4;
421 f1.uv3 = p3;
422
423 f2 = new Face(p1, p2, p4, p1, p2, p4);
424 f2.uv1 = p1;
425 f2.uv2 = p2;
426 f2.uv3 = p4;
427 }
428 else
429 {
430 f1 = new Face(p1, p3, p4, p1, p3, p4);
431 f1.uv1 = p1;
432 f1.uv2 = p3;
433 f1.uv3 = p4;
434
435 f2 = new Face(p1, p4, p2, p1, p4, p2);
436 f2.uv1 = p1;
437 f2.uv2 = p4;
438 f2.uv3 = p2;
439 }
440 }
441 else
442 {
443 if (invert)
444 {
445 f1 = new Face(p1, p4, p3);
446 f2 = new Face(p1, p2, p4);
447 }
448 else
449 {
450 f1 = new Face(p1, p3, p4);
451 f2 = new Face(p1, p4, p2);
452 }
453 }
454
455 this.faces.Add(f1);
456 this.faces.Add(f2);
457 }
458 }
459 }
460
461 if (viewerMode)
462 calcVertexNormals(sculptType, coordsAcross, coordsDown);
463 }
464
465 /// <summary>
466 /// Duplicates a SculptMesh object. All object properties are copied by value, including lists.
467 /// </summary>
468 /// <returns></returns>
469 public SculptMesh Copy()
470 {
471 return new SculptMesh(this);
472 }
473
474 public SculptMesh(SculptMesh sm)
475 {
476 coords = new List<Coord>(sm.coords);
477 faces = new List<Face>(sm.faces);
478 viewerFaces = new List<ViewerFace>(sm.viewerFaces);
479 normals = new List<Coord>(sm.normals);
480 uvs = new List<UVCoord>(sm.uvs);
481 }
482
483 private void calcVertexNormals(SculptType sculptType, int xSize, int ySize)
484 { // compute vertex normals by summing all the surface normals of all the triangles sharing
485 // each vertex and then normalizing
486 int numFaces = this.faces.Count;
487 for (int i = 0; i < numFaces; i++)
488 {
489 Face face = this.faces[i];
490 Coord surfaceNormal = face.SurfaceNormal(this.coords);
491 this.normals[face.n1] += surfaceNormal;
492 this.normals[face.n2] += surfaceNormal;
493 this.normals[face.n3] += surfaceNormal;
494 }
495
496 int numNormals = this.normals.Count;
497 for (int i = 0; i < numNormals; i++)
498 this.normals[i] = this.normals[i].Normalize();
499
500 if (sculptType != SculptType.plane)
501 { // blend the vertex normals at the cylinder seam
502 for (int y = 0; y < ySize; y++)
503 {
504 int rowOffset = y * xSize;
505
506 this.normals[rowOffset] = this.normals[rowOffset + xSize - 1] = (this.normals[rowOffset] + this.normals[rowOffset + xSize - 1]).Normalize();
507 }
508 }
509
510 foreach (Face face in this.faces)
511 {
512 ViewerFace vf = new ViewerFace(0);
513 vf.v1 = this.coords[face.v1];
514 vf.v2 = this.coords[face.v2];
515 vf.v3 = this.coords[face.v3];
516
517 vf.coordIndex1 = face.v1;
518 vf.coordIndex2 = face.v2;
519 vf.coordIndex3 = face.v3;
520
521 vf.n1 = this.normals[face.n1];
522 vf.n2 = this.normals[face.n2];
523 vf.n3 = this.normals[face.n3];
524
525 vf.uv1 = this.uvs[face.uv1];
526 vf.uv2 = this.uvs[face.uv2];
527 vf.uv3 = this.uvs[face.uv3];
528
529 this.viewerFaces.Add(vf);
530 }
531 }
532
533 /// <summary>
534 /// Adds a value to each XYZ vertex coordinate in the mesh
535 /// </summary>
536 /// <param name="x"></param>
537 /// <param name="y"></param>
538 /// <param name="z"></param>
539 public void AddPos(float x, float y, float z)
540 {
541 int i;
542 int numVerts = this.coords.Count;
543 Coord vert;
544
545 for (i = 0; i < numVerts; i++)
546 {
547 vert = this.coords[i];
548 vert.X += x;
549 vert.Y += y;
550 vert.Z += z;
551 this.coords[i] = vert;
552 }
553
554 if (this.viewerFaces != null)
555 {
556 int numViewerFaces = this.viewerFaces.Count;
557
558 for (i = 0; i < numViewerFaces; i++)
559 {
560 ViewerFace v = this.viewerFaces[i];
561 v.AddPos(x, y, z);
562 this.viewerFaces[i] = v;
563 }
564 }
565 }
566
567 /// <summary>
568 /// Rotates the mesh
569 /// </summary>
570 /// <param name="q"></param>
571 public void AddRot(Quat q)
572 {
573 int i;
574 int numVerts = this.coords.Count;
575
576 for (i = 0; i < numVerts; i++)
577 this.coords[i] *= q;
578
579 int numNormals = this.normals.Count;
580 for (i = 0; i < numNormals; i++)
581 this.normals[i] *= q;
582
583 if (this.viewerFaces != null)
584 {
585 int numViewerFaces = this.viewerFaces.Count;
586
587 for (i = 0; i < numViewerFaces; i++)
588 {
589 ViewerFace v = this.viewerFaces[i];
590 v.v1 *= q;
591 v.v2 *= q;
592 v.v3 *= q;
593
594 v.n1 *= q;
595 v.n2 *= q;
596 v.n3 *= q;
597
598 this.viewerFaces[i] = v;
599 }
600 }
601 }
602
603 public void Scale(float x, float y, float z)
604 {
605 int i;
606 int numVerts = this.coords.Count;
607
608 Coord m = new Coord(x, y, z);
609 for (i = 0; i < numVerts; i++)
610 this.coords[i] *= m;
611
612 if (this.viewerFaces != null)
613 {
614 int numViewerFaces = this.viewerFaces.Count;
615 for (i = 0; i < numViewerFaces; i++)
616 {
617 ViewerFace v = this.viewerFaces[i];
618 v.v1 *= m;
619 v.v2 *= m;
620 v.v3 *= m;
621 this.viewerFaces[i] = v;
622 }
623 }
624 }
625
626 public void DumpRaw(String path, String name, String title)
627 {
628 if (path == null)
629 return;
630 String fileName = name + "_" + title + ".raw";
631 String completePath = System.IO.Path.Combine(path, fileName);
632 StreamWriter sw = new StreamWriter(completePath);
633
634 for (int i = 0; i < this.faces.Count; i++)
635 {
636 string s = this.coords[this.faces[i].v1].ToString();
637 s += " " + this.coords[this.faces[i].v2].ToString();
638 s += " " + this.coords[this.faces[i].v3].ToString();
639
640 sw.WriteLine(s);
641 }
642
643 sw.Close();
644 }
645 }
646}
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..6e4e41f
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1470 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28
29// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private bool _zeroFlag = false;
78 private Vector3 _velocity;
79 private Vector3 _target_velocity;
80 private Vector3 _acceleration;
81 private Vector3 m_rotationalVelocity;
82 private float m_mass = 80f;
83 public float m_density = 60f;
84 private bool m_pidControllerActive = true;
85 public float PID_D = 800.0f;
86 public float PID_P = 900.0f;
87 //private static float POSTURE_SERVO = 10000.0f;
88 public float CAPSULE_RADIUS = 0.37f;
89 public float CAPSULE_LENGTH = 2.140599f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool m_iscolliding = false;
94 private bool m_iscollidingGround = false;
95 private bool m_iscollidingObj = false;
96 private bool m_alwaysRun = false;
97 private int m_requestedUpdateFrequency = 0;
98 public uint m_localID = 0;
99 public bool m_returnCollisions = false;
100 // taints and their non-tainted counterparts
101 public bool m_isPhysical = false; // the current physical status
102 public float MinimumGroundFlightOffset = 3f;
103
104 private float m_buoyancy = 0f;
105
106 private bool m_freemove = false;
107 // private CollisionLocker ode;
108
109 private string m_name = String.Empty;
110 // other filter control
111 int m_colliderfilter = 0;
112 int m_colliderGroundfilter = 0;
113 int m_colliderObjectfilter = 0;
114
115 // Default we're a Character
116 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
117
118 // Default, Collide with Other Geometries, spaces, bodies and characters.
119 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
120 | CollisionCategories.Geom
121 | CollisionCategories.VolumeDtc
122 );
123 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero;
127 public IntPtr Amotor = IntPtr.Zero;
128 public d.Mass ShellMass;
129// public bool collidelock = false;
130
131 public int m_eventsubscription = 0;
132 private int m_cureventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = null;
134 private bool SentEmptyCollisionsEvent;
135
136 // unique UUID of this character object
137 public UUID m_uuid;
138 public bool bad = false;
139
140 float mu;
141
142 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
143 {
144 m_uuid = UUID.Random();
145
146 if (pos.IsFinite())
147 {
148 if (pos.Z > 99999f)
149 {
150 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
151 }
152 if (pos.Z < -100f) // shouldn't this be 0 ?
153 {
154 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
155 }
156 _position = pos;
157 }
158 else
159 {
160 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
161 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
162 }
163
164 _parent_scene = parent_scene;
165
166 PID_D = pid_d;
167 PID_P = pid_p;
168 CAPSULE_RADIUS = capsule_radius;
169 m_density = density;
170 m_mass = 80f; // sure we have a default
171
172 // force lower density for testing
173 m_density = 3.0f;
174
175
176 mu = parent_scene.AvatarFriction;
177
178 walkDivisor = walk_divisor;
179 runDivisor = rundivisor;
180
181 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
182 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
183
184 m_isPhysical = false; // current status: no ODE information exists
185
186 m_name = avName;
187
188 AddChange(changes.Add, null);
189 }
190
191 public override int PhysicsActorType
192 {
193 get { return (int)ActorTypes.Agent; }
194 set { return; }
195 }
196
197 public override void getContactData(ref ContactData cdata)
198 {
199 cdata.mu = mu;
200 cdata.bounce = 0;
201 cdata.softcolide = false;
202 }
203
204 public override bool Building { get; set; }
205
206 /// <summary>
207 /// If this is set, the avatar will move faster
208 /// </summary>
209 public override bool SetAlwaysRun
210 {
211 get { return m_alwaysRun; }
212 set { m_alwaysRun = value; }
213 }
214
215 public override uint LocalID
216 {
217 set { m_localID = value; }
218 }
219
220 public override bool Grabbed
221 {
222 set { return; }
223 }
224
225 public override bool Selected
226 {
227 set { return; }
228 }
229
230 public override float Buoyancy
231 {
232 get { return m_buoyancy; }
233 set { m_buoyancy = value; }
234 }
235
236 public override bool FloatOnWater
237 {
238 set { return; }
239 }
240
241 public override bool IsPhysical
242 {
243 get { return m_isPhysical; }
244 set { return; }
245 }
246
247 public override bool ThrottleUpdates
248 {
249 get { return false; }
250 set { return; }
251 }
252
253 public override bool Flying
254 {
255 get { return flying; }
256 set
257 {
258 flying = value;
259 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
260 }
261 }
262
263 /// <summary>
264 /// Returns if the avatar is colliding in general.
265 /// This includes the ground and objects and avatar.
266 /// </summary>
267 public override bool IsColliding
268 {
269 get { return (m_iscolliding || m_iscollidingGround); }
270 set
271 {
272 if (value)
273 {
274 m_colliderfilter += 2;
275 if (m_colliderfilter > 2)
276 m_colliderfilter = 2;
277 }
278 else
279 {
280 m_colliderfilter--;
281 if (m_colliderfilter < 0)
282 m_colliderfilter = 0;
283 }
284
285 if (m_colliderfilter == 0)
286 m_iscolliding = false;
287 else
288 {
289 m_pidControllerActive = true;
290 m_iscolliding = true;
291 }
292 }
293 }
294
295 /// <summary>
296 /// Returns if an avatar is colliding with the ground
297 /// </summary>
298 public override bool CollidingGround
299 {
300 get { return m_iscollidingGround; }
301 set
302 {
303 /* we now control this
304 if (value)
305 {
306 m_colliderGroundfilter += 2;
307 if (m_colliderGroundfilter > 2)
308 m_colliderGroundfilter = 2;
309 }
310 else
311 {
312 m_colliderGroundfilter--;
313 if (m_colliderGroundfilter < 0)
314 m_colliderGroundfilter = 0;
315 }
316
317 if (m_colliderGroundfilter == 0)
318 m_iscollidingGround = false;
319 else
320 m_iscollidingGround = true;
321 */
322 }
323
324 }
325
326 /// <summary>
327 /// Returns if the avatar is colliding with an object
328 /// </summary>
329 public override bool CollidingObj
330 {
331 get { return m_iscollidingObj; }
332 set
333 {
334 // Ubit filter this also
335 if (value)
336 {
337 m_colliderObjectfilter += 2;
338 if (m_colliderObjectfilter > 2)
339 m_colliderObjectfilter = 2;
340 }
341 else
342 {
343 m_colliderObjectfilter--;
344 if (m_colliderObjectfilter < 0)
345 m_colliderObjectfilter = 0;
346 }
347
348 if (m_colliderObjectfilter == 0)
349 m_iscollidingObj = false;
350 else
351 m_iscollidingObj = true;
352
353 // m_iscollidingObj = value;
354
355 if (m_iscollidingObj)
356 m_pidControllerActive = false;
357 else
358 m_pidControllerActive = true;
359 }
360 }
361
362 /// <summary>
363 /// turn the PID controller on or off.
364 /// The PID Controller will turn on all by itself in many situations
365 /// </summary>
366 /// <param name="status"></param>
367 public void SetPidStatus(bool status)
368 {
369 m_pidControllerActive = status;
370 }
371
372 public override bool Stopped
373 {
374 get { return _zeroFlag; }
375 }
376
377 /// <summary>
378 /// This 'puts' an avatar somewhere in the physics space.
379 /// Not really a good choice unless you 'know' it's a good
380 /// spot otherwise you're likely to orbit the avatar.
381 /// </summary>
382 public override Vector3 Position
383 {
384 get { return _position; }
385 set
386 {
387 if (value.IsFinite())
388 {
389 if (value.Z > 9999999f)
390 {
391 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
392 }
393 if (value.Z < -100f)
394 {
395 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
396 }
397 AddChange(changes.Position, value);
398 }
399 else
400 {
401 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
402 }
403 }
404 }
405
406 public override Vector3 RotationalVelocity
407 {
408 get { return m_rotationalVelocity; }
409 set { m_rotationalVelocity = value; }
410 }
411
412 /// <summary>
413 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
414 /// and use it to offset landings properly
415 /// </summary>
416 public override Vector3 Size
417 {
418 get {
419 float d = CAPSULE_RADIUS * 2;
420 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
421 set
422 {
423 if (value.IsFinite())
424 {
425 AddChange(changes.Size, value);
426 }
427 else
428 {
429 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
430 }
431 }
432 }
433
434 /// <summary>
435 /// This creates the Avatar's physical Surrogate at the position supplied
436 /// </summary>
437 /// <param name="npositionX"></param>
438 /// <param name="npositionY"></param>
439 /// <param name="npositionZ"></param>
440
441 //
442 /// <summary>
443 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
444 /// This may be used in calculations in the scene/scenepresence
445 /// </summary>
446 public override float Mass
447 {
448 get
449 {
450 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
451 return m_density * AVvolume;
452 }
453 }
454 public override void link(PhysicsActor obj)
455 {
456
457 }
458
459 public override void delink()
460 {
461
462 }
463
464 public override void LockAngularMotion(Vector3 axis)
465 {
466
467 }
468
469
470 public override Vector3 Force
471 {
472 get { return _target_velocity; }
473 set { return; }
474 }
475
476 public override int VehicleType
477 {
478 get { return 0; }
479 set { return; }
480 }
481
482 public override void VehicleFloatParam(int param, float value)
483 {
484
485 }
486
487 public override void VehicleVectorParam(int param, Vector3 value)
488 {
489
490 }
491
492 public override void VehicleRotationParam(int param, Quaternion rotation)
493 {
494
495 }
496
497 public override void VehicleFlags(int param, bool remove)
498 {
499
500 }
501
502 public override void SetVolumeDetect(int param)
503 {
504
505 }
506
507 public override Vector3 CenterOfMass
508 {
509 get
510 {
511 Vector3 pos = _position;
512 return pos;
513 }
514 }
515
516 public override Vector3 GeometricCenter
517 {
518 get
519 {
520 Vector3 pos = _position;
521 return pos;
522 }
523 }
524
525 public override PrimitiveBaseShape Shape
526 {
527 set { return; }
528 }
529
530 public override Vector3 Velocity
531 {
532 get
533 {
534 return _velocity;
535 }
536 set
537 {
538 if (value.IsFinite())
539 {
540 AddChange(changes.Velocity, value);
541 }
542 else
543 {
544 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
545 }
546 }
547 }
548
549 public override Vector3 Torque
550 {
551 get { return Vector3.Zero; }
552 set { return; }
553 }
554
555 public override float CollisionScore
556 {
557 get { return 0f; }
558 set { }
559 }
560
561 public override bool Kinematic
562 {
563 get { return false; }
564 set { }
565 }
566
567 public override Quaternion Orientation
568 {
569 get { return Quaternion.Identity; }
570 set
571 {
572 }
573 }
574
575 public override Vector3 Acceleration
576 {
577 get { return _acceleration; }
578 set { }
579 }
580
581 public void SetAcceleration(Vector3 accel)
582 {
583 m_pidControllerActive = true;
584 _acceleration = accel;
585 }
586
587 /// <summary>
588 /// Adds the force supplied to the Target Velocity
589 /// The PID controller takes this target velocity and tries to make it a reality
590 /// </summary>
591 /// <param name="force"></param>
592 public override void AddForce(Vector3 force, bool pushforce)
593 {
594 if (force.IsFinite())
595 {
596 if (pushforce)
597 {
598 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
599 }
600 else
601 {
602 AddChange(changes.Velocity, force);
603 }
604 }
605 else
606 {
607 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
608 }
609 //m_lastUpdateSent = false;
610 }
611
612 public override void AddAngularForce(Vector3 force, bool pushforce)
613 {
614
615 }
616
617 public override void SetMomentum(Vector3 momentum)
618 {
619 if (momentum.IsFinite())
620 AddChange(changes.Momentum, momentum);
621 }
622
623
624 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
625 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
626 // place that is safe to call this routine AvatarGeomAndBodyCreation.
627 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
628 {
629 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
630 if (CAPSULE_LENGTH <= 0)
631 {
632 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
633 CAPSULE_LENGTH = 0.01f;
634
635 }
636
637 if (CAPSULE_RADIUS <= 0)
638 {
639 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
640 CAPSULE_RADIUS = 0.01f;
641
642 }
643 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
644
645 d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories);
646 d.GeomSetCollideBits(Shell, (uint)m_collisionFlags);
647
648 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
649
650 m_mass = ShellMass.mass; // update mass
651
652 // rescale PID parameters
653 PID_D = _parent_scene.avPIDD;
654 PID_P = _parent_scene.avPIDP;
655
656 // rescale PID parameters so that this aren't affected by mass
657 // and so don't get unstable for some masses
658 // also scale by ode time step so you don't need to refix them
659
660 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
661 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
662 PID_P /= 50 * 80;
663 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
664
665 Body = d.BodyCreate(_parent_scene.world);
666
667 _zeroFlag = false;
668 m_pidControllerActive = true;
669 m_freemove = false;
670
671 d.BodySetAutoDisableFlag(Body, false);
672 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
673
674 _position.X = npositionX;
675 _position.Y = npositionY;
676 _position.Z = npositionZ;
677
678 d.BodySetMass(Body, ref ShellMass);
679 d.GeomSetBody(Shell, Body);
680
681 // The purpose of the AMotor here is to keep the avatar's physical
682 // surrogate from rotating while moving
683 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
684 d.JointAttach(Amotor, Body, IntPtr.Zero);
685
686 d.JointSetAMotorMode(Amotor, 0);
687 d.JointSetAMotorNumAxes(Amotor, 3);
688 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
689 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
690 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
691
692 d.JointSetAMotorAngle(Amotor, 0, 0);
693 d.JointSetAMotorAngle(Amotor, 1, 0);
694 d.JointSetAMotorAngle(Amotor, 2, 0);
695
696 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
697 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
698 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
699 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
700 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
701 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
702
703 // These lowstops and high stops are effectively (no wiggle room)
704 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
705 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
706 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
707 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
708 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
709 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
710
711 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
712 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
713 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
714
715 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
716 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
717 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
718 }
719
720 /// <summary>
721 /// Destroys the avatar body and geom
722
723 private void AvatarGeomAndBodyDestroy()
724 {
725 // Kill the Amotor
726 if (Amotor != IntPtr.Zero)
727 {
728 d.JointDestroy(Amotor);
729 Amotor = IntPtr.Zero;
730 }
731
732 if (Body != IntPtr.Zero)
733 {
734 //kill the body
735 d.BodyDestroy(Body);
736 Body = IntPtr.Zero;
737 }
738
739 //kill the Geometry
740 if (Shell != IntPtr.Zero)
741 {
742 _parent_scene.geom_name_map.Remove(Shell);
743 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
744 d.GeomDestroy(Shell);
745 Shell = IntPtr.Zero;
746 }
747 }
748
749 /// <summary>
750 /// Called from Simulate
751 /// This is the avatar's movement control + PID Controller
752 /// </summary>
753 /// <param name="timeStep"></param>
754 public void Move(float timeStep, List<OdeCharacter> defects)
755 {
756 if (Body == IntPtr.Zero)
757 return;
758
759 d.Vector3 dtmp = d.BodyGetPosition(Body);
760 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
761
762 // the Amotor still lets avatar rotation to drift during colisions
763 // so force it back to identity
764
765 d.Quaternion qtmp;
766 qtmp.W = 1;
767 qtmp.X = 0;
768 qtmp.Y = 0;
769 qtmp.Z = 0;
770 d.BodySetQuaternion(Body, ref qtmp);
771
772 if (m_pidControllerActive == false)
773 {
774 _zeroPosition = localpos;
775 }
776
777 if (!localpos.IsFinite())
778 {
779 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
780 defects.Add(this);
781 // _parent_scene.RemoveCharacter(this);
782
783 // destroy avatar capsule and related ODE data
784 AvatarGeomAndBodyDestroy();
785 return;
786 }
787
788 // check outbounds forcing to be in world
789 bool fixbody = false;
790 if (localpos.X < 0.0f)
791 {
792 fixbody = true;
793 localpos.X = 0.1f;
794 }
795 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
796 {
797 fixbody = true;
798 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
799 }
800 if (localpos.Y < 0.0f)
801 {
802 fixbody = true;
803 localpos.Y = 0.1f;
804 }
805 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
806 {
807 fixbody = true;
808 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
809 }
810 if (fixbody)
811 {
812 m_freemove = false;
813 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
814 }
815
816 float breakfactor;
817
818 Vector3 vec = Vector3.Zero;
819 dtmp = d.BodyGetLinearVel(Body);
820 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
821 float velLengthSquared = vel.LengthSquared();
822
823 float movementdivisor = 1f;
824 //Ubit change divisions into multiplications below
825 if (!m_alwaysRun)
826 movementdivisor = 1 / walkDivisor;
827 else
828 movementdivisor = 1 / runDivisor;
829
830 //******************************************
831 // colide with land
832 d.AABB aabb;
833 d.GeomGetAABB(Shell, out aabb);
834 float chrminZ = aabb.MinZ;
835
836 Vector3 posch = localpos;
837
838 float ftmp;
839
840 if (flying)
841 {
842 ftmp = timeStep;
843 posch.X += vel.X * ftmp;
844 posch.Y += vel.Y * ftmp;
845 }
846
847 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
848 if (chrminZ < terrainheight)
849 {
850 float depth = terrainheight - chrminZ;
851 if (!flying)
852 {
853 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
854 }
855 else
856 vec.Z = depth * PID_P * 50;
857
858 if (depth < 0.1f)
859 {
860 m_colliderGroundfilter++;
861 if (m_colliderGroundfilter > 2)
862 {
863 m_iscolliding = true;
864 m_colliderfilter = 2;
865
866 if (m_colliderGroundfilter > 10)
867 {
868 m_colliderGroundfilter = 10;
869 m_freemove = false;
870 }
871
872 m_iscollidingGround = true;
873
874 ContactPoint contact = new ContactPoint();
875 contact.PenetrationDepth = depth;
876 contact.Position.X = localpos.X;
877 contact.Position.Y = localpos.Y;
878 contact.Position.Z = chrminZ;
879 contact.SurfaceNormal.X = 0f;
880 contact.SurfaceNormal.Y = 0f;
881 contact.SurfaceNormal.Z = -1f;
882 contact.RelativeSpeed = -vel.Z;
883 AddCollisionEvent(0, contact);
884
885 vec.Z *= 0.5f;
886 }
887 }
888
889 else
890 {
891 m_colliderGroundfilter = 0;
892 m_iscollidingGround = false;
893 }
894 }
895 else
896 {
897 m_colliderGroundfilter = 0;
898 m_iscollidingGround = false;
899 }
900
901 //******************************************
902
903 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
904
905 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
906 if (!tviszero)
907 m_freemove = false;
908
909 if (!m_freemove)
910 {
911
912 // if velocity is zero, use position control; otherwise, velocity control
913 if (tviszero && m_iscolliding)
914 {
915 // keep track of where we stopped. No more slippin' & slidin'
916 if (!_zeroFlag)
917 {
918 _zeroFlag = true;
919 _zeroPosition = localpos;
920 }
921 if (m_pidControllerActive)
922 {
923 // We only want to deactivate the PID Controller if we think we want to have our surrogate
924 // react to the physics scene by moving it's position.
925 // Avatar to Avatar collisions
926 // Prim to avatar collisions
927
928 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
929 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
930 if (flying)
931 {
932 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
933 }
934 }
935 //PidStatus = true;
936 }
937 else
938 {
939 m_pidControllerActive = true;
940 _zeroFlag = false;
941
942 if (m_iscolliding)
943 {
944 if (!flying)
945 {
946 if (_target_velocity.Z > 0.0f)
947 {
948 // We're colliding with something and we're not flying but we're moving
949 // This means we're walking or running. JUMPING
950 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
951 }
952 // We're standing on something
953 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
954 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
955 }
956 else
957 {
958 // We're flying and colliding with something
959 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
960 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
961 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
962 }
963 }
964 else // ie not colliding
965 {
966 if (flying) //(!m_iscolliding && flying)
967 {
968 // we're in mid air suspended
969 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
970 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
971 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
972 }
973
974 else
975 {
976 // we're not colliding and we're not flying so that means we're falling!
977 // m_iscolliding includes collisions with the ground.
978
979 // d.Vector3 pos = d.BodyGetPosition(Body);
980 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
981 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
982 }
983 }
984 }
985
986 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
987 {
988 breakfactor = 0.16f * m_mass;
989 vec.X -= breakfactor * vel.X;
990 vec.Y -= breakfactor * vel.Y;
991 vec.Z -= breakfactor * vel.Z;
992 }
993 }
994 else
995 {
996 breakfactor = m_mass;
997 vec.X -= breakfactor * vel.X;
998 vec.Y -= breakfactor * vel.Y;
999 if (flying)
1000 vec.Z -= breakfactor * vel.Z;
1001 else
1002 vec.Z -= .5f* m_mass * vel.Z;
1003 }
1004
1005 if (flying)
1006 {
1007 vec.Z -= _parent_scene.gravityz * m_mass;
1008
1009 //Added for auto fly height. Kitto Flora
1010 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1011
1012 if (localpos.Z < target_altitude)
1013 {
1014 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1015 }
1016 // end add Kitto Flora
1017 }
1018
1019 if (vec.IsFinite())
1020 {
1021 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1022 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1023 }
1024 else
1025 {
1026 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1027 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1028 defects.Add(this);
1029 // _parent_scene.RemoveCharacter(this);
1030 // destroy avatar capsule and related ODE data
1031 AvatarGeomAndBodyDestroy();
1032 return;
1033 }
1034
1035 // update our local ideia of position velocity and aceleration
1036 _position = localpos;
1037 if (_zeroFlag)
1038 {
1039 _velocity = Vector3.Zero;
1040 _acceleration = Vector3.Zero;
1041 }
1042 else
1043 {
1044 _acceleration = _velocity; // previus velocity
1045 _velocity = vel;
1046 _acceleration = (vel - _acceleration) / timeStep;
1047 }
1048
1049 }
1050
1051 /// <summary>
1052 /// Updates the reported position and velocity.
1053 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1054 /// also outbounds checking
1055 /// copy and outbounds now done in move(..) at ode rate
1056 ///
1057 /// </summary>
1058 public void UpdatePositionAndVelocity()
1059 {
1060 return;
1061
1062// if (Body == IntPtr.Zero)
1063// return;
1064
1065 }
1066
1067 /// <summary>
1068 /// Cleanup the things we use in the scene.
1069 /// </summary>
1070 public void Destroy()
1071 {
1072 AddChange(changes.Remove, null);
1073 }
1074
1075 public override void CrossingFailure()
1076 {
1077 }
1078
1079 public override Vector3 PIDTarget { set { return; } }
1080 public override bool PIDActive { set { return; } }
1081 public override float PIDTau { set { return; } }
1082
1083 public override float PIDHoverHeight { set { return; } }
1084 public override bool PIDHoverActive { set { return; } }
1085 public override PIDHoverType PIDHoverType { set { return; } }
1086 public override float PIDHoverTau { set { return; } }
1087
1088 public override Quaternion APIDTarget { set { return; } }
1089
1090 public override bool APIDActive { set { return; } }
1091
1092 public override float APIDStrength { set { return; } }
1093
1094 public override float APIDDamping { set { return; } }
1095
1096
1097 public override void SubscribeEvents(int ms)
1098 {
1099 m_eventsubscription = ms;
1100 m_cureventsubscription = 0;
1101 if (CollisionEventsThisFrame == null)
1102 CollisionEventsThisFrame = new CollisionEventUpdate();
1103 SentEmptyCollisionsEvent = false;
1104 }
1105
1106 public override void UnSubscribeEvents()
1107 {
1108 if (CollisionEventsThisFrame != null)
1109 {
1110 CollisionEventsThisFrame.Clear();
1111 CollisionEventsThisFrame = null;
1112 }
1113 m_eventsubscription = 0;
1114 }
1115
1116 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1117 {
1118 if (CollisionEventsThisFrame == null)
1119 CollisionEventsThisFrame = new CollisionEventUpdate();
1120 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1121 _parent_scene.AddCollisionEventReporting(this);
1122 }
1123
1124 public void SendCollisions()
1125 {
1126 if (CollisionEventsThisFrame == null)
1127 return;
1128
1129 if (m_cureventsubscription < m_eventsubscription)
1130 return;
1131
1132 m_cureventsubscription = 0;
1133
1134 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1135
1136 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1137 {
1138 base.SendCollisionUpdate(CollisionEventsThisFrame);
1139
1140 if (ncolisions == 0)
1141 {
1142 SentEmptyCollisionsEvent = true;
1143 _parent_scene.RemoveCollisionEventReporting(this);
1144 }
1145 else
1146 {
1147 SentEmptyCollisionsEvent = false;
1148 CollisionEventsThisFrame.Clear();
1149 }
1150 }
1151 }
1152
1153 internal void AddCollisionFrameTime(int t)
1154 {
1155 // protect it from overflow crashing
1156 if (m_cureventsubscription < 50000)
1157 m_cureventsubscription += t;
1158 }
1159
1160 public override bool SubscribedEvents()
1161 {
1162 if (m_eventsubscription > 0)
1163 return true;
1164 return false;
1165 }
1166
1167 private void changePhysicsStatus(bool NewStatus)
1168 {
1169 if (NewStatus != m_isPhysical)
1170 {
1171 if (NewStatus)
1172 {
1173 // Create avatar capsule and related ODE data
1174 if ((Shell != IntPtr.Zero))
1175 {
1176 // a lost shell ?
1177 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1178 + (Shell != IntPtr.Zero ? "Shell " : "")
1179 + (Body != IntPtr.Zero ? "Body " : "")
1180 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1181 AvatarGeomAndBodyDestroy();
1182 }
1183
1184 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1185 _parent_scene.geom_name_map[Shell] = m_name;
1186 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1187 _parent_scene.AddCharacter(this);
1188 }
1189 else
1190 {
1191 _parent_scene.RemoveCharacter(this);
1192 // destroy avatar capsule and related ODE data
1193 AvatarGeomAndBodyDestroy();
1194 }
1195 m_freemove = false;
1196 m_isPhysical = NewStatus;
1197 }
1198 }
1199
1200 private void changeAdd()
1201 {
1202 changePhysicsStatus(true);
1203 }
1204
1205 private void changeRemove()
1206 {
1207 changePhysicsStatus(false);
1208 }
1209
1210 private void changeShape(PrimitiveBaseShape arg)
1211 {
1212 }
1213
1214 private void changeSize(Vector3 Size)
1215 {
1216 if (Size.IsFinite())
1217 {
1218 float caplen = Size.Z;
1219
1220 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1221
1222 if (caplen != CAPSULE_LENGTH)
1223 {
1224 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1225 {
1226 AvatarGeomAndBodyDestroy();
1227
1228 float prevCapsule = CAPSULE_LENGTH;
1229 CAPSULE_LENGTH = caplen;
1230
1231 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1232 _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f);
1233
1234 Velocity = Vector3.Zero;
1235
1236 _parent_scene.geom_name_map[Shell] = m_name;
1237 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1238 }
1239 else
1240 {
1241 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1242 + (Shell == IntPtr.Zero ? "Shell " : "")
1243 + (Body == IntPtr.Zero ? "Body " : "")
1244 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1245 }
1246 }
1247 m_freemove = false;
1248 m_pidControllerActive = true;
1249 }
1250 else
1251 {
1252 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1253 }
1254 }
1255
1256 private void changePosition( Vector3 newPos)
1257 {
1258 if (Body != IntPtr.Zero)
1259 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1260 _position = newPos;
1261 m_freemove = false;
1262 m_pidControllerActive = true;
1263 }
1264
1265 private void changeOrientation(Quaternion newOri)
1266 {
1267 }
1268
1269 private void changeVelocity(Vector3 newVel)
1270 {
1271 m_pidControllerActive = true;
1272 m_freemove = false;
1273 _target_velocity = newVel;
1274 }
1275
1276 private void changeSetTorque(Vector3 newTorque)
1277 {
1278 }
1279
1280 private void changeAddForce(Vector3 newForce)
1281 {
1282 }
1283
1284 private void changeAddAngularForce(Vector3 arg)
1285 {
1286 }
1287
1288 private void changeAngularLock(Vector3 arg)
1289 {
1290 }
1291
1292 private void changeFloatOnWater(bool arg)
1293 {
1294 }
1295
1296 private void changeVolumedetetion(bool arg)
1297 {
1298 }
1299
1300 private void changeSelectedStatus(bool arg)
1301 {
1302 }
1303
1304 private void changeDisable(bool arg)
1305 {
1306 }
1307
1308 private void changeBuilding(bool arg)
1309 {
1310 }
1311
1312 private void setFreeMove()
1313 {
1314 m_pidControllerActive = true;
1315 _zeroFlag = false;
1316 _target_velocity = Vector3.Zero;
1317 m_freemove = true;
1318 m_colliderfilter = -2;
1319 m_colliderObjectfilter = -2;
1320 m_colliderGroundfilter = -2;
1321
1322 m_iscolliding = false;
1323 m_iscollidingGround = false;
1324 m_iscollidingObj = false;
1325
1326 CollisionEventsThisFrame.Clear();
1327 }
1328
1329 private void changeForce(Vector3 newForce)
1330 {
1331 setFreeMove();
1332
1333 if (Body != IntPtr.Zero)
1334 {
1335 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1336 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1337 }
1338 }
1339
1340 // for now momentum is actually velocity
1341 private void changeMomentum(Vector3 newmomentum)
1342 {
1343 _velocity = newmomentum;
1344 setFreeMove();
1345
1346 if (Body != IntPtr.Zero)
1347 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1348 }
1349
1350 private void donullchange()
1351 {
1352 }
1353
1354 public bool DoAChange(changes what, object arg)
1355 {
1356 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1357 {
1358 return false;
1359 }
1360
1361 // nasty switch
1362 switch (what)
1363 {
1364 case changes.Add:
1365 changeAdd();
1366 break;
1367 case changes.Remove:
1368 changeRemove();
1369 break;
1370
1371 case changes.Position:
1372 changePosition((Vector3)arg);
1373 break;
1374
1375 case changes.Orientation:
1376 changeOrientation((Quaternion)arg);
1377 break;
1378
1379 case changes.PosOffset:
1380 donullchange();
1381 break;
1382
1383 case changes.OriOffset:
1384 donullchange();
1385 break;
1386
1387 case changes.Velocity:
1388 changeVelocity((Vector3)arg);
1389 break;
1390
1391 // case changes.Acceleration:
1392 // changeacceleration((Vector3)arg);
1393 // break;
1394 // case changes.AngVelocity:
1395 // changeangvelocity((Vector3)arg);
1396 // break;
1397
1398 case changes.Force:
1399 changeForce((Vector3)arg);
1400 break;
1401
1402 case changes.Torque:
1403 changeSetTorque((Vector3)arg);
1404 break;
1405
1406 case changes.AddForce:
1407 changeAddForce((Vector3)arg);
1408 break;
1409
1410 case changes.AddAngForce:
1411 changeAddAngularForce((Vector3)arg);
1412 break;
1413
1414 case changes.AngLock:
1415 changeAngularLock((Vector3)arg);
1416 break;
1417
1418 case changes.Size:
1419 changeSize((Vector3)arg);
1420 break;
1421
1422 case changes.Momentum:
1423 changeMomentum((Vector3)arg);
1424 break;
1425/* not in use for now
1426 case changes.Shape:
1427 changeShape((PrimitiveBaseShape)arg);
1428 break;
1429
1430 case changes.CollidesWater:
1431 changeFloatOnWater((bool)arg);
1432 break;
1433
1434 case changes.VolumeDtc:
1435 changeVolumedetetion((bool)arg);
1436 break;
1437
1438 case changes.Physical:
1439 changePhysicsStatus((bool)arg);
1440 break;
1441
1442 case changes.Selected:
1443 changeSelectedStatus((bool)arg);
1444 break;
1445
1446 case changes.disabled:
1447 changeDisable((bool)arg);
1448 break;
1449
1450 case changes.building:
1451 changeBuilding((bool)arg);
1452 break;
1453*/
1454 case changes.Null:
1455 donullchange();
1456 break;
1457
1458 default:
1459 donullchange();
1460 break;
1461 }
1462 return false;
1463 }
1464
1465 public void AddChange(changes what, object arg)
1466 {
1467 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1468 }
1469 }
1470}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..e27be1e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1083 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30 * ODEPrim.cs contains methods dealing with Prim editing, Prim
31 * characteristics and Kinetic motion.
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use.
39 */
40
41// Extensive change Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager;
52
53namespace OpenSim.Region.Physics.OdePlugin
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private OdeScene _pParentScene;
64
65 // Vehicle properties
66 // WARNING this are working copies for internel use
67 // their values may not be the corresponding parameter
68
69 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71
72 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73
74 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75 // HOVER_TERRAIN_ONLY
76 // HOVER_GLOBAL_HEIGHT
77 // NO_DEFLECTION_UP
78 // HOVER_WATER_ONLY
79 // HOVER_UP_ONLY
80 // LIMIT_MOTOR_UP
81 // LIMIT_ROLL_ONLY
82 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83
84 // Linear properties
85 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87 private float m_linearMotorDecayTimescale = 120;
88 private float m_linearMotorTimescale = 1000;
89 private Vector3 m_linearMotorOffset = Vector3.Zero;
90
91 //Angular properties
92 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96
97 //Deflection properties
98 private float m_angularDeflectionEfficiency = 0;
99 private float m_angularDeflectionTimescale = 1000;
100 private float m_linearDeflectionEfficiency = 0;
101 private float m_linearDeflectionTimescale = 1000;
102
103 //Banking properties
104 private float m_bankingEfficiency = 0;
105 private float m_bankingMix = 0;
106 private float m_bankingTimescale = 1000;
107
108 //Hover and Buoyancy properties
109 private float m_VhoverHeight = 0f;
110 private float m_VhoverEfficiency = 0f;
111 private float m_VhoverTimescale = 1000f;
112 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116
117 //Attractor properties
118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120
121
122 // auxiliar
123 private float m_lmEfect = 0f; // current linear motor eficiency
124 private float m_lmDecay = 0f; // current linear decay
125
126 private float m_amEfect = 0; // current angular motor eficiency
127 private float m_amDecay = 0f; // current linear decay
128
129 private float m_ffactor = 1.0f;
130
131 private float m_timestep = 0.02f;
132 private float m_invtimestep = 50;
133
134
135 float m_ampwr;
136 float m_amdampX;
137 float m_amdampY;
138 float m_amdampZ;
139
140 public float FrictionFactor
141 {
142 get
143 {
144 return m_ffactor;
145 }
146 }
147
148 public ODEDynamics(OdePrim rootp)
149 {
150 rootPrim = rootp;
151 _pParentScene = rootPrim._parent_scene;
152 m_timestep = _pParentScene.ODE_STEPSIZE;
153 m_invtimestep = 1.0f / m_timestep;
154 }
155
156 public void DoSetVehicle(VehicleData vd)
157 {
158 m_type = vd.m_type;
159 m_flags = vd.m_flags;
160
161
162 // Linear properties
163 m_linearMotorDirection = vd.m_linearMotorDirection;
164
165 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
166 if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
167 if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
168 if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
169
170 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
171 if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
172 m_linearMotorDecayTimescale += 0.2f;
173 m_linearMotorDecayTimescale *= m_invtimestep;
174
175 m_linearMotorTimescale = vd.m_linearMotorTimescale;
176 if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
177
178 m_linearMotorOffset = vd.m_linearMotorOffset;
179
180 //Angular properties
181 m_angularMotorDirection = vd.m_angularMotorDirection;
182 m_angularMotorTimescale = vd.m_angularMotorTimescale;
183 if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
184
185 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
186 if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
187 m_angularMotorDecayTimescale *= m_invtimestep;
188
189 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
190 if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
191 if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
192 if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
193
194 //Deflection properties
195 m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
196 m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
197 if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
198
199 m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
200 m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
201 if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
202
203 //Banking properties
204 m_bankingEfficiency = vd.m_bankingEfficiency;
205 m_bankingMix = vd.m_bankingMix;
206 m_bankingTimescale = vd.m_bankingTimescale;
207 if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
208
209 //Hover and Buoyancy properties
210 m_VhoverHeight = vd.m_VhoverHeight;
211 m_VhoverEfficiency = vd.m_VhoverEfficiency;
212 m_VhoverTimescale = vd.m_VhoverTimescale;
213 if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
214
215 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
216
217 //Attractor properties
218 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
219 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
220 if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
221
222 // Axis
223 m_referenceFrame = vd.m_referenceFrame;
224
225 m_lmEfect = 0;
226 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
227 m_amEfect = 0;
228 m_ffactor = 1.0f;
229 }
230
231 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
232 {
233 float len;
234
235 switch (pParam)
236 {
237 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
238 if (pValue < 0f) pValue = 0f;
239 if (pValue > 1f) pValue = 1f;
240 m_angularDeflectionEfficiency = pValue;
241 break;
242 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
243 if (pValue < m_timestep) pValue = m_timestep;
244 m_angularDeflectionTimescale = pValue;
245 break;
246 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
247 if (pValue < m_timestep) pValue = m_timestep;
248 else if (pValue > 120) pValue = 120;
249 m_angularMotorDecayTimescale = pValue * m_invtimestep;
250 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
251 break;
252 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
253 if (pValue < m_timestep) pValue = m_timestep;
254 m_angularMotorTimescale = pValue;
255 break;
256 case Vehicle.BANKING_EFFICIENCY:
257 if (pValue < -1f) pValue = -1f;
258 if (pValue > 1f) pValue = 1f;
259 m_bankingEfficiency = pValue;
260 break;
261 case Vehicle.BANKING_MIX:
262 if (pValue < 0f) pValue = 0f;
263 if (pValue > 1f) pValue = 1f;
264 m_bankingMix = pValue;
265 break;
266 case Vehicle.BANKING_TIMESCALE:
267 if (pValue < m_timestep) pValue = m_timestep;
268 m_bankingTimescale = pValue;
269 break;
270 case Vehicle.BUOYANCY:
271 if (pValue < -1f) pValue = -1f;
272 if (pValue > 1f) pValue = 1f;
273 m_VehicleBuoyancy = pValue;
274 break;
275 case Vehicle.HOVER_EFFICIENCY:
276 if (pValue < 0f) pValue = 0f;
277 if (pValue > 1f) pValue = 1f;
278 m_VhoverEfficiency = pValue;
279 break;
280 case Vehicle.HOVER_HEIGHT:
281 m_VhoverHeight = pValue;
282 break;
283 case Vehicle.HOVER_TIMESCALE:
284 if (pValue < m_timestep) pValue = m_timestep;
285 m_VhoverTimescale = pValue;
286 break;
287 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
288 if (pValue < 0f) pValue = 0f;
289 if (pValue > 1f) pValue = 1f;
290 m_linearDeflectionEfficiency = pValue;
291 break;
292 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
293 if (pValue < m_timestep) pValue = m_timestep;
294 m_linearDeflectionTimescale = pValue;
295 break;
296 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
297 if (pValue < m_timestep) pValue = m_timestep;
298 else if (pValue > 120) pValue = 120;
299 m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
300 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
301 break;
302 case Vehicle.LINEAR_MOTOR_TIMESCALE:
303 if (pValue < m_timestep) pValue = m_timestep;
304 m_linearMotorTimescale = pValue;
305 break;
306 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
307 if (pValue < 0f) pValue = 0f;
308 if (pValue > 1f) pValue = 1f;
309 m_verticalAttractionEfficiency = pValue;
310 break;
311 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
312 if (pValue < m_timestep) pValue = m_timestep;
313 m_verticalAttractionTimescale = pValue;
314 break;
315
316 // These are vector properties but the engine lets you use a single float value to
317 // set all of the components to the same value
318 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
319 if (pValue < m_timestep) pValue = m_timestep;
320 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
321 break;
322 case Vehicle.ANGULAR_MOTOR_DIRECTION:
323 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
324 len = m_angularMotorDirection.Length();
325 if (len > 12.566f)
326 m_angularMotorDirection *= (12.566f / len);
327
328 m_amEfect = 1.0f ; // turn it on
329 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
330
331 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
332 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
333 d.BodyEnable(rootPrim.Body);
334 break;
335 case Vehicle.LINEAR_FRICTION_TIMESCALE:
336 if (pValue < m_timestep) pValue = m_timestep;
337 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
338 break;
339 case Vehicle.LINEAR_MOTOR_DIRECTION:
340 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
341 len = m_linearMotorDirection.Length();
342 if (len > 100.0f)
343 m_linearMotorDirection *= (100.0f / len);
344
345 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
346 m_lmEfect = 1.0f; // turn it on
347
348 m_ffactor = 0.01f;
349 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
350 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
351 d.BodyEnable(rootPrim.Body);
352 break;
353 case Vehicle.LINEAR_MOTOR_OFFSET:
354 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
355 len = m_linearMotorOffset.Length();
356 if (len > 100.0f)
357 m_linearMotorOffset *= (100.0f / len);
358 break;
359 }
360 }//end ProcessFloatVehicleParam
361
362 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
363 {
364 float len;
365
366 switch (pParam)
367 {
368 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
369 if (pValue.X < m_timestep) pValue.X = m_timestep;
370 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
371 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
372
373 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
374 break;
375 case Vehicle.ANGULAR_MOTOR_DIRECTION:
376 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
377 // Limit requested angular speed to 2 rps= 4 pi rads/sec
378 len = m_angularMotorDirection.Length();
379 if (len > 12.566f)
380 m_angularMotorDirection *= (12.566f / len);
381
382 m_amEfect = 1.0f; // turn it on
383 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
384
385 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
386 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
387 d.BodyEnable(rootPrim.Body);
388 break;
389 case Vehicle.LINEAR_FRICTION_TIMESCALE:
390 if (pValue.X < m_timestep) pValue.X = m_timestep;
391 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
392 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
393 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
394 break;
395 case Vehicle.LINEAR_MOTOR_DIRECTION:
396 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
397 len = m_linearMotorDirection.Length();
398 if (len > 100.0f)
399 m_linearMotorDirection *= (100.0f / len);
400
401 m_lmEfect = 1.0f; // turn it on
402 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
403
404 m_ffactor = 0.01f;
405 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
406 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
407 d.BodyEnable(rootPrim.Body);
408 break;
409 case Vehicle.LINEAR_MOTOR_OFFSET:
410 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
411 len = m_linearMotorOffset.Length();
412 if (len > 100.0f)
413 m_linearMotorOffset *= (100.0f / len);
414 break;
415 case Vehicle.BLOCK_EXIT:
416 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
417 break;
418 }
419 }//end ProcessVectorVehicleParam
420
421 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
422 {
423 switch (pParam)
424 {
425 case Vehicle.REFERENCE_FRAME:
426 m_referenceFrame = Quaternion.Inverse(pValue);
427 break;
428 case Vehicle.ROLL_FRAME:
429 m_RollreferenceFrame = pValue;
430 break;
431 }
432 }//end ProcessRotationVehicleParam
433
434 internal void ProcessVehicleFlags(int pParam, bool remove)
435 {
436 if (remove)
437 {
438 m_flags &= ~((VehicleFlag)pParam);
439 }
440 else
441 {
442 m_flags |= (VehicleFlag)pParam;
443 }
444 }//end ProcessVehicleFlags
445
446 internal void ProcessTypeChange(Vehicle pType)
447 {
448 m_lmEfect = 0;
449
450 m_amEfect = 0;
451 m_ffactor = 1f;
452
453 m_linearMotorDirection = Vector3.Zero;
454 m_angularMotorDirection = Vector3.Zero;
455
456 m_BlockingEndPoint = Vector3.Zero;
457 m_RollreferenceFrame = Quaternion.Identity;
458 m_linearMotorOffset = Vector3.Zero;
459
460 m_referenceFrame = Quaternion.Identity;
461
462 // Set Defaults For Type
463 m_type = pType;
464 switch (pType)
465 {
466 case Vehicle.TYPE_NONE:
467 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
468 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
469 m_linearMotorTimescale = 1000;
470 m_linearMotorDecayTimescale = 120 * m_invtimestep;
471 m_angularMotorTimescale = 1000;
472 m_angularMotorDecayTimescale = 1000 * m_invtimestep;
473 m_VhoverHeight = 0;
474 m_VhoverEfficiency = 1;
475 m_VhoverTimescale = 1000;
476 m_VehicleBuoyancy = 0;
477 m_linearDeflectionEfficiency = 0;
478 m_linearDeflectionTimescale = 1000;
479 m_angularDeflectionEfficiency = 0;
480 m_angularDeflectionTimescale = 1000;
481 m_bankingEfficiency = 0;
482 m_bankingMix = 1;
483 m_bankingTimescale = 1000;
484 m_verticalAttractionEfficiency = 0;
485 m_verticalAttractionTimescale = 1000;
486
487 m_flags = (VehicleFlag)0;
488 break;
489
490 case Vehicle.TYPE_SLED:
491 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
492 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
493 m_linearMotorTimescale = 1000;
494 m_linearMotorDecayTimescale = 120 * m_invtimestep;
495 m_angularMotorTimescale = 1000;
496 m_angularMotorDecayTimescale = 120 * m_invtimestep;
497 m_VhoverHeight = 0;
498 m_VhoverEfficiency = 1;
499 m_VhoverTimescale = 10;
500 m_VehicleBuoyancy = 0;
501 m_linearDeflectionEfficiency = 1;
502 m_linearDeflectionTimescale = 1;
503 m_angularDeflectionEfficiency = 0;
504 m_angularDeflectionTimescale = 10;
505 m_verticalAttractionEfficiency = 1;
506 m_verticalAttractionTimescale = 1000;
507 m_bankingEfficiency = 0;
508 m_bankingMix = 1;
509 m_bankingTimescale = 10;
510 m_flags &=
511 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
512 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
513 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
514 VehicleFlag.LIMIT_ROLL_ONLY |
515 VehicleFlag.LIMIT_MOTOR_UP);
516 break;
517
518 case Vehicle.TYPE_CAR:
519 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
520 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
521 m_linearMotorTimescale = 1;
522 m_linearMotorDecayTimescale = 60 * m_invtimestep;
523 m_angularMotorTimescale = 1;
524 m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
525 m_VhoverHeight = 0;
526 m_VhoverEfficiency = 0;
527 m_VhoverTimescale = 1000;
528 m_VehicleBuoyancy = 0;
529 m_linearDeflectionEfficiency = 1;
530 m_linearDeflectionTimescale = 2;
531 m_angularDeflectionEfficiency = 0;
532 m_angularDeflectionTimescale = 10;
533 m_verticalAttractionEfficiency = 1f;
534 m_verticalAttractionTimescale = 10f;
535 m_bankingEfficiency = -0.2f;
536 m_bankingMix = 1;
537 m_bankingTimescale = 1;
538 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
539 VehicleFlag.HOVER_TERRAIN_ONLY |
540 VehicleFlag.HOVER_GLOBAL_HEIGHT);
541 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
542 VehicleFlag.LIMIT_ROLL_ONLY |
543 VehicleFlag.LIMIT_MOTOR_UP |
544 VehicleFlag.HOVER_UP_ONLY);
545 break;
546 case Vehicle.TYPE_BOAT:
547 m_linearFrictionTimescale = new Vector3(10, 3, 2);
548 m_angularFrictionTimescale = new Vector3(10, 10, 10);
549 m_linearMotorTimescale = 5;
550 m_linearMotorDecayTimescale = 60 * m_invtimestep;
551 m_angularMotorTimescale = 4;
552 m_angularMotorDecayTimescale = 4 * m_invtimestep;
553 m_VhoverHeight = 0;
554 m_VhoverEfficiency = 0.5f;
555 m_VhoverTimescale = 2;
556 m_VehicleBuoyancy = 1;
557 m_linearDeflectionEfficiency = 0.5f;
558 m_linearDeflectionTimescale = 3;
559 m_angularDeflectionEfficiency = 0.5f;
560 m_angularDeflectionTimescale = 5;
561 m_verticalAttractionEfficiency = 0.5f;
562 m_verticalAttractionTimescale = 5f;
563 m_bankingEfficiency = -0.3f;
564 m_bankingMix = 0.8f;
565 m_bankingTimescale = 1;
566 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
567 VehicleFlag.HOVER_GLOBAL_HEIGHT |
568 VehicleFlag.HOVER_UP_ONLY); // |
569// VehicleFlag.LIMIT_ROLL_ONLY);
570 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
571 VehicleFlag.LIMIT_MOTOR_UP |
572 VehicleFlag.HOVER_UP_ONLY | // new sl
573 VehicleFlag.HOVER_WATER_ONLY);
574 break;
575
576 case Vehicle.TYPE_AIRPLANE:
577 m_linearFrictionTimescale = new Vector3(200, 10, 5);
578 m_angularFrictionTimescale = new Vector3(20, 20, 20);
579 m_linearMotorTimescale = 2;
580 m_linearMotorDecayTimescale = 60 * m_invtimestep;
581 m_angularMotorTimescale = 4;
582 m_angularMotorDecayTimescale = 8 * m_invtimestep;
583 m_VhoverHeight = 0;
584 m_VhoverEfficiency = 0.5f;
585 m_VhoverTimescale = 1000;
586 m_VehicleBuoyancy = 0;
587 m_linearDeflectionEfficiency = 0.5f;
588 m_linearDeflectionTimescale = 0.5f;
589 m_angularDeflectionEfficiency = 1;
590 m_angularDeflectionTimescale = 2;
591 m_verticalAttractionEfficiency = 0.9f;
592 m_verticalAttractionTimescale = 2f;
593 m_bankingEfficiency = 1;
594 m_bankingMix = 0.7f;
595 m_bankingTimescale = 2;
596 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
597 VehicleFlag.HOVER_TERRAIN_ONLY |
598 VehicleFlag.HOVER_GLOBAL_HEIGHT |
599 VehicleFlag.HOVER_UP_ONLY |
600 VehicleFlag.NO_DEFLECTION_UP |
601 VehicleFlag.LIMIT_MOTOR_UP);
602 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
603 break;
604
605 case Vehicle.TYPE_BALLOON:
606 m_linearFrictionTimescale = new Vector3(5, 5, 5);
607 m_angularFrictionTimescale = new Vector3(10, 10, 10);
608 m_linearMotorTimescale = 5;
609 m_linearMotorDecayTimescale = 60 * m_invtimestep;
610 m_angularMotorTimescale = 6;
611 m_angularMotorDecayTimescale = 10 * m_invtimestep;
612 m_VhoverHeight = 5;
613 m_VhoverEfficiency = 0.8f;
614 m_VhoverTimescale = 10;
615 m_VehicleBuoyancy = 1;
616 m_linearDeflectionEfficiency = 0;
617 m_linearDeflectionTimescale = 5 * m_invtimestep;
618 m_angularDeflectionEfficiency = 0;
619 m_angularDeflectionTimescale = 5;
620 m_verticalAttractionEfficiency = 1f;
621 m_verticalAttractionTimescale = 1000f;
622 m_bankingEfficiency = 0;
623 m_bankingMix = 0.7f;
624 m_bankingTimescale = 5;
625 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
626 VehicleFlag.HOVER_TERRAIN_ONLY |
627 VehicleFlag.HOVER_UP_ONLY |
628 VehicleFlag.NO_DEFLECTION_UP |
629 VehicleFlag.LIMIT_MOTOR_UP | //);
630 VehicleFlag.LIMIT_ROLL_ONLY | // new sl
631 VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
632
633// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
634// VehicleFlag.HOVER_GLOBAL_HEIGHT);
635 break;
636
637 }
638
639 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
640 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
641
642 }//end SetDefaultsForType
643
644 internal void Stop()
645 {
646 m_lmEfect = 0;
647 m_lmDecay = 0f;
648 m_amEfect = 0;
649 m_amDecay = 0;
650 m_ffactor = 1f;
651 }
652
653 public static Vector3 Xrot(Quaternion rot)
654 {
655 Vector3 vec;
656 rot.Normalize(); // just in case
657 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
658 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
659 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
660 return vec;
661 }
662
663 public static Vector3 Zrot(Quaternion rot)
664 {
665 Vector3 vec;
666 rot.Normalize(); // just in case
667 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
668 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
669 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
670
671 return vec;
672 }
673
674 private const float pi = (float)Math.PI;
675 private const float halfpi = 0.5f * (float)Math.PI;
676 private const float twopi = 2.0f * pi;
677
678 public static Vector3 ubitRot2Euler(Quaternion rot)
679 {
680 // returns roll in X
681 // pitch in Y
682 // yaw in Z
683 Vector3 vec;
684
685 // assuming rot is normalised
686 // rot.Normalize();
687
688 float zX = rot.X * rot.Z + rot.Y * rot.W;
689
690 if (zX < -0.49999f)
691 {
692 vec.X = 0;
693 vec.Y = -halfpi;
694 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
695 }
696 else if (zX > 0.49999f)
697 {
698 vec.X = 0;
699 vec.Y = halfpi;
700 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
701 }
702 else
703 {
704 vec.Y = (float)Math.Asin(2 * zX);
705
706 float sqw = rot.W * rot.W;
707
708 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
709 float zZ = rot.Z * rot.Z + sqw - 0.5f;
710
711 vec.X = (float)Math.Atan2(minuszY, zZ);
712
713 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
714 float yY = rot.X * rot.X + sqw - 0.5f;
715 vec.Z = (float)Math.Atan2(yX, yY);
716 }
717 return vec;
718 }
719
720 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
721 {
722 // assuming rot is normalised
723 // rot.Normalize();
724
725 float zX = rot.X * rot.Z + rot.Y * rot.W;
726
727 if (zX < -0.49999f)
728 {
729 roll = 0;
730 pitch = -halfpi;
731 }
732 else if (zX > 0.49999f)
733 {
734 roll = 0;
735 pitch = halfpi;
736 }
737 else
738 {
739 pitch = (float)Math.Asin(2 * zX);
740
741 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
742 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
743
744 roll = (float)Math.Atan2(minuszY, zZ);
745 }
746 return ;
747 }
748
749 internal void Step()
750 {
751 IntPtr Body = rootPrim.Body;
752
753 d.Mass dmass;
754 d.BodyGetMass(Body, out dmass);
755
756 d.Quaternion rot = d.BodyGetQuaternion(Body);
757 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
758 Quaternion rotq = objrotq; // rotq = rotation of object
759 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
760 Quaternion irotq = Quaternion.Inverse(rotq);
761
762 d.Vector3 dvtmp;
763 Vector3 tmpV;
764 Vector3 curVel; // velocity in world
765 Vector3 curAngVel; // angular velocity in world
766 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
767 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
768 d.Vector3 dtorque = new d.Vector3();
769
770 dvtmp = d.BodyGetLinearVel(Body);
771 curVel.X = dvtmp.X;
772 curVel.Y = dvtmp.Y;
773 curVel.Z = dvtmp.Z;
774 Vector3 curLocalVel = curVel * irotq; // current velocity in local
775
776 dvtmp = d.BodyGetAngularVel(Body);
777 curAngVel.X = dvtmp.X;
778 curAngVel.Y = dvtmp.Y;
779 curAngVel.Z = dvtmp.Z;
780 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
781
782 float ldampZ = 0;
783
784 // linear motor
785 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
786 {
787 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
788 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
789 tmpV *= rotq; // to world
790
791 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
792 tmpV.Z = 0;
793
794 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
795 {
796 // have offset, do it now
797 tmpV *= dmass.mass;
798 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
799 }
800 else
801 {
802 force.X += tmpV.X;
803 force.Y += tmpV.Y;
804 force.Z += tmpV.Z;
805 }
806
807 m_lmEfect *= m_lmDecay;
808 m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
809 }
810 else
811 {
812 m_lmEfect = 0;
813 m_ffactor = 1f;
814 }
815
816 // hover
817 if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
818 {
819 // d.Vector3 pos = d.BodyGetPosition(Body);
820 d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
821 pos.Z -= 0.21f; // minor offset that seems to be always there in sl
822
823 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
824 float perr;
825
826 // default to global but don't go underground
827 perr = m_VhoverHeight - pos.Z;
828
829 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
830 {
831 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
832 {
833 perr += _pParentScene.GetWaterLevel();
834 }
835 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
836 {
837 perr += t;
838 }
839 else
840 {
841 float w = _pParentScene.GetWaterLevel();
842 if (t > w)
843 perr += t;
844 else
845 perr += w;
846 }
847 }
848 else if (t > m_VhoverHeight)
849 perr = t - pos.Z; ;
850
851 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
852 {
853 ldampZ = m_VhoverEfficiency * m_invtimestep;
854
855 perr *= (1.0f + ldampZ) / m_VhoverTimescale;
856
857 // force.Z += perr - curVel.Z * tmp;
858 force.Z += perr;
859 ldampZ *= -curVel.Z;
860
861 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
862 }
863 else // no buoyancy
864 force.Z += _pParentScene.gravityz;
865 }
866 else
867 {
868 // default gravity and Buoyancy
869 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
870 }
871
872 // linear deflection
873 if (m_linearDeflectionEfficiency > 0)
874 {
875 float len = curVel.Length();
876 if (len > 0.01) // if moving
877 {
878 Vector3 atAxis;
879 atAxis = Xrot(rotq); // where are we pointing to
880 atAxis *= len; // make it same size as world velocity vector
881
882 tmpV = -atAxis; // oposite direction
883 atAxis -= curVel; // error to one direction
884 len = atAxis.LengthSquared();
885
886 tmpV -= curVel; // error to oposite
887 float lens = tmpV.LengthSquared();
888
889 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
890 {
891 if (len < lens)
892 tmpV = atAxis;
893
894 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
895 force.X += tmpV.X;
896 force.Y += tmpV.Y;
897 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
898 force.Z += tmpV.Z;
899 }
900 }
901 }
902
903 // linear friction/damping
904 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
905 {
906 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
907 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
908 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
909 tmpV *= rotq; // to world
910
911 if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
912 tmpV.Z = ldampZ;
913 force.X += tmpV.X;
914 force.Y += tmpV.Y;
915 force.Z += tmpV.Z;
916 }
917
918 // vertical atractor
919 if (m_verticalAttractionTimescale < 300)
920 {
921 float roll;
922 float pitch;
923
924
925
926 float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
927
928 float ftmp2;
929 ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
930 m_amdampX = ftmp2;
931
932 m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
933
934 GetRollPitch(irotq, out roll, out pitch);
935
936 if (roll > halfpi)
937 roll = pi - roll;
938 else if (roll < -halfpi)
939 roll = -pi - roll;
940
941 float effroll = pitch / halfpi;
942 effroll *= effroll;
943 effroll = 1 - effroll;
944 effroll *= roll;
945
946
947 torque.X += effroll * ftmp;
948
949 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
950 {
951 float effpitch = roll / halfpi;
952 effpitch *= effpitch;
953 effpitch = 1 - effpitch;
954 effpitch *= pitch;
955
956 torque.Y += effpitch * ftmp;
957 }
958
959 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
960 {
961
962 float broll = effroll;
963 /*
964 if (broll > halfpi)
965 broll = pi - broll;
966 else if (broll < -halfpi)
967 broll = -pi - broll;
968 */
969 broll *= m_bankingEfficiency;
970 if (m_bankingMix != 0)
971 {
972 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
973 if (vfact > 1.0f) vfact = 1.0f;
974
975 if (curLocalVel.X >= 0)
976 broll *= (1 + (vfact - 1) * m_bankingMix);
977 else
978 broll *= -(1 + (vfact - 1) * m_bankingMix);
979 }
980 // make z rot be in world Z not local as seems to be in sl
981
982 broll = broll / m_bankingTimescale;
983
984
985 tmpV = Zrot(irotq);
986 tmpV *= broll;
987
988 torque.X += tmpV.X;
989 torque.Y += tmpV.Y;
990 torque.Z += tmpV.Z;
991
992 m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
993 m_amdampY = m_amdampZ;
994
995 }
996 else
997 {
998 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
999 m_amdampY = m_amdampX;
1000 }
1001 }
1002 else
1003 {
1004 m_ampwr = 1.0f;
1005 m_amdampX = 1 / m_angularFrictionTimescale.X;
1006 m_amdampY = 1 / m_angularFrictionTimescale.Y;
1007 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1008 }
1009
1010 // angular motor
1011 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
1012 {
1013 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
1014 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
1015 torque.X += tmpV.X * m_ampwr;
1016 torque.Y += tmpV.Y * m_ampwr;
1017 torque.Z += tmpV.Z;
1018
1019 m_amEfect *= m_amDecay;
1020 }
1021 else
1022 m_amEfect = 0;
1023
1024 // angular deflection
1025 if (m_angularDeflectionEfficiency > 0)
1026 {
1027 Vector3 dirv;
1028
1029 if (curLocalVel.X > 0.01f)
1030 dirv = curLocalVel;
1031 else if (curLocalVel.X < -0.01f)
1032 // use oposite
1033 dirv = -curLocalVel;
1034 else
1035 {
1036 // make it fall into small positive x case
1037 dirv.X = 0.01f;
1038 dirv.Y = curLocalVel.Y;
1039 dirv.Z = curLocalVel.Z;
1040 }
1041
1042 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1043
1044 if (Math.Abs(dirv.Z) > 0.01)
1045 {
1046 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
1047 }
1048
1049 if (Math.Abs(dirv.Y) > 0.01)
1050 {
1051 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
1052 }
1053 }
1054
1055 // angular friction
1056 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1057 {
1058 torque.X -= curLocalAngVel.X * m_amdampX;
1059 torque.Y -= curLocalAngVel.Y * m_amdampY;
1060 torque.Z -= curLocalAngVel.Z * m_amdampZ;
1061 }
1062
1063
1064
1065 if (force.X != 0 || force.Y != 0 || force.Z != 0)
1066 {
1067 force *= dmass.mass;
1068 d.BodyAddForce(Body, force.X, force.Y, force.Z);
1069 }
1070
1071 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1072 {
1073 torque *= m_referenceFrame; // to object frame
1074 dtorque.X = torque.X ;
1075 dtorque.Y = torque.Y;
1076 dtorque.Z = torque.Z;
1077
1078 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
1079 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1080 }
1081 }
1082 }
1083}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..9a40ab5
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,4074 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
33/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
36 * ODEPrim.cs contains methods dealing with Prim editing, Prim
37 * characteristics and Kinetic motion.
38 * ODEDynamics.cs contains methods dealing with Prim Physical motion
39 * (dynamics) and the associated settings. Old Linear and angular
40 * motors for dynamic motion have been replace with MoveLinear()
41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
43 * switch between 'VEHICLE' parameter use and general dynamics
44 * settings use.
45 */
46
47//#define SPAM
48
49using System;
50using System.Collections.Generic;
51using System.Reflection;
52using System.Runtime.InteropServices;
53using System.Threading;
54using log4net;
55using OpenMetaverse;
56using OdeAPI;
57using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager;
59
60
61namespace OpenSim.Region.Physics.OdePlugin
62{
63 public class OdePrim : PhysicsActor
64 {
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66
67 private bool m_isphysical;
68 private bool m_fakeisphysical;
69 private bool m_isphantom;
70 private bool m_fakeisphantom;
71 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
72 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
73
74 protected bool m_building;
75 protected bool m_forcePosOrRotation;
76 private bool m_iscolliding;
77
78 internal bool m_isSelected;
79 private bool m_delaySelect;
80 private bool m_lastdoneSelected;
81 internal bool m_outbounds;
82
83 private Quaternion m_lastorientation = new Quaternion();
84 private Quaternion _orientation;
85
86 private Vector3 _position;
87 private Vector3 _velocity;
88 private Vector3 _torque;
89 private Vector3 m_lastVelocity;
90 private Vector3 m_lastposition;
91 private Vector3 m_rotationalVelocity;
92 private Vector3 _size;
93 private Vector3 _acceleration;
94 private Vector3 m_angularlock = Vector3.One;
95 private IntPtr Amotor = IntPtr.Zero;
96
97 private Vector3 m_force;
98 private Vector3 m_forceacc;
99 private Vector3 m_angularForceacc;
100
101 private float m_invTimeStep = 50.0f;
102 private float m_timeStep = .02f;
103
104
105 private Vector3 m_PIDTarget;
106 private float m_PIDTau;
107 private bool m_usePID;
108
109 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
110 // and are for non-VEHICLES only.
111
112 private float m_PIDHoverHeight;
113 private float m_PIDHoverTau;
114 private bool m_useHoverPID;
115 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
116 private float m_targetHoverHeight;
117 private float m_groundHeight;
118 private float m_waterHeight;
119 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
120
121 private int body_autodisable_frames = 5;
122 private int bodydisablecontrol = 0;
123
124
125 // Default we're a Geometry
126 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
127 // Default colide nonphysical don't try to colide with anything
128 private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
129
130 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
131 CollisionCategories.Character |
132 CollisionCategories.Land |
133 CollisionCategories.VolumeDtc);
134
135// private bool m_collidesLand = true;
136 private bool m_collidesWater;
137 public bool m_returnCollisions;
138
139 private bool m_NoColide; // for now only for internal use for bad meshs
140
141
142 // Default, Collide with Other Geometries, spaces and Bodies
143 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
144
145 public bool m_disabled;
146
147 public uint m_localID;
148
149 private IMesh m_mesh;
150 private object m_meshlock = new object();
151 private PrimitiveBaseShape _pbs;
152 public OdeScene _parent_scene;
153
154 /// <summary>
155 /// The physics space which contains prim geometry
156 /// </summary>
157 public IntPtr m_targetSpace = IntPtr.Zero;
158
159 public IntPtr prim_geom;
160 public IntPtr _triMeshData;
161
162 private PhysicsActor _parent;
163
164 private List<OdePrim> childrenPrim = new List<OdePrim>();
165
166
167 private bool m_throttleUpdates;
168 private int throttleCounter;
169 public float m_collisionscore;
170 int m_colliderfilter = 0;
171
172 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
173
174 private float m_density = 10.000006836f; // Aluminum g/cm3;
175 private byte m_shapetype;
176 public bool _zeroFlag;
177 private bool m_lastUpdateSent;
178
179 public IntPtr Body = IntPtr.Zero;
180 public String Name { get; private set; }
181 private Vector3 _target_velocity;
182
183 public Vector3 primOOBsize; // prim real dimensions from mesh
184 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
185 public float primOOBradiusSQ;
186 public d.Mass primdMass; // prim inertia information on it's own referencial
187 float primMass; // prim own mass
188 float primVolume; // prim own volume;
189 float _mass; // object mass acording to case
190 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
191
192 public int givefakepos = 0;
193 private Vector3 fakepos;
194 public int givefakeori = 0;
195 private Quaternion fakeori;
196
197 private int m_eventsubscription;
198 private int m_cureventsubscription;
199 private CollisionEventUpdate CollisionEventsThisFrame = null;
200 private bool SentEmptyCollisionsEvent;
201
202 public volatile bool childPrim;
203
204 public ODEDynamics m_vehicle;
205
206 internal int m_material = (int)Material.Wood;
207 private float mu;
208 private float bounce;
209
210 /// <summary>
211 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
212 /// </summary>
213 public override bool IsPhysical // this is not reliable for internal use
214 {
215 get { return m_fakeisphysical; }
216 set
217 {
218 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
219 // and also to stop imediatly some updates
220 // but real change will only happen in taintprocessing
221
222 if (!value) // Zero the remembered last velocity
223 m_lastVelocity = Vector3.Zero;
224 AddChange(changes.Physical, value);
225 }
226 }
227
228 public override bool IsVolumeDtc
229 {
230 get { return m_fakeisVolumeDetect; }
231 set
232 {
233 m_fakeisVolumeDetect = value;
234 AddChange(changes.VolumeDtc, value);
235 }
236 }
237
238
239 public override bool Phantom // this is not reliable for internal use
240 {
241 get { return m_fakeisphantom; }
242 set
243 {
244 m_fakeisphantom = value;
245 AddChange(changes.Phantom, value);
246 }
247 }
248
249 public override bool Building // this is not reliable for internal use
250 {
251 get { return m_building; }
252 set
253 {
254 if (value)
255 m_building = true;
256 AddChange(changes.building, value);
257 }
258 }
259
260 public override void getContactData(ref ContactData cdata)
261 {
262 cdata.mu = mu;
263 cdata.bounce = bounce;
264
265 // cdata.softcolide = m_softcolide;
266 cdata.softcolide = false;
267
268 if (m_isphysical)
269 {
270 ODEDynamics veh;
271 if (_parent != null)
272 veh = ((OdePrim)_parent).m_vehicle;
273 else
274 veh = m_vehicle;
275
276 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
277 cdata.mu *= veh.FrictionFactor;
278 }
279 }
280
281 public override int PhysicsActorType
282 {
283 get { return (int)ActorTypes.Prim; }
284 set { return; }
285 }
286
287 public override bool SetAlwaysRun
288 {
289 get { return false; }
290 set { return; }
291 }
292
293 public override uint LocalID
294 {
295 get
296 {
297 return m_localID;
298 }
299 set
300 {
301 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
302 m_localID = value;
303 }
304 }
305
306 public override bool Grabbed
307 {
308 set { return; }
309 }
310
311 public override bool Selected
312 {
313 set
314 {
315 if (value)
316 m_isSelected = value; // if true set imediatly to stop moves etc
317 AddChange(changes.Selected, value);
318 }
319 }
320
321 public override bool Flying
322 {
323 // no flying prims for you
324 get { return false; }
325 set { }
326 }
327
328 public override bool IsColliding
329 {
330 get { return m_iscolliding; }
331 set
332 {
333 if (value)
334 {
335 m_colliderfilter += 2;
336 if (m_colliderfilter > 2)
337 m_colliderfilter = 2;
338 }
339 else
340 {
341 m_colliderfilter--;
342 if (m_colliderfilter < 0)
343 m_colliderfilter = 0;
344 }
345
346 if (m_colliderfilter == 0)
347 m_iscolliding = false;
348 else
349 m_iscolliding = true;
350 }
351 }
352
353 public override bool CollidingGround
354 {
355 get { return false; }
356 set { return; }
357 }
358
359 public override bool CollidingObj
360 {
361 get { return false; }
362 set { return; }
363 }
364
365 public override bool ThrottleUpdates
366 {
367 get { return m_throttleUpdates; }
368 set { m_throttleUpdates = value; }
369 }
370
371 public override bool Stopped
372 {
373 get { return _zeroFlag; }
374 }
375
376 public override Vector3 Position
377 {
378 get
379 {
380 if (givefakepos > 0)
381 return fakepos;
382 else
383 return _position;
384 }
385
386 set
387 {
388 fakepos = value;
389 givefakepos++;
390 AddChange(changes.Position, value);
391 }
392 }
393
394 public override Vector3 Size
395 {
396 get { return _size; }
397 set
398 {
399 if (value.IsFinite())
400 {
401 AddChange(changes.Size, value);
402 }
403 else
404 {
405 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
406 }
407 }
408 }
409
410 public override float Mass
411 {
412 get { return primMass; }
413 }
414
415 public override Vector3 Force
416 {
417 //get { return Vector3.Zero; }
418 get { return m_force; }
419 set
420 {
421 if (value.IsFinite())
422 {
423 AddChange(changes.Force, value);
424 }
425 else
426 {
427 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
428 }
429 }
430 }
431
432 public override void SetVolumeDetect(int param)
433 {
434 m_fakeisVolumeDetect = (param != 0);
435 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
436 }
437
438 public override Vector3 GeometricCenter
439 {
440 // this is not real geometric center but a average of positions relative to root prim acording to
441 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
442 // ignoring tortured prims details since sl also seems to ignore
443 // so no real use in doing it on physics
444 get
445 {
446 return Vector3.Zero;
447 }
448 }
449
450 public override Vector3 CenterOfMass
451 {
452 get
453 {
454 lock (_parent_scene.OdeLock)
455 {
456 d.Vector3 dtmp;
457 if (!childPrim && Body != IntPtr.Zero)
458 {
459 dtmp = d.BodyGetPosition(Body);
460 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
461 }
462 else if (prim_geom != IntPtr.Zero)
463 {
464 d.Quaternion dq;
465 d.GeomCopyQuaternion(prim_geom, out dq);
466 Quaternion q;
467 q.X = dq.X;
468 q.Y = dq.Y;
469 q.Z = dq.Z;
470 q.W = dq.W;
471
472 Vector3 Ptot = primOOBoffset * q;
473 dtmp = d.GeomGetPosition(prim_geom);
474 Ptot.X += dtmp.X;
475 Ptot.Y += dtmp.Y;
476 Ptot.Z += dtmp.Z;
477
478 // if(childPrim) we only know about physical linksets
479 return Ptot;
480/*
481 float tmass = _mass;
482 Ptot *= tmass;
483
484 float m;
485
486 foreach (OdePrim prm in childrenPrim)
487 {
488 m = prm._mass;
489 Ptot += prm.CenterOfMass * m;
490 tmass += m;
491 }
492
493 if (tmass == 0)
494 tmass = 0;
495 else
496 tmass = 1.0f / tmass;
497
498 Ptot *= tmass;
499 return Ptot;
500*/
501 }
502 else
503 return _position;
504 }
505 }
506 }
507
508 public override Vector3 OOBsize
509 {
510 get
511 {
512 return primOOBsize;
513 }
514 }
515
516 public override Vector3 OOBoffset
517 {
518 get
519 {
520 return primOOBoffset;
521 }
522 }
523
524 public override float OOBRadiusSQ
525 {
526 get
527 {
528 return primOOBradiusSQ;
529 }
530 }
531
532 public override PrimitiveBaseShape Shape
533 {
534 set
535 {
536/*
537 IMesh mesh = null;
538 if (_parent_scene.needsMeshing(value))
539 {
540 bool convex;
541 if (m_shapetype == 0)
542 convex = false;
543 else
544 convex = true;
545 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
546 }
547
548 if (mesh != null)
549 {
550 lock (m_meshlock)
551 m_mesh = mesh;
552 }
553*/
554 AddChange(changes.Shape, value);
555 }
556 }
557
558 public override byte PhysicsShapeType
559 {
560 get
561 {
562 return m_shapetype;
563 }
564 set
565 {
566 m_shapetype = value;
567 AddChange(changes.Shape, null);
568 }
569 }
570
571
572 public override Vector3 Velocity
573 {
574 get
575 {
576 if (_zeroFlag)
577 return Vector3.Zero;
578 return _velocity;
579 }
580 set
581 {
582 if (value.IsFinite())
583 {
584 AddChange(changes.Velocity, value);
585// _velocity = value;
586
587 }
588 else
589 {
590 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
591 }
592
593 }
594 }
595
596 public override Vector3 Torque
597 {
598 get
599 {
600 if (!IsPhysical || Body == IntPtr.Zero)
601 return Vector3.Zero;
602
603 return _torque;
604 }
605
606 set
607 {
608 if (value.IsFinite())
609 {
610 AddChange(changes.Torque, value);
611 }
612 else
613 {
614 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
615 }
616 }
617 }
618
619 public override float CollisionScore
620 {
621 get { return m_collisionscore; }
622 set { m_collisionscore = value; }
623 }
624
625 public override bool Kinematic
626 {
627 get { return false; }
628 set { }
629 }
630
631 public override Quaternion Orientation
632 {
633 get
634 {
635 if (givefakeori > 0)
636 return fakeori;
637 else
638
639 return _orientation;
640 }
641 set
642 {
643 if (QuaternionIsFinite(value))
644 {
645 fakeori = value;
646 givefakeori++;
647 AddChange(changes.Orientation, value);
648 }
649 else
650 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
651
652 }
653 }
654
655 public override Vector3 Acceleration
656 {
657 get { return _acceleration; }
658 set { }
659 }
660
661 public override Vector3 RotationalVelocity
662 {
663 get
664 {
665 Vector3 pv = Vector3.Zero;
666 if (_zeroFlag)
667 return pv;
668
669 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
670 return pv;
671
672 return m_rotationalVelocity;
673 }
674 set
675 {
676 if (value.IsFinite())
677 {
678 m_rotationalVelocity = value;
679 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
680 d.BodyEnable(Body);
681 }
682 else
683 {
684 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
685 }
686 }
687 }
688
689
690 public override float Buoyancy
691 {
692 get { return m_buoyancy; }
693 set
694 {
695 m_buoyancy = value;
696 }
697 }
698
699 public override bool FloatOnWater
700 {
701 set
702 {
703 AddChange(changes.CollidesWater, value);
704 }
705 }
706
707 public override Vector3 PIDTarget
708 {
709 set
710 {
711 if (value.IsFinite())
712 {
713 m_PIDTarget = value;
714 }
715 else
716 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
717 }
718 }
719
720 public override bool PIDActive { set { m_usePID = value; } }
721 public override float PIDTau
722 {
723 set
724 {
725 if (value <= 0)
726 m_PIDTau = 0;
727 else
728 {
729 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
730 if (value < mint)
731 m_PIDTau = mint;
732 else
733 m_PIDTau = value;
734 }
735 }
736 }
737
738 public override float PIDHoverHeight
739 {
740 set
741 {
742 m_PIDHoverHeight = value;
743 if (value == 0)
744 m_useHoverPID = false;
745 }
746 }
747 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
748 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
749 public override float PIDHoverTau
750 {
751 set
752 {
753 if (value <= 0)
754 m_PIDHoverTau = 0;
755 else
756 {
757 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
758 if (value < mint)
759 m_PIDHoverTau = mint;
760 else
761 m_PIDHoverTau = value;
762 }
763 }
764 }
765
766 public override Quaternion APIDTarget { set { return; } }
767
768 public override bool APIDActive { set { return; } }
769
770 public override float APIDStrength { set { return; } }
771
772 public override float APIDDamping { set { return; } }
773
774 public override int VehicleType
775 {
776 // we may need to put a fake on this
777 get
778 {
779 if (m_vehicle == null)
780 return (int)Vehicle.TYPE_NONE;
781 else
782 return (int)m_vehicle.Type;
783 }
784 set
785 {
786 AddChange(changes.VehicleType, value);
787 }
788 }
789
790 public override void VehicleFloatParam(int param, float value)
791 {
792 strVehicleFloatParam fp = new strVehicleFloatParam();
793 fp.param = param;
794 fp.value = value;
795 AddChange(changes.VehicleFloatParam, fp);
796 }
797
798 public override void VehicleVectorParam(int param, Vector3 value)
799 {
800 strVehicleVectorParam fp = new strVehicleVectorParam();
801 fp.param = param;
802 fp.value = value;
803 AddChange(changes.VehicleVectorParam, fp);
804 }
805
806 public override void VehicleRotationParam(int param, Quaternion value)
807 {
808 strVehicleQuatParam fp = new strVehicleQuatParam();
809 fp.param = param;
810 fp.value = value;
811 AddChange(changes.VehicleRotationParam, fp);
812 }
813
814 public override void VehicleFlags(int param, bool value)
815 {
816 strVehicleBoolParam bp = new strVehicleBoolParam();
817 bp.param = param;
818 bp.value = value;
819 AddChange(changes.VehicleFlags, bp);
820 }
821
822 public override void SetVehicle(object vdata)
823 {
824 AddChange(changes.SetVehicle, vdata);
825 }
826 public void SetAcceleration(Vector3 accel)
827 {
828 _acceleration = accel;
829 }
830
831 public override void AddForce(Vector3 force, bool pushforce)
832 {
833 if (force.IsFinite())
834 {
835 if(pushforce)
836 AddChange(changes.AddForce, force);
837 else // a impulse
838 AddChange(changes.AddForce, force * m_invTimeStep);
839 }
840 else
841 {
842 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
843 }
844 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
845 }
846
847 public override void AddAngularForce(Vector3 force, bool pushforce)
848 {
849 if (force.IsFinite())
850 {
851// if(pushforce) for now applyrotationimpulse seems more happy applied as a force
852 AddChange(changes.AddAngForce, force);
853// else // a impulse
854// AddChange(changes.AddAngForce, force * m_invTimeStep);
855 }
856 else
857 {
858 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
859 }
860 }
861
862 public override void CrossingFailure()
863 {
864 if (m_outbounds)
865 {
866 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
867 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
868 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
869
870 m_lastposition = _position;
871 _velocity.X = 0;
872 _velocity.Y = 0;
873 _velocity.Z = 0;
874
875 m_lastVelocity = _velocity;
876 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
877 m_vehicle.Stop();
878
879 if(Body != IntPtr.Zero)
880 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
881 if (prim_geom != IntPtr.Zero)
882 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
883
884 m_outbounds = false;
885 changeDisable(false);
886 base.RequestPhysicsterseUpdate();
887 }
888 }
889
890 public override void SetMomentum(Vector3 momentum)
891 {
892 }
893
894 public override void SetMaterial(int pMaterial)
895 {
896 m_material = pMaterial;
897 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
898 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
899 }
900
901 public void setPrimForRemoval()
902 {
903 AddChange(changes.Remove, null);
904 }
905
906 public override void link(PhysicsActor obj)
907 {
908 AddChange(changes.Link, obj);
909 }
910
911 public override void delink()
912 {
913 AddChange(changes.DeLink, null);
914 }
915
916 public override void LockAngularMotion(Vector3 axis)
917 {
918 // reverse the zero/non zero values for ODE.
919 if (axis.IsFinite())
920 {
921 axis.X = (axis.X > 0) ? 1f : 0f;
922 axis.Y = (axis.Y > 0) ? 1f : 0f;
923 axis.Z = (axis.Z > 0) ? 1f : 0f;
924 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
925 AddChange(changes.AngLock, axis);
926 }
927 else
928 {
929 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
930 }
931 }
932
933 public override void SubscribeEvents(int ms)
934 {
935 m_eventsubscription = ms;
936 m_cureventsubscription = 0;
937 if (CollisionEventsThisFrame == null)
938 CollisionEventsThisFrame = new CollisionEventUpdate();
939 SentEmptyCollisionsEvent = false;
940 }
941
942 public override void UnSubscribeEvents()
943 {
944 if (CollisionEventsThisFrame != null)
945 {
946 CollisionEventsThisFrame.Clear();
947 CollisionEventsThisFrame = null;
948 }
949 m_eventsubscription = 0;
950 }
951
952 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
953 {
954 if (CollisionEventsThisFrame == null)
955 CollisionEventsThisFrame = new CollisionEventUpdate();
956 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
957 }
958
959 public void SendCollisions()
960 {
961 if (CollisionEventsThisFrame == null)
962 return;
963
964 if (m_cureventsubscription < m_eventsubscription)
965 return;
966
967 m_cureventsubscription = 0;
968
969 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
970
971 if (!SentEmptyCollisionsEvent || ncolisions > 0)
972 {
973 base.SendCollisionUpdate(CollisionEventsThisFrame);
974
975 if (ncolisions == 0)
976 {
977 SentEmptyCollisionsEvent = true;
978 _parent_scene.RemoveCollisionEventReporting(this);
979 }
980 else
981 {
982 SentEmptyCollisionsEvent = false;
983 CollisionEventsThisFrame.Clear();
984 }
985 }
986 }
987
988 internal void AddCollisionFrameTime(int t)
989 {
990 if (m_cureventsubscription < 50000)
991 m_cureventsubscription += t;
992 }
993
994 public override bool SubscribedEvents()
995 {
996 if (m_eventsubscription > 0)
997 return true;
998 return false;
999 }
1000
1001
1002 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
1003 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1004 {
1005 Name = primName;
1006 LocalID = plocalID;
1007
1008 m_vehicle = null;
1009
1010 if (!pos.IsFinite())
1011 {
1012 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1013 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1014 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1015 }
1016 _position = pos;
1017 givefakepos = 0;
1018
1019 m_timeStep = parent_scene.ODE_STEPSIZE;
1020 m_invTimeStep = 1f / m_timeStep;
1021
1022 m_density = parent_scene.geomDefaultDensity;
1023 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
1024 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
1025
1026 prim_geom = IntPtr.Zero;
1027 collide_geom = IntPtr.Zero;
1028 Body = IntPtr.Zero;
1029
1030 if (!size.IsFinite())
1031 {
1032 size = new Vector3(0.5f, 0.5f, 0.5f);
1033 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
1034 }
1035
1036 if (size.X <= 0) size.X = 0.01f;
1037 if (size.Y <= 0) size.Y = 0.01f;
1038 if (size.Z <= 0) size.Z = 0.01f;
1039
1040 _size = size;
1041
1042 if (!QuaternionIsFinite(rotation))
1043 {
1044 rotation = Quaternion.Identity;
1045 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
1046 }
1047
1048 _orientation = rotation;
1049 givefakeori = 0;
1050
1051 _pbs = pbs;
1052
1053 _parent_scene = parent_scene;
1054 m_targetSpace = IntPtr.Zero;
1055
1056 if (pos.Z < 0)
1057 {
1058 m_isphysical = false;
1059 }
1060 else
1061 {
1062 m_isphysical = pisPhysical;
1063 }
1064 m_fakeisphysical = m_isphysical;
1065
1066 m_isVolumeDetect = false;
1067 m_fakeisVolumeDetect = false;
1068
1069 m_force = Vector3.Zero;
1070
1071 m_iscolliding = false;
1072 m_colliderfilter = 0;
1073 m_NoColide = false;
1074
1075 hasOOBoffsetFromMesh = false;
1076 _triMeshData = IntPtr.Zero;
1077
1078 m_shapetype = _shapeType;
1079
1080 m_lastdoneSelected = false;
1081 m_isSelected = false;
1082 m_delaySelect = false;
1083
1084 m_isphantom = pisPhantom;
1085 m_fakeisphantom = pisPhantom;
1086
1087 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
1088 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
1089
1090 CalcPrimBodyData();
1091
1092 m_mesh = null;
1093 if (_parent_scene.needsMeshing(pbs))
1094 {
1095 bool convex;
1096 if (m_shapetype == 0)
1097 convex = false;
1098 else
1099 convex = true;
1100
1101 m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1102 }
1103
1104
1105 m_building = true; // control must set this to false when done
1106
1107 AddChange(changes.Add, null);
1108 }
1109
1110 private void resetCollisionAccounting()
1111 {
1112 m_collisionscore = 0;
1113 }
1114
1115 private void UpdateCollisionCatFlags()
1116 {
1117 if(m_isphysical && m_disabled)
1118 {
1119 m_collisionCategories = 0;
1120 m_collisionFlags = 0;
1121 }
1122
1123 else if (m_isSelected)
1124 {
1125 m_collisionCategories = CollisionCategories.Selected;
1126 m_collisionFlags = 0;
1127 }
1128
1129 else if (m_isVolumeDetect)
1130 {
1131 m_collisionCategories = CollisionCategories.VolumeDtc;
1132 if (m_isphysical)
1133 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1134 else
1135 m_collisionFlags = 0;
1136 }
1137 else if (m_isphantom)
1138 {
1139 m_collisionCategories = CollisionCategories.Phantom;
1140 if (m_isphysical)
1141 m_collisionFlags = CollisionCategories.Land;
1142 else
1143 m_collisionFlags = 0;
1144 }
1145 else
1146 {
1147 m_collisionCategories = CollisionCategories.Geom;
1148 if (m_isphysical)
1149 m_collisionFlags = m_default_collisionFlagsPhysical;
1150 else
1151 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1152 }
1153 }
1154
1155 private void ApplyCollisionCatFlags()
1156 {
1157 if (prim_geom != IntPtr.Zero)
1158 {
1159 if (!childPrim && childrenPrim.Count > 0)
1160 {
1161 foreach (OdePrim prm in childrenPrim)
1162 {
1163 if (m_isphysical && m_disabled)
1164 {
1165 prm.m_collisionCategories = 0;
1166 prm.m_collisionFlags = 0;
1167 }
1168 else
1169 {
1170 // preserve some
1171 if (prm.m_isSelected)
1172 {
1173 prm.m_collisionCategories = CollisionCategories.Selected;
1174 prm.m_collisionFlags = 0;
1175 }
1176 else if (prm.m_isVolumeDetect)
1177 {
1178 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1179 if (m_isphysical)
1180 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1181 else
1182 prm.m_collisionFlags = 0;
1183 }
1184 else if (prm.m_isphantom)
1185 {
1186 prm.m_collisionCategories = CollisionCategories.Phantom;
1187 if (m_isphysical)
1188 prm.m_collisionFlags = CollisionCategories.Land;
1189 else
1190 prm.m_collisionFlags = 0;
1191 }
1192 else
1193 {
1194 prm.m_collisionCategories = m_collisionCategories;
1195 prm.m_collisionFlags = m_collisionFlags;
1196 }
1197 }
1198
1199 if (prm.prim_geom != IntPtr.Zero)
1200 {
1201 if (prm.m_NoColide)
1202 {
1203 d.GeomSetCategoryBits(prm.prim_geom, 0);
1204 if (m_isphysical)
1205 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1206 else
1207 d.GeomSetCollideBits(prm.prim_geom, 0);
1208 }
1209 else
1210 {
1211 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1212 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1213 }
1214 }
1215 }
1216 }
1217
1218 if (m_NoColide)
1219 {
1220 d.GeomSetCategoryBits(prim_geom, 0);
1221 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1222 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1223 {
1224 d.GeomSetCategoryBits(collide_geom, 0);
1225 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
1226 }
1227 }
1228 else
1229 {
1230 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1231 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1232 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1233 {
1234 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1235 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1236 }
1237 }
1238 }
1239 }
1240
1241 private void createAMotor(Vector3 axis)
1242 {
1243 if (Body == IntPtr.Zero)
1244 return;
1245
1246 if (Amotor != IntPtr.Zero)
1247 {
1248 d.JointDestroy(Amotor);
1249 Amotor = IntPtr.Zero;
1250 }
1251
1252 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1253
1254 if (axisnum <= 0)
1255 return;
1256
1257 // stop it
1258 d.BodySetTorque(Body, 0, 0, 0);
1259 d.BodySetAngularVel(Body, 0, 0, 0);
1260
1261 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1262 d.JointAttach(Amotor, Body, IntPtr.Zero);
1263
1264 d.JointSetAMotorMode(Amotor, 0);
1265
1266 d.JointSetAMotorNumAxes(Amotor, axisnum);
1267
1268 // get current orientation to lock
1269
1270 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1271 Quaternion curr; // crap convertion between identical things
1272 curr.X = dcur.X;
1273 curr.Y = dcur.Y;
1274 curr.Z = dcur.Z;
1275 curr.W = dcur.W;
1276 Vector3 ax;
1277
1278 int i = 0;
1279 int j = 0;
1280 if (axis.X == 0)
1281 {
1282 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1283 // ODE should do this with axis relative to body 1 but seems to fail
1284 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1285 d.JointSetAMotorAngle(Amotor, 0, 0);
1286 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1287 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1288 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1289 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1290 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1291 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1292 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1294 i++;
1295 j = 256; // move to next axis set
1296 }
1297
1298 if (axis.Y == 0)
1299 {
1300 ax = (new Vector3(0, 1, 0)) * curr;
1301 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1302 d.JointSetAMotorAngle(Amotor, i, 0);
1303 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1304 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1305 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1306 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1307 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1308 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1309 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1311 i++;
1312 j += 256;
1313 }
1314
1315 if (axis.Z == 0)
1316 {
1317 ax = (new Vector3(0, 0, 1)) * curr;
1318 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1319 d.JointSetAMotorAngle(Amotor, i, 0);
1320 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1321 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1322 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1323 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1324 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1325 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1326 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1328 }
1329 }
1330
1331 private bool setMesh(OdeScene parent_scene)
1332 {
1333 IntPtr vertices, indices;
1334 int vertexCount, indexCount;
1335 int vertexStride, triStride;
1336
1337 if (Body != IntPtr.Zero)
1338 {
1339 if (childPrim)
1340 {
1341 if (_parent != null)
1342 {
1343 OdePrim parent = (OdePrim)_parent;
1344 parent.ChildDelink(this, false);
1345 }
1346 }
1347 else
1348 {
1349 DestroyBody();
1350 }
1351 }
1352
1353 IMesh mesh = null;
1354
1355
1356 lock (m_meshlock)
1357 {
1358 if (m_mesh == null)
1359 {
1360 bool convex;
1361 if (m_shapetype == 0)
1362 convex = false;
1363 else
1364 convex = true;
1365
1366 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1367 }
1368 else
1369 {
1370 mesh = m_mesh;
1371 }
1372
1373 if (mesh == null)
1374 {
1375 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1376 return false;
1377 }
1378
1379
1380 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1381 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1382
1383 if (vertexCount == 0 || indexCount == 0)
1384 {
1385 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1386 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1387 mesh.releaseSourceMeshData();
1388 return false;
1389 }
1390
1391 primOOBoffset = mesh.GetCentroid();
1392 hasOOBoffsetFromMesh = true;
1393
1394 mesh.releaseSourceMeshData();
1395 m_mesh = null;
1396 }
1397
1398 IntPtr geo = IntPtr.Zero;
1399
1400 try
1401 {
1402 _triMeshData = d.GeomTriMeshDataCreate();
1403
1404 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1405 d.GeomTriMeshDataPreprocess(_triMeshData);
1406
1407 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1408 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1409 }
1410
1411 catch (Exception e)
1412 {
1413 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1414 if (_triMeshData != IntPtr.Zero)
1415 {
1416 d.GeomTriMeshDataDestroy(_triMeshData);
1417 _triMeshData = IntPtr.Zero;
1418 }
1419 return false;
1420 }
1421
1422 SetGeom(geo);
1423 return true;
1424 }
1425
1426 private void SetGeom(IntPtr geom)
1427 {
1428 prim_geom = geom;
1429 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1430 if (prim_geom != IntPtr.Zero)
1431 {
1432 if (m_NoColide)
1433 {
1434 d.GeomSetCategoryBits(prim_geom, 0);
1435 if (m_isphysical)
1436 {
1437 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1438 }
1439 else
1440 {
1441 d.GeomSetCollideBits(prim_geom, 0);
1442 d.GeomDisable(prim_geom);
1443 }
1444 }
1445 else
1446 {
1447 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1448 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1449 }
1450
1451 CalcPrimBodyData();
1452
1453 _parent_scene.geom_name_map[prim_geom] = Name;
1454 _parent_scene.actor_name_map[prim_geom] = this;
1455
1456 }
1457 else
1458 m_log.Warn("Setting bad Geom");
1459 }
1460
1461
1462 /// <summary>
1463 /// Create a geometry for the given mesh in the given target space.
1464 /// </summary>
1465 /// <param name="m_targetSpace"></param>
1466 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1467 private void CreateGeom()
1468 {
1469 if (_triMeshData != IntPtr.Zero)
1470 {
1471 d.GeomTriMeshDataDestroy(_triMeshData);
1472 _triMeshData = IntPtr.Zero;
1473 }
1474
1475 bool haveMesh = false;
1476 hasOOBoffsetFromMesh = false;
1477 m_NoColide = false;
1478
1479 if (_parent_scene.needsMeshing(_pbs))
1480 {
1481 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1482 if (!haveMesh)
1483 m_NoColide = true;
1484 }
1485
1486 if (!haveMesh)
1487 {
1488 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1489 && _size.X == _size.Y && _size.Y == _size.Z)
1490 { // it's a sphere
1491 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1492 try
1493 {
1494 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1495 }
1496 catch (Exception e)
1497 {
1498 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1499 return;
1500 }
1501 }
1502 else
1503 {// do it as a box
1504 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1505 try
1506 {
1507 //Console.WriteLine(" CreateGeom 4");
1508 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1509 }
1510 catch (Exception e)
1511 {
1512 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1513 return;
1514 }
1515 }
1516 }
1517 }
1518
1519 /// <summary>
1520 /// Set a new geometry for this prim.
1521 /// </summary>
1522 /// <param name="geom"></param>
1523 private void RemoveGeom()
1524 {
1525 if (prim_geom != IntPtr.Zero)
1526 {
1527 _parent_scene.geom_name_map.Remove(prim_geom);
1528 _parent_scene.actor_name_map.Remove(prim_geom);
1529 try
1530 {
1531 d.GeomDestroy(prim_geom);
1532 if (_triMeshData != IntPtr.Zero)
1533 {
1534 d.GeomTriMeshDataDestroy(_triMeshData);
1535 _triMeshData = IntPtr.Zero;
1536 }
1537 }
1538 // catch (System.AccessViolationException)
1539 catch (Exception e)
1540 {
1541 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1542 }
1543
1544 prim_geom = IntPtr.Zero;
1545 collide_geom = IntPtr.Zero;
1546 }
1547 else
1548 {
1549 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1550 }
1551 Body = IntPtr.Zero;
1552 hasOOBoffsetFromMesh = false;
1553 }
1554/*
1555 private void ChildSetGeom(OdePrim odePrim)
1556 {
1557 // well..
1558 DestroyBody();
1559 MakeBody();
1560 }
1561*/
1562 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1563 // should only be called for non physical prims unless they are becoming non physical
1564 private void SetInStaticSpace(OdePrim prim)
1565 {
1566 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1567 prim.m_targetSpace = targetSpace;
1568 collide_geom = IntPtr.Zero;
1569 }
1570
1571 public void enableBodySoft()
1572 {
1573 m_disabled = false;
1574 if (!childPrim && !m_isSelected)
1575 {
1576 if (m_isphysical && Body != IntPtr.Zero)
1577 {
1578 UpdateCollisionCatFlags();
1579 ApplyCollisionCatFlags();
1580
1581 d.BodyEnable(Body);
1582 }
1583 }
1584 resetCollisionAccounting();
1585 }
1586
1587 private void disableBodySoft()
1588 {
1589 m_disabled = true;
1590 if (!childPrim)
1591 {
1592 if (m_isphysical && Body != IntPtr.Zero)
1593 {
1594 if (m_isSelected)
1595 m_collisionFlags = CollisionCategories.Selected;
1596 else
1597 m_collisionCategories = 0;
1598 m_collisionFlags = 0;
1599 ApplyCollisionCatFlags();
1600 d.BodyDisable(Body);
1601 }
1602 }
1603 }
1604
1605 private void MakeBody()
1606 {
1607 if (!m_isphysical) // only physical get bodies
1608 return;
1609
1610 if (childPrim) // child prims don't get bodies;
1611 return;
1612
1613 if (m_building)
1614 return;
1615
1616 if (prim_geom == IntPtr.Zero)
1617 {
1618 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1619 return;
1620 }
1621
1622 if (Body != IntPtr.Zero)
1623 {
1624// d.BodyDestroy(Body);
1625// Body = IntPtr.Zero;
1626 // do a more complet destruction
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.BodySetLinearDampingThreshold(Body, 0.01f);
1740// d.BodySetAngularDampingThreshold(Body, 0.001f);
1741 d.BodySetDamping(Body, .002f, .002f);
1742
1743 if (m_targetSpace != IntPtr.Zero)
1744 {
1745 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1746 if (d.SpaceQuery(m_targetSpace, prim_geom))
1747 d.SpaceRemove(m_targetSpace, prim_geom);
1748 }
1749
1750
1751 if (childrenPrim.Count == 0)
1752 {
1753 collide_geom = prim_geom;
1754 m_targetSpace = _parent_scene.ActiveSpace;
1755 d.SpaceAdd(m_targetSpace, prim_geom);
1756 }
1757 else
1758 {
1759 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1760 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1761 d.SpaceSetSublevel(m_targetSpace, 3);
1762 d.SpaceSetCleanup(m_targetSpace, false);
1763 d.SpaceAdd(m_targetSpace, prim_geom);
1764
1765 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1766 CollisionCategories.Geom |
1767 CollisionCategories.Phantom |
1768 CollisionCategories.VolumeDtc
1769 ));
1770 d.GeomSetCollideBits(m_targetSpace, 0);
1771 collide_geom = m_targetSpace;
1772 }
1773
1774 if (m_delaySelect)
1775 {
1776 m_isSelected = true;
1777 m_delaySelect = false;
1778 }
1779
1780 lock (childrenPrim)
1781 {
1782 foreach (OdePrim prm in childrenPrim)
1783 {
1784 if (prm.prim_geom == IntPtr.Zero)
1785 continue;
1786
1787 Vector3 ppos = prm._position;
1788 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1789
1790 if (prm.m_targetSpace != m_targetSpace)
1791 {
1792 if (prm.m_targetSpace != IntPtr.Zero)
1793 {
1794 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1795 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1796 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1797 }
1798 prm.m_targetSpace = m_targetSpace;
1799 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1800 }
1801
1802 prm.m_collisionscore = 0;
1803
1804 if(!m_disabled)
1805 prm.m_disabled = false;
1806
1807 _parent_scene.addActivePrim(prm);
1808 }
1809 }
1810
1811 // The body doesn't already have a finite rotation mode set here
1812 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1813 {
1814 createAMotor(m_angularlock);
1815 }
1816
1817 m_collisionscore = 0;
1818
1819 UpdateCollisionCatFlags();
1820 ApplyCollisionCatFlags();
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
1832 _parent_scene.addActivePrim(this);
1833 _parent_scene.addActiveGroups(this);
1834 }
1835
1836 private void DestroyBody()
1837 {
1838 if (Body != IntPtr.Zero)
1839 {
1840 _parent_scene.remActivePrim(this);
1841
1842 collide_geom = IntPtr.Zero;
1843
1844 if (m_disabled)
1845 m_collisionCategories = 0;
1846 else if (m_isSelected)
1847 m_collisionCategories = CollisionCategories.Selected;
1848 else if (m_isVolumeDetect)
1849 m_collisionCategories = CollisionCategories.VolumeDtc;
1850 else if (m_isphantom)
1851 m_collisionCategories = CollisionCategories.Phantom;
1852 else
1853 m_collisionCategories = CollisionCategories.Geom;
1854
1855 m_collisionFlags = 0;
1856
1857 if (prim_geom != IntPtr.Zero)
1858 {
1859 if (m_NoColide)
1860 {
1861 d.GeomSetCategoryBits(prim_geom, 0);
1862 d.GeomSetCollideBits(prim_geom, 0);
1863 }
1864 else
1865 {
1866 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1867 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1868 }
1869 UpdateDataFromGeom();
1870 d.GeomSetBody(prim_geom, IntPtr.Zero);
1871 SetInStaticSpace(this);
1872 }
1873
1874 if (!childPrim)
1875 {
1876 lock (childrenPrim)
1877 {
1878 foreach (OdePrim prm in childrenPrim)
1879 {
1880 _parent_scene.remActivePrim(prm);
1881
1882 if (prm.m_isSelected)
1883 prm.m_collisionCategories = CollisionCategories.Selected;
1884 else if (prm.m_isVolumeDetect)
1885 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1886 else if (prm.m_isphantom)
1887 prm.m_collisionCategories = CollisionCategories.Phantom;
1888 else
1889 prm.m_collisionCategories = CollisionCategories.Geom;
1890
1891 prm.m_collisionFlags = 0;
1892
1893 if (prm.prim_geom != IntPtr.Zero)
1894 {
1895 if (prm.m_NoColide)
1896 {
1897 d.GeomSetCategoryBits(prm.prim_geom, 0);
1898 d.GeomSetCollideBits(prm.prim_geom, 0);
1899 }
1900 else
1901 {
1902 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1903 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1904 }
1905 prm.UpdateDataFromGeom();
1906 SetInStaticSpace(prm);
1907 }
1908 prm.Body = IntPtr.Zero;
1909 prm._mass = prm.primMass;
1910 prm.m_collisionscore = 0;
1911 }
1912 }
1913 if (Amotor != IntPtr.Zero)
1914 {
1915 d.JointDestroy(Amotor);
1916 Amotor = IntPtr.Zero;
1917 }
1918 _parent_scene.remActiveGroup(this);
1919 d.BodyDestroy(Body);
1920 }
1921 Body = IntPtr.Zero;
1922 }
1923 _mass = primMass;
1924 m_collisionscore = 0;
1925 }
1926
1927 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1928 {
1929 d.Matrix3 mat = new d.Matrix3();
1930 d.Quaternion quat = new d.Quaternion();
1931
1932 d.Mass tmpdmass = new d.Mass { };
1933 d.Mass objdmass = new d.Mass { };
1934
1935 d.BodyGetMass(Body, out tmpdmass);
1936 objdmass = tmpdmass;
1937
1938 d.Vector3 dobjpos;
1939 d.Vector3 thispos;
1940
1941 // get current object position and rotation
1942 dobjpos = d.BodyGetPosition(Body);
1943
1944 // get prim own inertia in its local frame
1945 tmpdmass = primdMass;
1946
1947 // transform to object frame
1948 mat = d.GeomGetOffsetRotation(prim_geom);
1949 d.MassRotate(ref tmpdmass, ref mat);
1950
1951 thispos = d.GeomGetOffsetPosition(prim_geom);
1952 d.MassTranslate(ref tmpdmass,
1953 thispos.X,
1954 thispos.Y,
1955 thispos.Z);
1956
1957 // subtract current prim inertia from object
1958 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1959
1960 // back prim own inertia
1961 tmpdmass = primdMass;
1962
1963 // update to new position and orientation
1964 _position = NewPos;
1965 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1966 _orientation = newrot;
1967 quat.X = newrot.X;
1968 quat.Y = newrot.Y;
1969 quat.Z = newrot.Z;
1970 quat.W = newrot.W;
1971 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1972
1973 mat = d.GeomGetOffsetRotation(prim_geom);
1974 d.MassRotate(ref tmpdmass, ref mat);
1975
1976 thispos = d.GeomGetOffsetPosition(prim_geom);
1977 d.MassTranslate(ref tmpdmass,
1978 thispos.X,
1979 thispos.Y,
1980 thispos.Z);
1981
1982 d.MassAdd(ref objdmass, ref tmpdmass);
1983
1984 // fix all positions
1985 IntPtr g = d.BodyGetFirstGeom(Body);
1986 while (g != IntPtr.Zero)
1987 {
1988 thispos = d.GeomGetOffsetPosition(g);
1989 thispos.X -= objdmass.c.X;
1990 thispos.Y -= objdmass.c.Y;
1991 thispos.Z -= objdmass.c.Z;
1992 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1993 g = d.dBodyGetNextGeom(g);
1994 }
1995 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1996
1997 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1998 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1999 d.BodySetMass(Body, ref objdmass);
2000 _mass = objdmass.mass;
2001 }
2002
2003
2004
2005 private void FixInertia(Vector3 NewPos)
2006 {
2007 d.Matrix3 primmat = new d.Matrix3();
2008 d.Mass tmpdmass = new d.Mass { };
2009 d.Mass objdmass = new d.Mass { };
2010 d.Mass primmass = new d.Mass { };
2011
2012 d.Vector3 dobjpos;
2013 d.Vector3 thispos;
2014
2015 d.BodyGetMass(Body, out objdmass);
2016
2017 // get prim own inertia in its local frame
2018 primmass = primdMass;
2019 // transform to object frame
2020 primmat = d.GeomGetOffsetRotation(prim_geom);
2021 d.MassRotate(ref primmass, ref primmat);
2022
2023 tmpdmass = primmass;
2024
2025 thispos = d.GeomGetOffsetPosition(prim_geom);
2026 d.MassTranslate(ref tmpdmass,
2027 thispos.X,
2028 thispos.Y,
2029 thispos.Z);
2030
2031 // subtract current prim inertia from object
2032 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2033
2034 // update to new position
2035 _position = NewPos;
2036 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2037
2038 thispos = d.GeomGetOffsetPosition(prim_geom);
2039 d.MassTranslate(ref primmass,
2040 thispos.X,
2041 thispos.Y,
2042 thispos.Z);
2043
2044 d.MassAdd(ref objdmass, ref primmass);
2045
2046 // fix all positions
2047 IntPtr g = d.BodyGetFirstGeom(Body);
2048 while (g != IntPtr.Zero)
2049 {
2050 thispos = d.GeomGetOffsetPosition(g);
2051 thispos.X -= objdmass.c.X;
2052 thispos.Y -= objdmass.c.Y;
2053 thispos.Z -= objdmass.c.Z;
2054 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2055 g = d.dBodyGetNextGeom(g);
2056 }
2057
2058 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2059
2060 // get current object position and rotation
2061 dobjpos = d.BodyGetPosition(Body);
2062
2063 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2064 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2065 d.BodySetMass(Body, ref objdmass);
2066 _mass = objdmass.mass;
2067 }
2068
2069 private void FixInertia(Quaternion newrot)
2070 {
2071 d.Matrix3 mat = new d.Matrix3();
2072 d.Quaternion quat = new d.Quaternion();
2073
2074 d.Mass tmpdmass = new d.Mass { };
2075 d.Mass objdmass = new d.Mass { };
2076 d.Vector3 dobjpos;
2077 d.Vector3 thispos;
2078
2079 d.BodyGetMass(Body, out objdmass);
2080
2081 // get prim own inertia in its local frame
2082 tmpdmass = primdMass;
2083 mat = d.GeomGetOffsetRotation(prim_geom);
2084 d.MassRotate(ref tmpdmass, ref mat);
2085 // transform to object frame
2086 thispos = d.GeomGetOffsetPosition(prim_geom);
2087 d.MassTranslate(ref tmpdmass,
2088 thispos.X,
2089 thispos.Y,
2090 thispos.Z);
2091
2092 // subtract current prim inertia from object
2093 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2094
2095 // update to new orientation
2096 _orientation = newrot;
2097 quat.X = newrot.X;
2098 quat.Y = newrot.Y;
2099 quat.Z = newrot.Z;
2100 quat.W = newrot.W;
2101 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2102
2103 tmpdmass = primdMass;
2104 mat = d.GeomGetOffsetRotation(prim_geom);
2105 d.MassRotate(ref tmpdmass, ref mat);
2106 d.MassTranslate(ref tmpdmass,
2107 thispos.X,
2108 thispos.Y,
2109 thispos.Z);
2110
2111 d.MassAdd(ref objdmass, ref tmpdmass);
2112
2113 // fix all positions
2114 IntPtr g = d.BodyGetFirstGeom(Body);
2115 while (g != IntPtr.Zero)
2116 {
2117 thispos = d.GeomGetOffsetPosition(g);
2118 thispos.X -= objdmass.c.X;
2119 thispos.Y -= objdmass.c.Y;
2120 thispos.Z -= objdmass.c.Z;
2121 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2122 g = d.dBodyGetNextGeom(g);
2123 }
2124
2125 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2126 // get current object position and rotation
2127 dobjpos = d.BodyGetPosition(Body);
2128
2129 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2130 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2131 d.BodySetMass(Body, ref objdmass);
2132 _mass = objdmass.mass;
2133 }
2134
2135
2136 #region Mass Calculation
2137
2138 private float CalculatePrimVolume()
2139 {
2140 float volume = _size.X * _size.Y * _size.Z; // default
2141 float tmp;
2142
2143 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
2144 float hollowVolume = hollowAmount * hollowAmount;
2145
2146 switch (_pbs.ProfileShape)
2147 {
2148 case ProfileShape.Square:
2149 // default box
2150
2151 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2152 {
2153 if (hollowAmount > 0.0)
2154 {
2155 switch (_pbs.HollowShape)
2156 {
2157 case HollowShape.Square:
2158 case HollowShape.Same:
2159 break;
2160
2161 case HollowShape.Circle:
2162
2163 hollowVolume *= 0.78539816339f;
2164 break;
2165
2166 case HollowShape.Triangle:
2167
2168 hollowVolume *= (0.5f * .5f);
2169 break;
2170
2171 default:
2172 hollowVolume = 0;
2173 break;
2174 }
2175 volume *= (1.0f - hollowVolume);
2176 }
2177 }
2178
2179 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2180 {
2181 //a tube
2182
2183 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
2184 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
2185 volume -= volume * tmp * tmp;
2186
2187 if (hollowAmount > 0.0)
2188 {
2189 hollowVolume *= hollowAmount;
2190
2191 switch (_pbs.HollowShape)
2192 {
2193 case HollowShape.Square:
2194 case HollowShape.Same:
2195 break;
2196
2197 case HollowShape.Circle:
2198 hollowVolume *= 0.78539816339f;
2199 break;
2200
2201 case HollowShape.Triangle:
2202 hollowVolume *= 0.5f * 0.5f;
2203 break;
2204 default:
2205 hollowVolume = 0;
2206 break;
2207 }
2208 volume *= (1.0f - hollowVolume);
2209 }
2210 }
2211
2212 break;
2213
2214 case ProfileShape.Circle:
2215
2216 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2217 {
2218 volume *= 0.78539816339f; // elipse base
2219
2220 if (hollowAmount > 0.0)
2221 {
2222 switch (_pbs.HollowShape)
2223 {
2224 case HollowShape.Same:
2225 case HollowShape.Circle:
2226 break;
2227
2228 case HollowShape.Square:
2229 hollowVolume *= 0.5f * 2.5984480504799f;
2230 break;
2231
2232 case HollowShape.Triangle:
2233 hollowVolume *= .5f * 1.27323954473516f;
2234 break;
2235
2236 default:
2237 hollowVolume = 0;
2238 break;
2239 }
2240 volume *= (1.0f - hollowVolume);
2241 }
2242 }
2243
2244 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2245 {
2246 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
2247 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2248 volume *= (1.0f - tmp * tmp);
2249
2250 if (hollowAmount > 0.0)
2251 {
2252
2253 // calculate the hollow volume by it's shape compared to the prim shape
2254 hollowVolume *= hollowAmount;
2255
2256 switch (_pbs.HollowShape)
2257 {
2258 case HollowShape.Same:
2259 case HollowShape.Circle:
2260 break;
2261
2262 case HollowShape.Square:
2263 hollowVolume *= 0.5f * 2.5984480504799f;
2264 break;
2265
2266 case HollowShape.Triangle:
2267 hollowVolume *= .5f * 1.27323954473516f;
2268 break;
2269
2270 default:
2271 hollowVolume = 0;
2272 break;
2273 }
2274 volume *= (1.0f - hollowVolume);
2275 }
2276 }
2277 break;
2278
2279 case ProfileShape.HalfCircle:
2280 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2281 {
2282 volume *= 0.5236f;
2283
2284 if (hollowAmount > 0.0)
2285 {
2286 hollowVolume *= hollowAmount;
2287
2288 switch (_pbs.HollowShape)
2289 {
2290 case HollowShape.Circle:
2291 case HollowShape.Triangle: // diference in sl is minor and odd
2292 case HollowShape.Same:
2293 break;
2294
2295 case HollowShape.Square:
2296 hollowVolume *= 0.909f;
2297 break;
2298
2299 // case HollowShape.Triangle:
2300 // hollowVolume *= .827f;
2301 // break;
2302 default:
2303 hollowVolume = 0;
2304 break;
2305 }
2306 volume *= (1.0f - hollowVolume);
2307 }
2308
2309 }
2310 break;
2311
2312 case ProfileShape.EquilateralTriangle:
2313
2314 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2315 {
2316 volume *= 0.32475953f;
2317
2318 if (hollowAmount > 0.0)
2319 {
2320
2321 // calculate the hollow volume by it's shape compared to the prim shape
2322 switch (_pbs.HollowShape)
2323 {
2324 case HollowShape.Same:
2325 case HollowShape.Triangle:
2326 hollowVolume *= .25f;
2327 break;
2328
2329 case HollowShape.Square:
2330 hollowVolume *= 0.499849f * 3.07920140172638f;
2331 break;
2332
2333 case HollowShape.Circle:
2334 // Hollow shape is a perfect cyllinder in respect to the cube's scale
2335 // Cyllinder hollow volume calculation
2336
2337 hollowVolume *= 0.1963495f * 3.07920140172638f;
2338 break;
2339
2340 default:
2341 hollowVolume = 0;
2342 break;
2343 }
2344 volume *= (1.0f - hollowVolume);
2345 }
2346 }
2347 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2348 {
2349 volume *= 0.32475953f;
2350 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
2351 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2352 volume *= (1.0f - tmp * tmp);
2353
2354 if (hollowAmount > 0.0)
2355 {
2356
2357 hollowVolume *= hollowAmount;
2358
2359 switch (_pbs.HollowShape)
2360 {
2361 case HollowShape.Same:
2362 case HollowShape.Triangle:
2363 hollowVolume *= .25f;
2364 break;
2365
2366 case HollowShape.Square:
2367 hollowVolume *= 0.499849f * 3.07920140172638f;
2368 break;
2369
2370 case HollowShape.Circle:
2371
2372 hollowVolume *= 0.1963495f * 3.07920140172638f;
2373 break;
2374
2375 default:
2376 hollowVolume = 0;
2377 break;
2378 }
2379 volume *= (1.0f - hollowVolume);
2380 }
2381 }
2382 break;
2383
2384 default:
2385 break;
2386 }
2387
2388 float taperX1;
2389 float taperY1;
2390 float taperX;
2391 float taperY;
2392 float pathBegin;
2393 float pathEnd;
2394 float profileBegin;
2395 float profileEnd;
2396
2397 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
2398 {
2399 taperX1 = _pbs.PathScaleX * 0.01f;
2400 if (taperX1 > 1.0f)
2401 taperX1 = 2.0f - taperX1;
2402 taperX = 1.0f - taperX1;
2403
2404 taperY1 = _pbs.PathScaleY * 0.01f;
2405 if (taperY1 > 1.0f)
2406 taperY1 = 2.0f - taperY1;
2407 taperY = 1.0f - taperY1;
2408 }
2409 else
2410 {
2411 taperX = _pbs.PathTaperX * 0.01f;
2412 if (taperX < 0.0f)
2413 taperX = -taperX;
2414 taperX1 = 1.0f - taperX;
2415
2416 taperY = _pbs.PathTaperY * 0.01f;
2417 if (taperY < 0.0f)
2418 taperY = -taperY;
2419 taperY1 = 1.0f - taperY;
2420 }
2421
2422 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
2423
2424 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
2425 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
2426 volume *= (pathEnd - pathBegin);
2427
2428 // this is crude aproximation
2429 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
2430 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
2431 volume *= (profileEnd - profileBegin);
2432
2433 return volume;
2434 }
2435
2436
2437 private void CalcPrimBodyData()
2438 {
2439 float volume;
2440
2441 if (prim_geom == IntPtr.Zero)
2442 {
2443 // Ubit let's have a initial basic OOB
2444 primOOBsize.X = _size.X;
2445 primOOBsize.Y = _size.Y;
2446 primOOBsize.Z = _size.Z;
2447 primOOBoffset = Vector3.Zero;
2448 }
2449 else
2450 {
2451 d.AABB AABB;
2452 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
2453
2454 primOOBsize.X = (AABB.MaxX - AABB.MinX);
2455 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
2456 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
2457 if (!hasOOBoffsetFromMesh)
2458 {
2459 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
2460 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
2461 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
2462 }
2463 }
2464
2465 // also its own inertia and mass
2466 // keep using basic shape mass for now
2467 volume = CalculatePrimVolume();
2468
2469 primVolume = volume;
2470 primMass = m_density * volume;
2471
2472 if (primMass <= 0)
2473 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2474 if (primMass > _parent_scene.maximumMassObject)
2475 primMass = _parent_scene.maximumMassObject;
2476
2477 _mass = primMass; // just in case
2478
2479 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
2480
2481 d.MassTranslate(ref primdMass,
2482 primOOBoffset.X,
2483 primOOBoffset.Y,
2484 primOOBoffset.Z);
2485
2486 primOOBsize *= 0.5f; // let obb size be a corner coords
2487 primOOBradiusSQ = primOOBsize.LengthSquared();
2488 }
2489
2490
2491 #endregion
2492
2493
2494 /// <summary>
2495 /// Add a child prim to this parent prim.
2496 /// </summary>
2497 /// <param name="prim">Child prim</param>
2498 // I'm the parent
2499 // prim is the child
2500 public void ParentPrim(OdePrim prim)
2501 {
2502 //Console.WriteLine("ParentPrim " + m_primName);
2503 if (this.m_localID != prim.m_localID)
2504 {
2505 DestroyBody(); // for now we need to rebuil entire object on link change
2506
2507 lock (childrenPrim)
2508 {
2509 // adopt the prim
2510 if (!childrenPrim.Contains(prim))
2511 childrenPrim.Add(prim);
2512
2513 // see if this prim has kids and adopt them also
2514 // should not happen for now
2515 foreach (OdePrim prm in prim.childrenPrim)
2516 {
2517 if (!childrenPrim.Contains(prm))
2518 {
2519 if (prm.Body != IntPtr.Zero)
2520 {
2521 if (prm.prim_geom != IntPtr.Zero)
2522 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2523 if (prm.Body != prim.Body)
2524 prm.DestroyBody(); // don't loose bodies around
2525 prm.Body = IntPtr.Zero;
2526 }
2527
2528 childrenPrim.Add(prm);
2529 prm._parent = this;
2530 }
2531 }
2532 }
2533 //Remove old children from the prim
2534 prim.childrenPrim.Clear();
2535
2536 if (prim.Body != IntPtr.Zero)
2537 {
2538 if (prim.prim_geom != IntPtr.Zero)
2539 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2540 prim.DestroyBody(); // don't loose bodies around
2541 prim.Body = IntPtr.Zero;
2542 }
2543
2544 prim.childPrim = true;
2545 prim._parent = this;
2546
2547 MakeBody(); // full nasty reconstruction
2548 }
2549 }
2550
2551 private void UpdateChildsfromgeom()
2552 {
2553 if (childrenPrim.Count > 0)
2554 {
2555 foreach (OdePrim prm in childrenPrim)
2556 prm.UpdateDataFromGeom();
2557 }
2558 }
2559
2560 private void UpdateDataFromGeom()
2561 {
2562 if (prim_geom != IntPtr.Zero)
2563 {
2564 d.Quaternion qtmp;
2565 d.GeomCopyQuaternion(prim_geom, out qtmp);
2566 _orientation.X = qtmp.X;
2567 _orientation.Y = qtmp.Y;
2568 _orientation.Z = qtmp.Z;
2569 _orientation.W = qtmp.W;
2570
2571 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2572 _position.X = lpos.X;
2573 _position.Y = lpos.Y;
2574 _position.Z = lpos.Z;
2575 }
2576 }
2577
2578 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2579 {
2580 // Okay, we have a delinked child.. destroy all body and remake
2581 if (odePrim != this && !childrenPrim.Contains(odePrim))
2582 return;
2583
2584 DestroyBody();
2585
2586 if (odePrim == this) // delinking the root prim
2587 {
2588 OdePrim newroot = null;
2589 lock (childrenPrim)
2590 {
2591 if (childrenPrim.Count > 0)
2592 {
2593 newroot = childrenPrim[0];
2594 childrenPrim.RemoveAt(0);
2595 foreach (OdePrim prm in childrenPrim)
2596 {
2597 newroot.childrenPrim.Add(prm);
2598 }
2599 childrenPrim.Clear();
2600 }
2601 if (newroot != null)
2602 {
2603 newroot.childPrim = false;
2604 newroot._parent = null;
2605 if (remakebodies)
2606 newroot.MakeBody();
2607 }
2608 }
2609 }
2610
2611 else
2612 {
2613 lock (childrenPrim)
2614 {
2615 childrenPrim.Remove(odePrim);
2616 odePrim.childPrim = false;
2617 odePrim._parent = null;
2618 // odePrim.UpdateDataFromGeom();
2619 if (remakebodies)
2620 odePrim.MakeBody();
2621 }
2622 }
2623 if (remakebodies)
2624 MakeBody();
2625 }
2626
2627 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2628 {
2629 // Okay, we have a delinked child.. destroy all body and remake
2630 if (odePrim != this && !childrenPrim.Contains(odePrim))
2631 return;
2632
2633 DestroyBody();
2634
2635 if (odePrim == this)
2636 {
2637 OdePrim newroot = null;
2638 lock (childrenPrim)
2639 {
2640 if (childrenPrim.Count > 0)
2641 {
2642 newroot = childrenPrim[0];
2643 childrenPrim.RemoveAt(0);
2644 foreach (OdePrim prm in childrenPrim)
2645 {
2646 newroot.childrenPrim.Add(prm);
2647 }
2648 childrenPrim.Clear();
2649 }
2650 if (newroot != null)
2651 {
2652 newroot.childPrim = false;
2653 newroot._parent = null;
2654 newroot.MakeBody();
2655 }
2656 }
2657 if (reMakeBody)
2658 MakeBody();
2659 return;
2660 }
2661 else
2662 {
2663 lock (childrenPrim)
2664 {
2665 childrenPrim.Remove(odePrim);
2666 odePrim.childPrim = false;
2667 odePrim._parent = null;
2668 if (reMakeBody)
2669 odePrim.MakeBody();
2670 }
2671 }
2672 MakeBody();
2673 }
2674
2675 #region changes
2676
2677 private void changeadd()
2678 {
2679 CreateGeom();
2680
2681 if (prim_geom != IntPtr.Zero)
2682 {
2683 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2684 d.Quaternion myrot = new d.Quaternion();
2685 myrot.X = _orientation.X;
2686 myrot.Y = _orientation.Y;
2687 myrot.Z = _orientation.Z;
2688 myrot.W = _orientation.W;
2689 d.GeomSetQuaternion(prim_geom, ref myrot);
2690
2691 if (!m_isphysical)
2692 {
2693 SetInStaticSpace(this);
2694 UpdateCollisionCatFlags();
2695 ApplyCollisionCatFlags();
2696 }
2697 else
2698 MakeBody();
2699 }
2700 }
2701
2702 private void changeAngularLock(Vector3 newLock)
2703 {
2704 // do we have a Physical object?
2705 if (Body != IntPtr.Zero)
2706 {
2707 //Check that we have a Parent
2708 //If we have a parent then we're not authorative here
2709 if (_parent == null)
2710 {
2711 if (!newLock.ApproxEquals(Vector3.One, 0f))
2712 {
2713 createAMotor(newLock);
2714 }
2715 else
2716 {
2717 if (Amotor != IntPtr.Zero)
2718 {
2719 d.JointDestroy(Amotor);
2720 Amotor = IntPtr.Zero;
2721 }
2722 }
2723 }
2724 }
2725 // Store this for later in case we get turned into a separate body
2726 m_angularlock = newLock;
2727 }
2728
2729 private void changeLink(OdePrim NewParent)
2730 {
2731 if (_parent == null && NewParent != null)
2732 {
2733 NewParent.ParentPrim(this);
2734 }
2735 else if (_parent != null)
2736 {
2737 if (_parent is OdePrim)
2738 {
2739 if (NewParent != _parent)
2740 {
2741 (_parent as OdePrim).ChildDelink(this, false); // for now...
2742 childPrim = false;
2743
2744 if (NewParent != null)
2745 {
2746 NewParent.ParentPrim(this);
2747 }
2748 }
2749 }
2750 }
2751 _parent = NewParent;
2752 }
2753
2754
2755 private void Stop()
2756 {
2757 if (!childPrim)
2758 {
2759 m_force = Vector3.Zero;
2760 m_forceacc = Vector3.Zero;
2761 m_angularForceacc = Vector3.Zero;
2762 _torque = Vector3.Zero;
2763 _velocity = Vector3.Zero;
2764 _acceleration = Vector3.Zero;
2765 m_rotationalVelocity = Vector3.Zero;
2766 _target_velocity = Vector3.Zero;
2767 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2768 m_vehicle.Stop();
2769 }
2770
2771 if (Body != IntPtr.Zero)
2772 {
2773 d.BodySetForce(Body, 0f, 0f, 0f);
2774 d.BodySetTorque(Body, 0f, 0f, 0f);
2775 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2776 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2777 }
2778 }
2779
2780 private void changePhantomStatus(bool newval)
2781 {
2782 m_isphantom = newval;
2783
2784 UpdateCollisionCatFlags();
2785 ApplyCollisionCatFlags();
2786 }
2787
2788/* not in use
2789 internal void ChildSelectedChange(bool childSelect)
2790 {
2791 if(childPrim)
2792 return;
2793
2794 if (childSelect == m_isSelected)
2795 return;
2796
2797 if (childSelect)
2798 {
2799 DoSelectedStatus(true);
2800 }
2801
2802 else
2803 {
2804 foreach (OdePrim prm in childrenPrim)
2805 {
2806 if (prm.m_isSelected)
2807 return;
2808 }
2809 DoSelectedStatus(false);
2810 }
2811 }
2812*/
2813 private void changeSelectedStatus(bool newval)
2814 {
2815 if (m_lastdoneSelected == newval)
2816 return;
2817
2818 m_lastdoneSelected = newval;
2819 DoSelectedStatus(newval);
2820 }
2821
2822 private void CheckDelaySelect()
2823 {
2824 if (m_delaySelect)
2825 {
2826 DoSelectedStatus(m_isSelected);
2827 }
2828 }
2829
2830 private void DoSelectedStatus(bool newval)
2831 {
2832 m_isSelected = newval;
2833 Stop();
2834
2835 if (newval)
2836 {
2837 if (!childPrim && Body != IntPtr.Zero)
2838 d.BodyDisable(Body);
2839
2840 if (m_delaySelect || m_isphysical)
2841 {
2842 m_collisionCategories = CollisionCategories.Selected;
2843 m_collisionFlags = 0;
2844
2845 if (!childPrim)
2846 {
2847 foreach (OdePrim prm in childrenPrim)
2848 {
2849 prm.m_collisionCategories = m_collisionCategories;
2850 prm.m_collisionFlags = m_collisionFlags;
2851
2852 if (prm.prim_geom != null)
2853 {
2854
2855 if (prm.m_NoColide)
2856 {
2857 d.GeomSetCategoryBits(prm.prim_geom, 0);
2858 d.GeomSetCollideBits(prm.prim_geom, 0);
2859 }
2860 else
2861 {
2862 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2863 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2864 }
2865 }
2866 prm.m_delaySelect = false;
2867 }
2868 }
2869// else if (_parent != null)
2870// ((OdePrim)_parent).ChildSelectedChange(true);
2871
2872
2873 if (prim_geom != null)
2874 {
2875 if (m_NoColide)
2876 {
2877 d.GeomSetCategoryBits(prim_geom, 0);
2878 d.GeomSetCollideBits(prim_geom, 0);
2879 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2880 {
2881 d.GeomSetCategoryBits(collide_geom, 0);
2882 d.GeomSetCollideBits(collide_geom, 0);
2883 }
2884
2885 }
2886 else
2887 {
2888 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2889 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2890 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2891 {
2892 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
2893 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2894 }
2895 }
2896 }
2897
2898 m_delaySelect = false;
2899 }
2900 else if(!m_isphysical)
2901 {
2902 m_delaySelect = true;
2903 }
2904 }
2905 else
2906 {
2907 if (!childPrim)
2908 {
2909 if (Body != IntPtr.Zero && !m_disabled)
2910 d.BodyEnable(Body);
2911 }
2912// else if (_parent != null)
2913// ((OdePrim)_parent).ChildSelectedChange(false);
2914
2915 UpdateCollisionCatFlags();
2916 ApplyCollisionCatFlags();
2917
2918 m_delaySelect = false;
2919 }
2920
2921 resetCollisionAccounting();
2922 }
2923
2924 private void changePosition(Vector3 newPos)
2925 {
2926 CheckDelaySelect();
2927 if (m_isphysical)
2928 {
2929 if (childPrim) // inertia is messed, must rebuild
2930 {
2931 if (m_building)
2932 {
2933 _position = newPos;
2934 }
2935
2936 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2937 {
2938 FixInertia(newPos);
2939 if (!d.BodyIsEnabled(Body))
2940 d.BodyEnable(Body);
2941 }
2942 }
2943 else
2944 {
2945 if (_position != newPos)
2946 {
2947 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2948 _position = newPos;
2949 }
2950 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2951 d.BodyEnable(Body);
2952 }
2953 }
2954 else
2955 {
2956 if (prim_geom != IntPtr.Zero)
2957 {
2958 if (newPos != _position)
2959 {
2960 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2961 _position = newPos;
2962
2963 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2964 }
2965 }
2966 }
2967 givefakepos--;
2968 if (givefakepos < 0)
2969 givefakepos = 0;
2970// changeSelectedStatus();
2971 resetCollisionAccounting();
2972 }
2973
2974 private void changeOrientation(Quaternion newOri)
2975 {
2976 CheckDelaySelect();
2977 if (m_isphysical)
2978 {
2979 if (childPrim) // inertia is messed, must rebuild
2980 {
2981 if (m_building)
2982 {
2983 _orientation = newOri;
2984 }
2985/*
2986 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2987 {
2988 FixInertia(_position, newOri);
2989 if (!d.BodyIsEnabled(Body))
2990 d.BodyEnable(Body);
2991 }
2992*/
2993 }
2994 else
2995 {
2996 if (newOri != _orientation)
2997 {
2998 d.Quaternion myrot = new d.Quaternion();
2999 myrot.X = newOri.X;
3000 myrot.Y = newOri.Y;
3001 myrot.Z = newOri.Z;
3002 myrot.W = newOri.W;
3003 d.GeomSetQuaternion(prim_geom, ref myrot);
3004 _orientation = newOri;
3005 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
3006 createAMotor(m_angularlock);
3007 }
3008 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3009 d.BodyEnable(Body);
3010 }
3011 }
3012 else
3013 {
3014 if (prim_geom != IntPtr.Zero)
3015 {
3016 if (newOri != _orientation)
3017 {
3018 d.Quaternion myrot = new d.Quaternion();
3019 myrot.X = newOri.X;
3020 myrot.Y = newOri.Y;
3021 myrot.Z = newOri.Z;
3022 myrot.W = newOri.W;
3023 d.GeomSetQuaternion(prim_geom, ref myrot);
3024 _orientation = newOri;
3025 }
3026 }
3027 }
3028 givefakeori--;
3029 if (givefakeori < 0)
3030 givefakeori = 0;
3031 resetCollisionAccounting();
3032 }
3033
3034 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
3035 {
3036 CheckDelaySelect();
3037 if (m_isphysical)
3038 {
3039 if (childPrim && m_building) // inertia is messed, must rebuild
3040 {
3041 _position = newPos;
3042 _orientation = newOri;
3043 }
3044 else
3045 {
3046 if (newOri != _orientation)
3047 {
3048 d.Quaternion myrot = new d.Quaternion();
3049 myrot.X = newOri.X;
3050 myrot.Y = newOri.Y;
3051 myrot.Z = newOri.Z;
3052 myrot.W = newOri.W;
3053 d.GeomSetQuaternion(prim_geom, ref myrot);
3054 _orientation = newOri;
3055 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
3056 createAMotor(m_angularlock);
3057 }
3058 if (_position != newPos)
3059 {
3060 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3061 _position = newPos;
3062 }
3063 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3064 d.BodyEnable(Body);
3065 }
3066 }
3067 else
3068 {
3069 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
3070 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
3071
3072 if (prim_geom != IntPtr.Zero)
3073 {
3074 if (newOri != _orientation)
3075 {
3076 d.Quaternion myrot = new d.Quaternion();
3077 myrot.X = newOri.X;
3078 myrot.Y = newOri.Y;
3079 myrot.Z = newOri.Z;
3080 myrot.W = newOri.W;
3081 d.GeomSetQuaternion(prim_geom, ref myrot);
3082 _orientation = newOri;
3083 }
3084
3085 if (newPos != _position)
3086 {
3087 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3088 _position = newPos;
3089
3090 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
3091 }
3092 }
3093 }
3094 givefakepos--;
3095 if (givefakepos < 0)
3096 givefakepos = 0;
3097 givefakeori--;
3098 if (givefakeori < 0)
3099 givefakeori = 0;
3100 resetCollisionAccounting();
3101 }
3102
3103
3104 private void changeDisable(bool disable)
3105 {
3106 if (disable)
3107 {
3108 if (!m_disabled)
3109 disableBodySoft();
3110 }
3111 else
3112 {
3113 if (m_disabled)
3114 enableBodySoft();
3115 }
3116 }
3117
3118 private void changePhysicsStatus(bool NewStatus)
3119 {
3120 CheckDelaySelect();
3121
3122 m_isphysical = NewStatus;
3123
3124 if (!childPrim)
3125 {
3126 if (NewStatus)
3127 {
3128 if (Body == IntPtr.Zero)
3129 MakeBody();
3130 }
3131 else
3132 {
3133 if (Body != IntPtr.Zero)
3134 {
3135 DestroyBody();
3136 }
3137 Stop();
3138 }
3139 }
3140
3141 resetCollisionAccounting();
3142 }
3143
3144 private void changeprimsizeshape()
3145 {
3146 CheckDelaySelect();
3147
3148 OdePrim parent = (OdePrim)_parent;
3149
3150 bool chp = childPrim;
3151
3152 if (chp)
3153 {
3154 if (parent != null)
3155 {
3156 parent.DestroyBody();
3157 }
3158 }
3159 else
3160 {
3161 DestroyBody();
3162 }
3163
3164 RemoveGeom();
3165
3166 // we don't need to do space calculation because the client sends a position update also.
3167 if (_size.X <= 0)
3168 _size.X = 0.01f;
3169 if (_size.Y <= 0)
3170 _size.Y = 0.01f;
3171 if (_size.Z <= 0)
3172 _size.Z = 0.01f;
3173 // Construction of new prim
3174
3175 CreateGeom();
3176
3177 if (prim_geom != IntPtr.Zero)
3178 {
3179 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3180 d.Quaternion myrot = new d.Quaternion();
3181 myrot.X = _orientation.X;
3182 myrot.Y = _orientation.Y;
3183 myrot.Z = _orientation.Z;
3184 myrot.W = _orientation.W;
3185 d.GeomSetQuaternion(prim_geom, ref myrot);
3186 }
3187
3188 if (m_isphysical)
3189 {
3190 if (chp)
3191 {
3192 if (parent != null)
3193 {
3194 parent.MakeBody();
3195 }
3196 }
3197 else
3198 MakeBody();
3199 }
3200
3201 else
3202 {
3203 UpdateCollisionCatFlags();
3204 ApplyCollisionCatFlags();
3205 }
3206
3207 resetCollisionAccounting();
3208 }
3209
3210 private void changeSize(Vector3 newSize)
3211 {
3212 _size = newSize;
3213 changeprimsizeshape();
3214 }
3215
3216 private void changeShape(PrimitiveBaseShape newShape)
3217 {
3218 if(newShape != null)
3219 _pbs = newShape;
3220 changeprimsizeshape();
3221 }
3222
3223 private void changeFloatOnWater(bool newval)
3224 {
3225 m_collidesWater = newval;
3226
3227 UpdateCollisionCatFlags();
3228 ApplyCollisionCatFlags();
3229 }
3230
3231 private void changeSetTorque(Vector3 newtorque)
3232 {
3233 if (!m_isSelected)
3234 {
3235 if (m_isphysical && Body != IntPtr.Zero)
3236 {
3237 if (m_disabled)
3238 enableBodySoft();
3239 else if (!d.BodyIsEnabled(Body))
3240 d.BodyEnable(Body);
3241
3242 }
3243 _torque = newtorque;
3244 }
3245 }
3246
3247 private void changeForce(Vector3 force)
3248 {
3249 m_force = force;
3250 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3251 d.BodyEnable(Body);
3252 }
3253
3254
3255 private void changeAddImpulse(Vector3 impulse)
3256 {
3257 m_forceacc += impulse *m_invTimeStep;
3258 if (!m_isSelected)
3259 {
3260 lock (this)
3261 {
3262 //m_log.Info("[PHYSICS]: dequeing forcelist");
3263 if (m_isphysical && Body != IntPtr.Zero)
3264 {
3265 if (m_disabled)
3266 enableBodySoft();
3267 else if (!d.BodyIsEnabled(Body))
3268 d.BodyEnable(Body);
3269 }
3270 }
3271
3272 m_collisionscore = 0;
3273 }
3274 }
3275
3276 // actually angular impulse
3277 private void changeAddAngularImpulse(Vector3 aimpulse)
3278 {
3279 m_angularForceacc += aimpulse * m_invTimeStep;
3280 if (!m_isSelected)
3281 {
3282 lock (this)
3283 {
3284 if (m_isphysical && Body != IntPtr.Zero)
3285 {
3286 if (m_disabled)
3287 enableBodySoft();
3288 else if (!d.BodyIsEnabled(Body))
3289 d.BodyEnable(Body);
3290 }
3291 }
3292 m_collisionscore = 0;
3293 }
3294 }
3295
3296 private void changevelocity(Vector3 newVel)
3297 {
3298 if (!m_isSelected)
3299 {
3300 if (Body != IntPtr.Zero)
3301 {
3302 if (m_disabled)
3303 enableBodySoft();
3304 else if (!d.BodyIsEnabled(Body))
3305 d.BodyEnable(Body);
3306
3307 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3308 }
3309 //resetCollisionAccounting();
3310 }
3311 _velocity = newVel;
3312 }
3313
3314 private void changeVolumedetetion(bool newVolDtc)
3315 {
3316 m_isVolumeDetect = newVolDtc;
3317 m_fakeisVolumeDetect = newVolDtc;
3318 UpdateCollisionCatFlags();
3319 ApplyCollisionCatFlags();
3320 }
3321
3322 protected void changeBuilding(bool newbuilding)
3323 {
3324 // Check if we need to do anything
3325 if (newbuilding == m_building)
3326 return;
3327
3328 if ((bool)newbuilding)
3329 {
3330 m_building = true;
3331 if (!childPrim)
3332 DestroyBody();
3333 }
3334 else
3335 {
3336 m_building = false;
3337 CheckDelaySelect();
3338 if (!childPrim)
3339 MakeBody();
3340 }
3341 if (!childPrim && childrenPrim.Count > 0)
3342 {
3343 foreach (OdePrim prm in childrenPrim)
3344 prm.changeBuilding(m_building); // call directly
3345 }
3346 }
3347
3348 public void changeSetVehicle(VehicleData vdata)
3349 {
3350 if (m_vehicle == null)
3351 m_vehicle = new ODEDynamics(this);
3352 m_vehicle.DoSetVehicle(vdata);
3353 }
3354 private void changeVehicleType(int value)
3355 {
3356 if (value == (int)Vehicle.TYPE_NONE)
3357 {
3358 if (m_vehicle != null)
3359 m_vehicle = null;
3360 }
3361 else
3362 {
3363 if (m_vehicle == null)
3364 m_vehicle = new ODEDynamics(this);
3365
3366 m_vehicle.ProcessTypeChange((Vehicle)value);
3367 }
3368 }
3369
3370 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3371 {
3372 if (m_vehicle == null)
3373 return;
3374
3375 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3376 }
3377
3378 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3379 {
3380 if (m_vehicle == null)
3381 return;
3382 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3383 }
3384
3385 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3386 {
3387 if (m_vehicle == null)
3388 return;
3389 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3390 }
3391
3392 private void changeVehicleFlags(strVehicleBoolParam bp)
3393 {
3394 if (m_vehicle == null)
3395 return;
3396 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3397 }
3398
3399 #endregion
3400
3401 public void Move()
3402 {
3403 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3404 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3405 {
3406 if (!d.BodyIsEnabled(Body))
3407 {
3408 // let vehicles sleep
3409 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3410 return;
3411
3412 if (++bodydisablecontrol < 20)
3413 return;
3414
3415 bodydisablecontrol = 0;
3416 d.BodyEnable(Body);
3417 }
3418
3419 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3420
3421/* moved down to UpdateMove... where it belongs again
3422
3423 // check outside region
3424
3425 if (lpos.Z < -100 || lpos.Z > 100000f)
3426 {
3427 m_outbounds = true;
3428
3429 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3430 _acceleration.X = 0;
3431 _acceleration.Y = 0;
3432 _acceleration.Z = 0;
3433
3434 _velocity.X = 0;
3435 _velocity.Y = 0;
3436 _velocity.Z = 0;
3437 m_rotationalVelocity.X = 0;
3438 m_rotationalVelocity.Y = 0;
3439 m_rotationalVelocity.Z = 0;
3440
3441 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3442 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3443 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3444 m_lastposition = _position;
3445 m_lastorientation = _orientation;
3446
3447 base.RequestPhysicsterseUpdate();
3448
3449 throttleCounter = 0;
3450 _zeroFlag = true;
3451
3452 disableBodySoft(); // disable it and colisions
3453 base.RaiseOutOfBounds(_position);
3454 return;
3455 }
3456
3457 if (lpos.X < 0f)
3458 {
3459 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3460 m_outbounds = true;
3461 }
3462 else if (lpos.X > _parent_scene.WorldExtents.X)
3463 {
3464 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3465 m_outbounds = true;
3466 }
3467 if (lpos.Y < 0f)
3468 {
3469 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3470 m_outbounds = true;
3471 }
3472 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3473 {
3474 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3475 m_outbounds = true;
3476 }
3477
3478 if (m_outbounds)
3479 {
3480 m_lastposition = _position;
3481 m_lastorientation = _orientation;
3482
3483 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3484 m_rotationalVelocity.X = dtmp.X;
3485 m_rotationalVelocity.Y = dtmp.Y;
3486 m_rotationalVelocity.Z = dtmp.Z;
3487
3488 dtmp = d.BodyGetLinearVel(Body);
3489 _velocity.X = dtmp.X;
3490 _velocity.Y = dtmp.Y;
3491 _velocity.Z = dtmp.Z;
3492
3493 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3494 d.BodySetAngularVel(Body, 0, 0, 0);
3495 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3496 disableBodySoft(); // stop collisions
3497 base.RequestPhysicsterseUpdate();
3498 return;
3499 }
3500*/
3501 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3502 {
3503 // 'VEHICLES' are dealt with in ODEDynamics.cs
3504 m_vehicle.Step();
3505 return;
3506 }
3507
3508 float fx = 0;
3509 float fy = 0;
3510 float fz = 0;
3511
3512 float m_mass = _mass;
3513
3514 if (m_usePID && m_PIDTau > 0)
3515 {
3516 // for now position error
3517 _target_velocity =
3518 new Vector3(
3519 (m_PIDTarget.X - lpos.X),
3520 (m_PIDTarget.Y - lpos.Y),
3521 (m_PIDTarget.Z - lpos.Z)
3522 );
3523
3524 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3525 {
3526 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3527 d.BodySetLinearVel(Body, 0, 0, 0);
3528 return;
3529 }
3530 else
3531 {
3532 _zeroFlag = false;
3533
3534 float tmp = 1 / m_PIDTau;
3535 _target_velocity *= tmp;
3536
3537 // apply limits
3538 tmp = _target_velocity.Length();
3539 if (tmp > 50.0f)
3540 {
3541 tmp = 50 / tmp;
3542 _target_velocity *= tmp;
3543 }
3544 else if (tmp < 0.05f)
3545 {
3546 tmp = 0.05f / tmp;
3547 _target_velocity *= tmp;
3548 }
3549
3550 d.Vector3 vel = d.BodyGetLinearVel(Body);
3551 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3552 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3553 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3554// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3555 }
3556 } // end if (m_usePID)
3557
3558 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3559 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3560 {
3561
3562 // Non-Vehicles have a limited set of Hover options.
3563 // determine what our target height really is based on HoverType
3564
3565 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3566
3567 switch (m_PIDHoverType)
3568 {
3569 case PIDHoverType.Ground:
3570 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3571 break;
3572
3573 case PIDHoverType.GroundAndWater:
3574 m_waterHeight = _parent_scene.GetWaterLevel();
3575 if (m_groundHeight > m_waterHeight)
3576 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3577 else
3578 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3579 break;
3580 } // end switch (m_PIDHoverType)
3581
3582 // don't go underground unless volumedetector
3583
3584 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3585 {
3586 d.Vector3 vel = d.BodyGetLinearVel(Body);
3587
3588 fz = (m_targetHoverHeight - lpos.Z);
3589
3590 // if error is zero, use position control; otherwise, velocity control
3591 if (Math.Abs(fz) < 0.01f)
3592 {
3593 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3594 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3595 }
3596 else
3597 {
3598 _zeroFlag = false;
3599 fz /= m_PIDHoverTau;
3600
3601 float tmp = Math.Abs(fz);
3602 if (tmp > 50)
3603 fz = 50 * Math.Sign(fz);
3604 else if (tmp < 0.1)
3605 fz = 0.1f * Math.Sign(fz);
3606
3607 fz = ((fz - vel.Z) * m_invTimeStep);
3608 }
3609 }
3610 }
3611 else
3612 {
3613 float b = (1.0f - m_buoyancy);
3614 fx = _parent_scene.gravityx * b;
3615 fy = _parent_scene.gravityy * b;
3616 fz = _parent_scene.gravityz * b;
3617 }
3618
3619 fx *= m_mass;
3620 fy *= m_mass;
3621 fz *= m_mass;
3622
3623 // constant force
3624 fx += m_force.X;
3625 fy += m_force.Y;
3626 fz += m_force.Z;
3627
3628 fx += m_forceacc.X;
3629 fy += m_forceacc.Y;
3630 fz += m_forceacc.Z;
3631
3632 m_forceacc = Vector3.Zero;
3633
3634 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3635 if (fx != 0 || fy != 0 || fz != 0)
3636 {
3637 d.BodyAddForce(Body, fx, fy, fz);
3638 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3639 }
3640
3641 Vector3 trq;
3642
3643 trq = _torque;
3644 trq += m_angularForceacc;
3645 m_angularForceacc = Vector3.Zero;
3646 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3647 {
3648 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3649 }
3650 }
3651 else
3652 { // is not physical, or is not a body or is selected
3653 // _zeroPosition = d.BodyGetPosition(Body);
3654 return;
3655 //Console.WriteLine("Nothing " + Name);
3656
3657 }
3658 }
3659
3660 public void UpdatePositionAndVelocity()
3661 {
3662 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3663 {
3664 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3665 {
3666 bool lastZeroFlag = _zeroFlag;
3667
3668 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
3669
3670 // check outside region
3671 if (lpos.Z < -100 || lpos.Z > 100000f)
3672 {
3673 m_outbounds = true;
3674
3675 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3676 _acceleration.X = 0;
3677 _acceleration.Y = 0;
3678 _acceleration.Z = 0;
3679
3680 _velocity.X = 0;
3681 _velocity.Y = 0;
3682 _velocity.Z = 0;
3683 m_rotationalVelocity.X = 0;
3684 m_rotationalVelocity.Y = 0;
3685 m_rotationalVelocity.Z = 0;
3686
3687 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3688 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3689 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3690 m_lastposition = _position;
3691 m_lastorientation = _orientation;
3692
3693 base.RequestPhysicsterseUpdate();
3694
3695 throttleCounter = 0;
3696 _zeroFlag = true;
3697
3698 disableBodySoft(); // disable it and colisions
3699 base.RaiseOutOfBounds(_position);
3700 return;
3701 }
3702
3703 if (lpos.X < 0f)
3704 {
3705 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3706 m_outbounds = true;
3707 }
3708 else if (lpos.X > _parent_scene.WorldExtents.X)
3709 {
3710 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3711 m_outbounds = true;
3712 }
3713 if (lpos.Y < 0f)
3714 {
3715 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3716 m_outbounds = true;
3717 }
3718 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3719 {
3720 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3721 m_outbounds = true;
3722 }
3723
3724 if (m_outbounds)
3725 {
3726 m_lastposition = _position;
3727 m_lastorientation = _orientation;
3728
3729 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3730 m_rotationalVelocity.X = dtmp.X;
3731 m_rotationalVelocity.Y = dtmp.Y;
3732 m_rotationalVelocity.Z = dtmp.Z;
3733
3734 dtmp = d.BodyGetLinearVel(Body);
3735 _velocity.X = dtmp.X;
3736 _velocity.Y = dtmp.Y;
3737 _velocity.Z = dtmp.Z;
3738
3739 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3740 d.BodySetAngularVel(Body, 0, 0, 0);
3741 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3742 disableBodySoft(); // stop collisions
3743 base.RequestPhysicsterseUpdate();
3744 return;
3745 }
3746
3747 d.Quaternion ori;
3748 d.GeomCopyQuaternion(prim_geom, out ori);
3749
3750 // decide if moving
3751 // use positions since this are integrated quantities
3752 // tolerance values depende a lot on simulation noise...
3753 // use simple math.abs since we dont need to be exact
3754
3755 if (
3756 (Math.Abs(_position.X - lpos.X) < 0.001f)
3757 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3758 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3759 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3760 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3761 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3762 )
3763 {
3764 _zeroFlag = true;
3765 }
3766 else
3767 _zeroFlag = false;
3768
3769 // update velocities and aceleration
3770 if (!(_zeroFlag && lastZeroFlag))
3771 {
3772 d.Vector3 vel = d.BodyGetLinearVel(Body);
3773
3774 _acceleration = _velocity;
3775
3776 if ((Math.Abs(vel.X) < 0.001f) &&
3777 (Math.Abs(vel.Y) < 0.001f) &&
3778 (Math.Abs(vel.Z) < 0.001f))
3779 {
3780 _velocity = Vector3.Zero;
3781 float t = -m_invTimeStep;
3782 _acceleration = _acceleration * t;
3783 }
3784 else
3785 {
3786 _velocity.X = vel.X;
3787 _velocity.Y = vel.Y;
3788 _velocity.Z = vel.Z;
3789 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3790 }
3791
3792 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3793 (Math.Abs(_acceleration.Y) < 0.01f) &&
3794 (Math.Abs(_acceleration.Z) < 0.01f))
3795 {
3796 _acceleration = Vector3.Zero;
3797 }
3798
3799 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3800 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3801 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3802 )
3803 {
3804 m_rotationalVelocity = Vector3.Zero;
3805 }
3806 else
3807 {
3808 vel = d.BodyGetAngularVel(Body);
3809 m_rotationalVelocity.X = vel.X;
3810 m_rotationalVelocity.Y = vel.Y;
3811 m_rotationalVelocity.Z = vel.Z;
3812 }
3813 }
3814
3815 if (_zeroFlag)
3816 {
3817 if (lastZeroFlag)
3818 {
3819 _velocity = Vector3.Zero;
3820 _acceleration = Vector3.Zero;
3821 m_rotationalVelocity = Vector3.Zero;
3822 }
3823
3824 if (!m_lastUpdateSent)
3825 {
3826 base.RequestPhysicsterseUpdate();
3827 if (lastZeroFlag)
3828 m_lastUpdateSent = true;
3829 }
3830 return;
3831 }
3832
3833 _position.X = lpos.X;
3834 _position.Y = lpos.Y;
3835 _position.Z = lpos.Z;
3836
3837 _orientation.X = ori.X;
3838 _orientation.Y = ori.Y;
3839 _orientation.Z = ori.Z;
3840 _orientation.W = ori.W;
3841 base.RequestPhysicsterseUpdate();
3842 m_lastUpdateSent = false;
3843 }
3844 }
3845 }
3846
3847 internal static bool QuaternionIsFinite(Quaternion q)
3848 {
3849 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3850 return false;
3851 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3852 return false;
3853 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3854 return false;
3855 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3856 return false;
3857 return true;
3858 }
3859
3860 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
3861 {
3862 // assumes object center of mass is zero
3863 float smass = part.mass;
3864 theobj.mass -= smass;
3865
3866 smass *= 1.0f / (theobj.mass); ;
3867
3868 theobj.c.X -= part.c.X * smass;
3869 theobj.c.Y -= part.c.Y * smass;
3870 theobj.c.Z -= part.c.Z * smass;
3871
3872 theobj.I.M00 -= part.I.M00;
3873 theobj.I.M01 -= part.I.M01;
3874 theobj.I.M02 -= part.I.M02;
3875 theobj.I.M10 -= part.I.M10;
3876 theobj.I.M11 -= part.I.M11;
3877 theobj.I.M12 -= part.I.M12;
3878 theobj.I.M20 -= part.I.M20;
3879 theobj.I.M21 -= part.I.M21;
3880 theobj.I.M22 -= part.I.M22;
3881 }
3882
3883 private void donullchange()
3884 {
3885 }
3886
3887 public bool DoAChange(changes what, object arg)
3888 {
3889 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3890 {
3891 return false;
3892 }
3893
3894 // nasty switch
3895 switch (what)
3896 {
3897 case changes.Add:
3898 changeadd();
3899 break;
3900 case changes.Remove:
3901 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3902 //When we return true, it destroys all of the prims in the linkset anyway
3903 if (_parent != null)
3904 {
3905 OdePrim parent = (OdePrim)_parent;
3906 parent.ChildRemove(this, false);
3907 }
3908 else
3909 ChildRemove(this, false);
3910
3911 m_vehicle = null;
3912 RemoveGeom();
3913 m_targetSpace = IntPtr.Zero;
3914 if (m_eventsubscription > 0)
3915 UnSubscribeEvents();
3916 return true;
3917
3918 case changes.Link:
3919 OdePrim tmp = (OdePrim)arg;
3920 changeLink(tmp);
3921 break;
3922
3923 case changes.DeLink:
3924 changeLink(null);
3925 break;
3926
3927 case changes.Position:
3928 changePosition((Vector3)arg);
3929 break;
3930
3931 case changes.Orientation:
3932 changeOrientation((Quaternion)arg);
3933 break;
3934
3935 case changes.PosOffset:
3936 donullchange();
3937 break;
3938
3939 case changes.OriOffset:
3940 donullchange();
3941 break;
3942
3943 case changes.Velocity:
3944 changevelocity((Vector3)arg);
3945 break;
3946
3947// case changes.Acceleration:
3948// changeacceleration((Vector3)arg);
3949// break;
3950// case changes.AngVelocity:
3951// changeangvelocity((Vector3)arg);
3952// break;
3953
3954 case changes.Force:
3955 changeForce((Vector3)arg);
3956 break;
3957
3958 case changes.Torque:
3959 changeSetTorque((Vector3)arg);
3960 break;
3961
3962 case changes.AddForce:
3963 changeAddImpulse((Vector3)arg);
3964 break;
3965
3966 case changes.AddAngForce:
3967 changeAddAngularImpulse((Vector3)arg);
3968 break;
3969
3970 case changes.AngLock:
3971 changeAngularLock((Vector3)arg);
3972 break;
3973
3974 case changes.Size:
3975 changeSize((Vector3)arg);
3976 break;
3977
3978 case changes.Shape:
3979 changeShape((PrimitiveBaseShape)arg);
3980 break;
3981
3982 case changes.CollidesWater:
3983 changeFloatOnWater((bool)arg);
3984 break;
3985
3986 case changes.VolumeDtc:
3987 changeVolumedetetion((bool)arg);
3988 break;
3989
3990 case changes.Phantom:
3991 changePhantomStatus((bool)arg);
3992 break;
3993
3994 case changes.Physical:
3995 changePhysicsStatus((bool)arg);
3996 break;
3997
3998 case changes.Selected:
3999 changeSelectedStatus((bool)arg);
4000 break;
4001
4002 case changes.disabled:
4003 changeDisable((bool)arg);
4004 break;
4005
4006 case changes.building:
4007 changeBuilding((bool)arg);
4008 break;
4009
4010 case changes.VehicleType:
4011 changeVehicleType((int)arg);
4012 break;
4013
4014 case changes.VehicleFlags:
4015 changeVehicleFlags((strVehicleBoolParam) arg);
4016 break;
4017
4018 case changes.VehicleFloatParam:
4019 changeVehicleFloatParam((strVehicleFloatParam) arg);
4020 break;
4021
4022 case changes.VehicleVectorParam:
4023 changeVehicleVectorParam((strVehicleVectorParam) arg);
4024 break;
4025
4026 case changes.VehicleRotationParam:
4027 changeVehicleRotationParam((strVehicleQuatParam) arg);
4028 break;
4029
4030 case changes.SetVehicle:
4031 changeSetVehicle((VehicleData) arg);
4032 break;
4033 case changes.Null:
4034 donullchange();
4035 break;
4036
4037 default:
4038 donullchange();
4039 break;
4040 }
4041 return false;
4042 }
4043
4044 public void AddChange(changes what, object arg)
4045 {
4046 _parent_scene.AddChange((PhysicsActor) this, what, arg);
4047 }
4048
4049
4050 private struct strVehicleBoolParam
4051 {
4052 public int param;
4053 public bool value;
4054 }
4055
4056 private struct strVehicleFloatParam
4057 {
4058 public int param;
4059 public float value;
4060 }
4061
4062 private struct strVehicleQuatParam
4063 {
4064 public int param;
4065 public Quaternion value;
4066 }
4067
4068 private struct strVehicleVectorParam
4069 {
4070 public int param;
4071 public Vector3 value;
4072 }
4073 }
4074}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..5122ebf
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,603 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// Processes raycast requests as ODE is in a state to be able to do them.
43 /// This ensures that it's thread safe and there will be no conflicts.
44 /// Requests get returned by a different thread then they were requested by.
45 /// </summary>
46 public class ODERayCastRequestManager
47 {
48 /// <summary>
49 /// Pending ray requests
50 /// </summary>
51 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
52
53 /// <summary>
54 /// Scene that created this object.
55 /// </summary>
56 private OdeScene m_scene;
57
58 IntPtr ray; // the ray. we only need one for our lifetime
59
60 private const int ColisionContactGeomsPerTest = 5;
61 private const int DefaultMaxCount = 25;
62 private const int MaxTimePerCallMS = 30;
63
64 /// <summary>
65 /// ODE near callback delegate
66 /// </summary>
67 private d.NearCallback nearCallback;
68 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
69 private List<ContactResult> m_contactResults = new List<ContactResult>();
70 private RayFilterFlags CurrentRayFilter;
71 private int CurrentMaxCount;
72
73 public ODERayCastRequestManager(OdeScene pScene)
74 {
75 m_scene = pScene;
76 nearCallback = near;
77 ray = d.CreateRay(IntPtr.Zero, 1.0f);
78 d.GeomSetCategoryBits(ray,0);
79 }
80
81 /// <summary>
82 /// Queues request for a raycast to all world
83 /// </summary>
84 /// <param name="position">Origin of Ray</param>
85 /// <param name="direction">Ray direction</param>
86 /// <param name="length">Ray length</param>
87 /// <param name="retMethod">Return method to send the results</param>
88 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
89 {
90 ODERayRequest req = new ODERayRequest();
91 req.geom = IntPtr.Zero;
92 req.callbackMethod = retMethod;
93 req.Count = DefaultMaxCount;
94 req.length = length;
95 req.Normal = direction;
96 req.Origin = position;
97 req.filter = RayFilterFlags.AllPrims;
98
99 m_PendingRequests.Enqueue(req);
100 }
101
102 /// <summary>
103 /// Queues request for a raycast to particular part
104 /// </summary>
105 /// <param name="position">Origin of Ray</param>
106 /// <param name="direction">Ray direction</param>
107 /// <param name="length">Ray length</param>
108 /// <param name="retMethod">Return method to send the results</param>
109 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
110 {
111 ODERayRequest req = new ODERayRequest();
112 req.geom = geom;
113 req.callbackMethod = retMethod;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117 req.Count = DefaultMaxCount;
118 req.filter = RayFilterFlags.AllPrims;
119
120 m_PendingRequests.Enqueue(req);
121 }
122
123 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
124 {
125 ODERayRequest req = new ODERayRequest();
126 req.geom = IntPtr.Zero;
127 req.callbackMethod = retMethod;
128 req.Count = DefaultMaxCount;
129 req.length = length;
130 req.Normal = direction;
131 req.Origin = position;
132 req.filter = RayFilterFlags.AllPrims;
133
134 m_PendingRequests.Enqueue(req);
135 }
136
137 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
138 {
139 ODERayRequest req = new ODERayRequest();
140 req.geom = geom;
141 req.callbackMethod = retMethod;
142 req.length = length;
143 req.Normal = direction;
144 req.Origin = position;
145 req.Count = DefaultMaxCount;
146 req.filter = RayFilterFlags.AllPrims;
147
148 m_PendingRequests.Enqueue(req);
149 }
150
151 /// <summary>
152 /// Queues a raycast
153 /// </summary>
154 /// <param name="position">Origin of Ray</param>
155 /// <param name="direction">Ray normal</param>
156 /// <param name="length">Ray length</param>
157 /// <param name="count"></param>
158 /// <param name="retMethod">Return method to send the results</param>
159 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
160 {
161 ODERayRequest req = new ODERayRequest();
162 req.geom = IntPtr.Zero;
163 req.callbackMethod = retMethod;
164 req.length = length;
165 req.Normal = direction;
166 req.Origin = position;
167 req.Count = count;
168 req.filter = RayFilterFlags.AllPrims;
169
170 m_PendingRequests.Enqueue(req);
171 }
172
173
174 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod)
175 {
176 ODERayRequest req = new ODERayRequest();
177 req.geom = IntPtr.Zero;
178 req.callbackMethod = retMethod;
179 req.length = length;
180 req.Normal = direction;
181 req.Origin = position;
182 req.Count = count;
183 req.filter = filter;
184
185 m_PendingRequests.Enqueue(req);
186 }
187
188 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
189 {
190 ODERayRequest req = new ODERayRequest();
191 req.geom = geom;
192 req.callbackMethod = retMethod;
193 req.length = length;
194 req.Normal = direction;
195 req.Origin = position;
196 req.Count = count;
197 req.filter = RayFilterFlags.AllPrims;
198
199 m_PendingRequests.Enqueue(req);
200 }
201
202 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
203 {
204 ODERayRequest req = new ODERayRequest();
205 req.geom = IntPtr.Zero;
206 req.callbackMethod = retMethod;
207 req.length = length;
208 req.Normal = direction;
209 req.Origin = position;
210 req.Count = count;
211 req.filter = RayFilterFlags.AllPrims;
212
213 m_PendingRequests.Enqueue(req);
214 }
215
216 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
217 {
218 ODERayRequest req = new ODERayRequest();
219 req.geom = geom;
220 req.callbackMethod = retMethod;
221 req.length = length;
222 req.Normal = direction;
223 req.Origin = position;
224 req.Count = count;
225 req.filter = RayFilterFlags.AllPrims;
226
227 m_PendingRequests.Enqueue(req);
228 }
229
230 /// <summary>
231 /// Process all queued raycast requests
232 /// </summary>
233 /// <returns>Time in MS the raycasts took to process.</returns>
234 public int ProcessQueuedRequests()
235 {
236
237 if (m_PendingRequests.Count <= 0)
238 return 0;
239
240 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
241 // oops something got wrong or scene isn't ready still
242 {
243 m_PendingRequests.Clear();
244 return 0;
245 }
246
247 int time = Util.EnvironmentTickCount();
248
249 ODERayRequest req;
250 int closestHit;
251 int backfacecull;
252 CollisionCategories catflags;
253
254 while (m_PendingRequests.Dequeue(out req))
255 {
256 if (req.callbackMethod != null)
257 {
258 CurrentRayFilter = req.filter;
259 CurrentMaxCount = req.Count;
260
261 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
262 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
263
264 d.GeomRaySetLength(ray, req.length);
265 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
266 d.GeomRaySetParams(ray, 0, backfacecull);
267 d.GeomRaySetClosestHit(ray, closestHit);
268
269 if (req.callbackMethod is RaycastCallback)
270 // if we only want one get only one per colision pair saving memory
271 CurrentRayFilter |= RayFilterFlags.ClosestHit;
272
273 if (req.geom == IntPtr.Zero)
274 {
275 // translate ray filter to colision flags
276 catflags = 0;
277 if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
278 catflags |= CollisionCategories.VolumeDtc;
279 if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
280 catflags |= CollisionCategories.Phantom;
281 if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
282 catflags |= CollisionCategories.Character;
283 if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
284 catflags |= CollisionCategories.Geom;
285 if ((CurrentRayFilter & RayFilterFlags.land) != 0)
286 catflags |= CollisionCategories.Land;
287 if ((CurrentRayFilter & RayFilterFlags.water) != 0)
288 catflags |= CollisionCategories.Water;
289
290 if (catflags != 0)
291 doSpaceRay(req);
292 }
293 else
294 {
295 // if we select a geom don't use filters
296 d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
297 doGeomRay(req);
298 }
299 }
300
301 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
302 break;
303 }
304
305 lock (m_contactResults)
306 m_contactResults.Clear();
307
308 return Util.EnvironmentTickCountSubtract(time);
309 }
310 /// <summary>
311 /// Method that actually initiates the raycast with spaces
312 /// </summary>
313 /// <param name="req"></param>
314 ///
315
316 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
317 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
318
319 private void doSpaceRay(ODERayRequest req)
320 {
321 // Collide tests
322 if ((CurrentRayFilter & FilterActiveSpace) != 0)
323 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
324 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
325 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
326
327 if (req.callbackMethod is RaycastCallback)
328 {
329 // Define default results
330 bool hitYN = false;
331 uint hitConsumerID = 0;
332 float distance = float.MaxValue;
333 Vector3 closestcontact = Vector3.Zero;
334 Vector3 snormal = Vector3.Zero;
335
336 // Find closest contact and object.
337 lock (m_contactResults)
338 {
339 foreach (ContactResult cResult in m_contactResults)
340 {
341 if(cResult.Depth < distance)
342 {
343 closestcontact = cResult.Pos;
344 hitConsumerID = cResult.ConsumerID;
345 distance = cResult.Depth;
346 snormal = cResult.Normal;
347 }
348 }
349 m_contactResults.Clear();
350 }
351
352 if (distance > 0 && distance < float.MaxValue)
353 hitYN = true;
354 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
355 }
356 else
357 {
358 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
359 lock (m_PendingRequests)
360 {
361 cresult.AddRange(m_contactResults);
362 m_contactResults.Clear();
363 }
364 ((RayCallback)req.callbackMethod)(cresult);
365 }
366 }
367
368 /// <summary>
369 /// Method that actually initiates the raycast with a geom
370 /// </summary>
371 /// <param name="req"></param>
372 private void doGeomRay(ODERayRequest req)
373 {
374 // Collide test
375 d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
376
377 if (req.callbackMethod is RaycastCallback)
378 {
379 // Define default results
380 bool hitYN = false;
381 uint hitConsumerID = 0;
382 float distance = float.MaxValue;
383 Vector3 closestcontact = Vector3.Zero;
384 Vector3 snormal = Vector3.Zero;
385
386 // Find closest contact and object.
387 lock (m_contactResults)
388 {
389 foreach (ContactResult cResult in m_contactResults)
390 {
391 if(cResult.Depth < distance )
392 {
393 closestcontact = cResult.Pos;
394 hitConsumerID = cResult.ConsumerID;
395 distance = cResult.Depth;
396 snormal = cResult.Normal;
397 }
398 }
399 m_contactResults.Clear();
400 }
401
402 if (distance > 0 && distance < float.MaxValue)
403 hitYN = true;
404
405 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
406 }
407 else
408 {
409 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
410 lock (m_PendingRequests)
411 {
412 cresult.AddRange(m_contactResults);
413 m_contactResults.Clear();
414 }
415 ((RayCallback)req.callbackMethod)(cresult);
416 }
417 }
418
419 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
420 {
421 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
422 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
423 return false;
424
425 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
426 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
427 return true;
428 }
429
430 // This is the standard Near. g1 is the ray
431 private void near(IntPtr space, IntPtr g1, IntPtr g2)
432 {
433 if (g2 == IntPtr.Zero || g1 == g2)
434 return;
435
436 if (m_contactResults.Count >= CurrentMaxCount)
437 return;
438
439 if (d.GeomIsSpace(g2))
440 {
441 try
442 {
443 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
444 }
445 catch (Exception e)
446 {
447 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
448 }
449 return;
450 }
451
452 int count = 0;
453 try
454 {
455 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
456 }
457 catch (Exception e)
458 {
459 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
460 return;
461 }
462
463 if (count == 0)
464 return;
465
466 uint ID = 0;
467 PhysicsActor p2 = null;
468
469 m_scene.actor_name_map.TryGetValue(g2, out p2);
470
471 if (p2 == null)
472 {
473 string name;
474
475 if (!m_scene.geom_name_map.TryGetValue(g2, out name))
476 return;
477
478 if (name == "Terrain")
479 {
480 // land colision
481 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
482 return;
483 }
484 else if (name == "Water")
485 {
486 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
487 return;
488 }
489 else
490 return;
491 }
492 else
493 {
494 if (p2 is OdePrim)
495 {
496 RayFilterFlags thisFlags;
497
498 if (p2.IsPhysical)
499 thisFlags = RayFilterFlags.physical;
500 else
501 thisFlags = RayFilterFlags.nonphysical;
502
503 if (p2.Phantom)
504 thisFlags |= RayFilterFlags.phantom;
505
506 if (p2.IsVolumeDtc)
507 thisFlags |= RayFilterFlags.volumedtc;
508
509 if ((thisFlags & CurrentRayFilter) == 0)
510 return;
511
512 ID = ((OdePrim)p2).m_localID;
513 }
514 else if (p2 is OdeCharacter)
515 {
516 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
517 return;
518 else
519 ID = ((OdeCharacter)p2).m_localID;
520 }
521 else //??
522 return;
523 }
524
525 d.ContactGeom curcontact = new d.ContactGeom();
526
527 // closestHit for now only works for meshs, so must do it for others
528 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
529 {
530 // Loop all contacts, build results.
531 for (int i = 0; i < count; i++)
532 {
533 if (!GetCurContactGeom(i, ref curcontact))
534 break;
535
536 ContactResult collisionresult = new ContactResult();
537 collisionresult.ConsumerID = ID;
538 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
539 collisionresult.Depth = curcontact.depth;
540 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
541 curcontact.normal.Z);
542 lock (m_contactResults)
543 {
544 m_contactResults.Add(collisionresult);
545 if (m_contactResults.Count >= CurrentMaxCount)
546 return;
547 }
548 }
549 }
550 else
551 {
552 // keep only closest contact
553 ContactResult collisionresult = new ContactResult();
554 collisionresult.ConsumerID = ID;
555 collisionresult.Depth = float.MaxValue;
556
557 for (int i = 0; i < count; i++)
558 {
559 if (!GetCurContactGeom(i, ref curcontact))
560 break;
561
562 if (curcontact.depth < collisionresult.Depth)
563 {
564 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
565 collisionresult.Depth = curcontact.depth;
566 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
567 curcontact.normal.Z);
568 }
569 }
570
571 if (collisionresult.Depth != float.MaxValue)
572 {
573 lock (m_contactResults)
574 m_contactResults.Add(collisionresult);
575 }
576 }
577 }
578
579 /// <summary>
580 /// Dereference the creator scene so that it can be garbage collected if needed.
581 /// </summary>
582 internal void Dispose()
583 {
584 m_scene = null;
585 if (ray != IntPtr.Zero)
586 {
587 d.GeomDestroy(ray);
588 ray = IntPtr.Zero;
589 }
590 }
591 }
592
593 public struct ODERayRequest
594 {
595 public IntPtr geom;
596 public Vector3 Origin;
597 public Vector3 Normal;
598 public int Count;
599 public float length;
600 public object callbackMethod;
601 public RayFilterFlags filter;
602 }
603} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
new file mode 100644
index 0000000..0e4961b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,2005 @@
1/*
2 * based on:
3 * Ode.NET - .NET bindings for ODE
4 * Jason Perkins (starkos@industriousone.com)
5 * Licensed under the New BSD
6 * Part of the OpenDynamicsEngine
7Open Dynamics Engine
8Copyright (c) 2001-2007, Russell L. Smith.
9All rights reserved.
10
11Redistribution and use in source and binary forms, with or without
12modification, are permitted provided that the following conditions
13are met:
14
15Redistributions of source code must retain the above copyright notice,
16this list of conditions and the following disclaimer.
17
18Redistributions in binary form must reproduce the above copyright notice,
19this list of conditions and the following disclaimer in the documentation
20and/or other materials provided with the distribution.
21
22Neither the names of ODE's copyright owner nor the names of its
23contributors may be used to endorse or promote products derived from
24this software without specific prior written permission.
25
26THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
30OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
31SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
32TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
33PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
34LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
35NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
36SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
37 *
38 * changes by opensim team;
39 * changes by Aurora team http://www.aurora-sim.org/
40
41 * Revision/fixs by Ubit Umarov
42 */
43
44using System;
45using System.Runtime.InteropServices;
46using System.Security;
47
48namespace OdeAPI
49{
50//#if dDOUBLE
51// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
52// at least we save same memory and memory access time, FPU performance on intel usually is similar
53// using dReal = System.Double;
54//#else
55 using dReal = System.Single;
56//#endif
57
58 public static class d
59 {
60 public static dReal Infinity = dReal.MaxValue;
61 public static int NTotalBodies = 0;
62 public static int NTotalGeoms = 0;
63
64 #region Flags and Enumerations
65
66 [Flags]
67 public enum AllocateODEDataFlags : uint
68 {
69 BasicData = 0,
70 CollisionData = 0x00000001,
71 All = ~0u
72 }
73
74 [Flags]
75 public enum IniteODEFlags : uint
76 {
77 dInitFlagManualThreadCleanup = 0x00000001
78 }
79
80 [Flags]
81 public enum ContactFlags : int
82 {
83 Mu2 = 0x001,
84 FDir1 = 0x002,
85 Bounce = 0x004,
86 SoftERP = 0x008,
87 SoftCFM = 0x010,
88 Motion1 = 0x020,
89 Motion2 = 0x040,
90 MotionN = 0x080,
91 Slip1 = 0x100,
92 Slip2 = 0x200,
93 Approx0 = 0x0000,
94 Approx1_1 = 0x1000,
95 Approx1_2 = 0x2000,
96 Approx1 = 0x3000
97 }
98
99 public enum GeomClassID : int
100 {
101 SphereClass,
102 BoxClass,
103 CapsuleClass,
104 CylinderClass,
105 PlaneClass,
106 RayClass,
107 ConvexClass,
108 GeomTransformClass,
109 TriMeshClass,
110 HeightfieldClass,
111 FirstSpaceClass,
112 SimpleSpaceClass = FirstSpaceClass,
113 HashSpaceClass,
114 QuadTreeSpaceClass,
115 LastSpaceClass = QuadTreeSpaceClass,
116 UbitTerrainClass,
117 FirstUserClass,
118 LastUserClass = FirstUserClass + MaxUserClasses - 1,
119 NumClasses,
120 MaxUserClasses = 5
121 }
122
123 public enum JointType : int
124 {
125 None,
126 Ball,
127 Hinge,
128 Slider,
129 Contact,
130 Universal,
131 Hinge2,
132 Fixed,
133 Null,
134 AMotor,
135 LMotor,
136 Plane2D
137 }
138
139 public enum JointParam : int
140 {
141 LoStop,
142 HiStop,
143 Vel,
144 FMax,
145 FudgeFactor,
146 Bounce,
147 CFM,
148 StopERP,
149 StopCFM,
150 SuspensionERP,
151 SuspensionCFM,
152 LoStop2 = 256,
153 HiStop2,
154 Vel2,
155 FMax2,
156 FudgeFactor2,
157 Bounce2,
158 CFM2,
159 StopERP2,
160 StopCFM2,
161 SuspensionERP2,
162 SuspensionCFM2,
163 LoStop3 = 512,
164 HiStop3,
165 Vel3,
166 FMax3,
167 FudgeFactor3,
168 Bounce3,
169 CFM3,
170 StopERP3,
171 StopCFM3,
172 SuspensionERP3,
173 SuspensionCFM3
174 }
175
176 public enum dSweepAndPruneAxis : int
177 {
178 XYZ = ((0)|(1<<2)|(2<<4)),
179 XZY = ((0)|(2<<2)|(1<<4)),
180 YXZ = ((1)|(0<<2)|(2<<4)),
181 YZX = ((1)|(2<<2)|(0<<4)),
182 ZXY = ((2)|(0<<2)|(1<<4)),
183 ZYX = ((2)|(1<<2)|(0<<4))
184 }
185
186 #endregion
187
188 #region Callbacks
189
190 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
191 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
192
193 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
194 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
195
196 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
197 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
198
199 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
200 public delegate ColliderFn GetColliderFnFn(int num);
201
202 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
203 public delegate void GeomDtorFn(IntPtr o);
204
205 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
206 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
207
208 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
209 public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z);
210
211 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
212 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
213
214 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
215 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
216
217 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
218 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
219
220 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
221 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
222
223 #endregion
224
225 #region Structs
226
227 [StructLayout(LayoutKind.Sequential)]
228 public struct AABB
229 {
230 public dReal MinX, MaxX;
231 public dReal MinY, MaxY;
232 public dReal MinZ, MaxZ;
233 }
234
235
236 [StructLayout(LayoutKind.Sequential)]
237 public struct Contact
238 {
239 public SurfaceParameters surface;
240 public ContactGeom geom;
241 public Vector3 fdir1;
242 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
243 }
244
245
246 [StructLayout(LayoutKind.Sequential)]
247 public struct ContactGeom
248 {
249
250 public Vector3 pos;
251 public Vector3 normal;
252 public dReal depth;
253 public IntPtr g1;
254 public IntPtr g2;
255 public int side1;
256 public int side2;
257 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
258 }
259
260 [StructLayout(LayoutKind.Sequential)]
261 public struct GeomClass
262 {
263 public int bytes;
264 public GetColliderFnFn collider;
265 public GetAABBFn aabb;
266 public AABBTestFn aabb_test;
267 public GeomDtorFn dtor;
268 }
269
270
271 [StructLayout(LayoutKind.Sequential)]
272 public struct JointFeedback
273 {
274 public Vector3 f1;
275 public Vector3 t1;
276 public Vector3 f2;
277 public Vector3 t2;
278 }
279
280
281 [StructLayout(LayoutKind.Sequential)]
282 public struct Mass
283 {
284 public dReal mass;
285 public Vector4 c;
286 public Matrix3 I;
287 }
288
289
290 [StructLayout(LayoutKind.Sequential)]
291 public struct Matrix3
292 {
293 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
294 {
295 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
296 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
297 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
298 }
299 public dReal M00, M10, M20;
300 private dReal _m30;
301 public dReal M01, M11, M21;
302 private dReal _m31;
303 public dReal M02, M12, M22;
304 private dReal _m32;
305 }
306
307 [StructLayout(LayoutKind.Sequential)]
308 public struct Matrix4
309 {
310 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
311 dReal m01, dReal m11, dReal m21, dReal m31,
312 dReal m02, dReal m12, dReal m22, dReal m32,
313 dReal m03, dReal m13, dReal m23, dReal m33)
314 {
315 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
316 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
317 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
318 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
319 }
320 public dReal M00, M10, M20, M30;
321 public dReal M01, M11, M21, M31;
322 public dReal M02, M12, M22, M32;
323 public dReal M03, M13, M23, M33;
324 }
325
326 [StructLayout(LayoutKind.Sequential)]
327 public struct Quaternion
328 {
329 public dReal W, X, Y, Z;
330 }
331
332
333 [StructLayout(LayoutKind.Sequential)]
334 public struct SurfaceParameters
335 {
336 public ContactFlags mode;
337 public dReal mu;
338 public dReal mu2;
339 public dReal bounce;
340 public dReal bounce_vel;
341 public dReal soft_erp;
342 public dReal soft_cfm;
343 public dReal motion1;
344 public dReal motion2;
345 public dReal motionN;
346 public dReal slip1;
347 public dReal slip2;
348 }
349
350
351 [StructLayout(LayoutKind.Sequential)]
352 public struct Vector3
353 {
354 public Vector3(dReal x, dReal y, dReal z)
355 {
356 X = x; Y = y; Z = z; _w = 0.0f;
357 }
358 public dReal X, Y, Z;
359 private dReal _w;
360 }
361
362
363 [StructLayout(LayoutKind.Sequential)]
364 public struct Vector4
365 {
366 public Vector4(dReal x, dReal y, dReal z, dReal w)
367 {
368 X = x; Y = y; Z = z; W = w;
369 }
370 public dReal X, Y, Z, W;
371 }
372
373 #endregion
374
375 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
376 public static extern int AllocateODEDataForThread(uint ODEInitFlags);
377
378 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
379 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
380
381 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
382 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
383
384 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
385 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
386
387 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
388 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
389
390 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
391 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
392
393 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
394 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
395
396 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
397 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
398
399 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
400 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
401
402 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
403 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
404
405 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
406 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
407
408 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
409 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
410
411 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
412 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
413
414 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
415 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
416
417 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
418 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
419
420 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
421 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
422
423 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
424 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
425
426 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
427 public static extern IntPtr BodyiCreate(IntPtr world);
428 public static IntPtr BodyCreate(IntPtr world)
429 {
430 NTotalBodies++;
431 return BodyiCreate(world);
432 }
433
434 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
435 public static extern void BodyiDestroy(IntPtr body);
436 public static void BodyDestroy(IntPtr body)
437 {
438 NTotalBodies--;
439 BodyiDestroy(body);
440 }
441
442 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
443 public static extern void BodyDisable(IntPtr body);
444
445 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
446 public static extern void BodyEnable(IntPtr body);
447
448 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
449 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
450
451 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
452 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
453
454 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
455 public static extern void BodyGetAutoDisableDefaults(IntPtr body);
456
457 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
458 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
459
460 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
461 public static extern int BodyGetAutoDisableSteps(IntPtr body);
462
463 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
464 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
465
466 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
467 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
468 public static Vector3 BodyGetAngularVel(IntPtr body)
469 {
470 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
471 }
472
473 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
474 public static extern IntPtr BodyGetData(IntPtr body);
475
476 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
477 public static extern int BodyGetFiniteRotationMode(IntPtr body);
478
479 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
480 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
481
482 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
483 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
484 public static Vector3 BodyGetForce(IntPtr body)
485 {
486 unsafe { return *(BodyGetForceUnsafe(body)); }
487 }
488
489 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
490 public static extern bool BodyGetGravityMode(IntPtr body);
491
492 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
493 public static extern int BodyGetGyroscopicMode(IntPtr body);
494
495 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
496 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
497
498 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
499 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
500 public static Vector3 BodyGetLinearVel(IntPtr body)
501 {
502 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
503 }
504
505 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
506 public static extern void BodyGetMass(IntPtr body, out Mass mass);
507
508 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
509 public static extern int BodyGetNumJoints(IntPtr body);
510
511 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
512 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
513
514 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
515 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
516 public static Vector3 BodyGetPosition(IntPtr body)
517 {
518 unsafe { return *(BodyGetPositionUnsafe(body)); }
519 }
520
521 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
522 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
523
524 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
525 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
526 public static Quaternion BodyGetQuaternion(IntPtr body)
527 {
528 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
529 }
530
531 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
532 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
533
534 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
535 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
536
537 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
538 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
539 public static Matrix3 BodyGetRotation(IntPtr body)
540 {
541 unsafe { return *(BodyGetRotationUnsafe(body)); }
542 }
543
544 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
545 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
546 public static Vector3 BodyGetTorque(IntPtr body)
547 {
548 unsafe { return *(BodyGetTorqueUnsafe(body)); }
549 }
550
551 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
552 public static extern IntPtr BodyGetWorld(IntPtr body);
553
554 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
555 public static extern IntPtr BodyGetFirstGeom(IntPtr body);
556
557 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
558 public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
559
560
561 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
562 public static extern bool BodyIsEnabled(IntPtr body);
563
564 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
565 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
566
567 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
568 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
569
570 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
571 public static extern void BodySetAutoDisableDefaults(IntPtr body);
572
573 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
574 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
575
576 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
577 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
578
579 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
580 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
581
582 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
583 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
584
585 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
586 public static extern void BodySetData(IntPtr body, IntPtr data);
587
588 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
589 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
590
591 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
592 public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
593
594 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
595 public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
596
597 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
598 public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
599
600 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
601 public static extern dReal BodyGetLinearDamping(IntPtr body);
602
603 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
604 public static extern dReal BodyGetAngularDamping(IntPtr body);
605
606 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
607 public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
608
609 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
610 public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
611
612 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
613 public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
614
615 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
616 public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
617
618 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
619 public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
620
621 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
622 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
623
624 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
625 public static extern void BodySetGravityMode(IntPtr body, bool mode);
626
627 /// <summary>
628 /// Sets the Gyroscopic term status on the body specified.
629 /// </summary>
630 /// <param name="body">Pointer to body</param>
631 /// <param name="enabled">NonZero enabled, Zero disabled</param>
632 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
633 public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
634
635 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
636 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
637
638 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
639 public static extern void BodySetMass(IntPtr body, ref Mass mass);
640
641 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
642 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
643
644 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
645 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
646
647 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
648 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
649
650 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
651 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
652
653 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
654 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
655
656 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
657 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
658
659 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
660 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
661
662 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
663 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
664
665 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
666 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
667 ref Vector3 side1, ref Vector3 p2,
668 ref Matrix3 R2, ref Vector3 side2,
669 ref Vector3 normal, out dReal depth, out int return_code,
670 int maxc, out ContactGeom contact, int skip);
671
672 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
673 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
674 ref Vector3 side1, ref Vector3 _p2,
675 ref Matrix3 R2, ref Vector3 side2);
676
677 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
678 public static extern void CleanupODEAllDataForThread();
679
680 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
681 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
682 ref Vector3 b1, ref Vector3 b2,
683 ref Vector3 cp1, ref Vector3 cp2);
684
685 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
686 public static extern void CloseODE();
687
688 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
689 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
690 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
691 public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
692
693 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
694 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
695
696 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
697 public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
698 public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
699 {
700 NTotalGeoms++;
701 return CreateiBox(space, lx, ly, lz);
702 }
703
704 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
705 public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
706 public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
707 {
708 NTotalGeoms++;
709 return CreateiCapsule(space, radius, length);
710 }
711
712 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
713 public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
714 public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
715 {
716 NTotalGeoms++;
717 return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
718 }
719
720 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
721 public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
722 public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
723 {
724 NTotalGeoms++;
725 return CreateiCylinder(space, radius, length);
726 }
727
728 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
729 public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
730 public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
731 {
732 NTotalGeoms++;
733 return CreateiHeightfield(space, data, bPlaceable);
734 }
735
736 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity]
737 public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable);
738 public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable)
739 {
740 NTotalGeoms++;
741 return CreateiUbitTerrain(space, data, bPlaceable);
742 }
743
744
745
746
747
748 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
749 public static extern IntPtr CreateiGeom(int classnum);
750 public static IntPtr CreateGeom(int classnum)
751 {
752 NTotalGeoms++;
753 return CreateiGeom(classnum);
754 }
755
756 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
757 public static extern int CreateGeomClass(ref GeomClass classptr);
758
759 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
760 public static extern IntPtr CreateGeomTransform(IntPtr space);
761
762 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
763 public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
764 public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
765 {
766 NTotalGeoms++;
767 return CreateiPlane(space, a, b, c, d);
768 }
769
770 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
771 public static extern IntPtr CreateiRay(IntPtr space, dReal length);
772 public static IntPtr CreateRay(IntPtr space, dReal length)
773 {
774 NTotalGeoms++;
775 return CreateiRay(space, length);
776 }
777
778 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
779 public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
780 public static IntPtr CreateSphere(IntPtr space, dReal radius)
781 {
782 NTotalGeoms++;
783 return CreateiSphere(space, radius);
784 }
785
786 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
787 public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
788 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
789 public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
790 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
791 {
792 NTotalGeoms++;
793 return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
794 }
795 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
796 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
797
798 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
799 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
800
801 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
802 public static extern int FactorCholesky(ref dReal A00, int n);
803
804 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
805 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
806
807 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
808 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
809
810 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
811 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
812
813 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
814 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
815
816 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
817 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
818
819 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
820 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
821
822 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
823 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
824
825 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
826 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
827
828 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
829 public static extern void GeomClearOffset(IntPtr geom);
830
831 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
832 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
833
834 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
835 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
836
837 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
838 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
839
840 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
841 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
842
843 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
844 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
845
846 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
847 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
848
849 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
850 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
851
852 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
853 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
854
855 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
856 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
857
858 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
859 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
860
861 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
862 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
863
864 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
865 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
866
867 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
868 public static extern void GeomiDestroy(IntPtr geom);
869 public static void GeomDestroy(IntPtr geom)
870 {
871 NTotalGeoms--;
872 GeomiDestroy(geom);
873 }
874
875
876 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
877 public static extern void GeomDisable(IntPtr geom);
878
879 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
880 public static extern void GeomEnable(IntPtr geom);
881
882 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
883 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
884
885 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
886 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
887
888 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
889 public static extern IntPtr GeomGetBody(IntPtr geom);
890
891 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
892 public static extern uint GeomGetCategoryBits(IntPtr geom);
893
894 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
895 public static extern IntPtr GeomGetClassData(IntPtr geom);
896
897 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
898 public static extern uint GeomGetCollideBits(IntPtr geom);
899
900 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
901 public static extern GeomClassID GeomGetClass(IntPtr geom);
902
903 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
904 public static extern IntPtr GeomGetData(IntPtr geom);
905
906 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
907 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
908 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
909 {
910 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
911 }
912
913 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
914 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
915 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
916 {
917 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
918 }
919
920 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
921 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
922 public static Vector3 GeomGetPosition(IntPtr geom)
923 {
924 unsafe { return *(GeomGetPositionUnsafe(geom)); }
925 }
926
927 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
928 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
929
930 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
931 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
932
933 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
934 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
935 public static Matrix3 GeomGetRotation(IntPtr geom)
936 {
937 unsafe { return *(GeomGetRotationUnsafe(geom)); }
938 }
939
940 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
941 public static extern IntPtr GeomGetSpace(IntPtr geom);
942
943 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
944 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
945 dReal width, dReal depth, int widthSamples, int depthSamples,
946 dReal scale, dReal offset, dReal thickness, int bWrap);
947
948 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
949 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
950 dReal width, dReal depth, int widthSamples, int depthSamples,
951 dReal scale, dReal offset, dReal thickness, int bWrap);
952
953 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
954 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
955 dReal width, dReal depth, int widthSamples, int depthSamples,
956 dReal scale, dReal offset, dReal thickness, int bWrap);
957
958 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
959 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
960 dReal width, dReal depth, int widthSamples, int depthSamples,
961 dReal scale, dReal offset, dReal thickness, int bWrap);
962
963 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
964 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
965 dReal width, dReal depth, int widthSamples, int depthSamples,
966 dReal scale, dReal offset, dReal thickness, int bWrap);
967
968 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
969 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
970 dReal width, dReal depth, int widthSamples, int depthSamples,
971 dReal scale, dReal offset, dReal thickness, int bWrap);
972
973 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
974 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
975 dReal width, dReal depth, int widthSamples, int depthSamples,
976 dReal scale, dReal offset, dReal thickness, int bWrap);
977
978 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
979 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
980 dReal width, dReal depth, int widthSamples, int depthSamples,
981 dReal scale, dReal offset, dReal thickness, int bWrap);
982
983
984
985 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
986 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
987 dReal width, dReal depth, int widthSamples, int depthSamples,
988 dReal scale, dReal offset, dReal thickness, int bWrap);
989
990 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
991 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
992 dReal width, dReal depth, int widthSamples, int depthSamples,
993 dReal scale, dReal offset, dReal thickness, int bWrap);
994
995 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
996 public static extern IntPtr GeomHeightfieldDataCreate();
997
998 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
999 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
1000
1001 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
1002 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1003
1004 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1005 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
1006
1007 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1008 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
1009
1010
1011 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1012 public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData,
1013 dReal sampleSize, int widthSamples, int depthSamples,
1014 dReal offset, dReal thickness, int bWrap);
1015
1016 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity]
1017 public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
1018 dReal sampleSize, int widthSamples, int depthSamples,
1019 dReal thickness, int bWrap);
1020
1021 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity]
1022 public static extern IntPtr GeomUbitTerrainDataCreate();
1023
1024 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity]
1025 public static extern void GeomUbitTerrainDataDestroy(IntPtr d);
1026
1027 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity]
1028 public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
1029
1030 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1031 public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g);
1032
1033 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
1034 public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d);
1035
1036
1037 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
1038 public static extern bool GeomIsEnabled(IntPtr geom);
1039
1040 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
1041 public static extern bool GeomIsOffset(IntPtr geom);
1042
1043 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
1044 public static extern bool GeomIsSpace(IntPtr geom);
1045
1046 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1047 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
1048
1049 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1050 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
1051
1052 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
1053 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1054
1055 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
1056 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
1057
1058 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1059 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
1060
1061 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1062 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
1063
1064 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
1065 public static extern int GeomRayGetClosestHit(IntPtr ray);
1066
1067 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
1068 public static extern dReal GeomRayGetLength(IntPtr ray);
1069
1070 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
1071 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
1072
1073 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
1074 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
1075
1076 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
1077 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
1078
1079 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
1080 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
1081
1082 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
1083 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
1084
1085 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
1086 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
1087
1088 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
1089 public static extern void GeomSetCategoryBits(IntPtr geom, uint bits);
1090
1091 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1092 public static extern void GeomSetCollideBits(IntPtr geom, uint bits);
1093
1094 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
1095 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
1096
1097 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
1098 public static extern void GeomSetData(IntPtr geom, IntPtr data);
1099
1100 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
1101 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1102
1103 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1104 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
1105
1106 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1107 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
1108
1109 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1110 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
1111
1112 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1113 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
1114
1115 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
1116 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
1117
1118 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1119 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
1120
1121 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1122 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
1123
1124 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1125 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
1126
1127 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1128 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
1129
1130 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
1131 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1132
1133 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1134 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
1135
1136 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1137 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
1138
1139 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1140 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
1141
1142 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1143 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
1144
1145 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
1146 public static extern dReal GeomSphereGetRadius(IntPtr geom);
1147
1148 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
1149 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1150
1151 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
1152 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
1153
1154 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
1155 public static extern int GeomTransformGetCleanup(IntPtr geom);
1156
1157 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
1158 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
1159
1160 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
1161 public static extern int GeomTransformGetInfo(IntPtr geom);
1162
1163 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
1164 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
1165
1166 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
1167 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
1168
1169 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
1170 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
1171
1172 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1173 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1174 double[] vertices, int vertexStride, int vertexCount,
1175 int[] indices, int indexCount, int triStride);
1176
1177 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1178 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1179 IntPtr vertices, int vertexStride, int vertexCount,
1180 IntPtr indices, int indexCount, int triStride);
1181
1182 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1183 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
1184 double[] vertices, int vertexStride, int vertexCount,
1185 int[] indices, int indexCount, int triStride,
1186 double[] normals);
1187
1188 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1189 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1190 IntPtr vertices, int vertexStride, int vertexCount,
1191 IntPtr indices, int indexCount, int triStride,
1192 IntPtr normals);
1193
1194 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1195 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1196 dReal[] vertices, int vertexStride, int vertexCount,
1197 int[] indices, int indexCount, int triStride);
1198
1199 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1200 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1201 IntPtr vertices, int vertexStride, int vertexCount,
1202 IntPtr indices, int indexCount, int triStride);
1203
1204 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1205 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1206 dReal[] vertices, int vertexStride, int vertexCount,
1207 int[] indices, int indexCount, int triStride,
1208 dReal[] normals);
1209
1210 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1211 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1212 IntPtr vertices, int vertexStride, int vertexCount,
1213 IntPtr indices, int indexCount, int triStride,
1214 IntPtr normals);
1215
1216 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1217 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1218 float[] vertices, int vertexStride, int vertexCount,
1219 int[] indices, int indexCount, int triStride);
1220
1221 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1222 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1223 IntPtr vertices, int vertexStride, int vertexCount,
1224 IntPtr indices, int indexCount, int triStride);
1225
1226 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1227 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1228 float[] vertices, int vertexStride, int vertexCount,
1229 int[] indices, int indexCount, int triStride,
1230 float[] normals);
1231
1232 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1233 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1234 IntPtr vertices, int vertexStride, int vertexCount,
1235 IntPtr indices, int indexCount, int triStride,
1236 IntPtr normals);
1237
1238 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1239 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1240
1241 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1242 public static extern IntPtr GeomTriMeshDataCreate();
1243
1244 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1245 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1246
1247 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1248 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1249
1250 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1251 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1252
1253 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1254 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1255
1256 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1257 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1258
1259 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1260 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1261
1262 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1263 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1264
1265 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1266 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1267
1268 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1269 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1270
1271 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1272 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1273 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1274 {
1275 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1276 }
1277
1278 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1279 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1280
1281 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1282 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1283
1284 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1285 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1286
1287 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1288 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1289
1290 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1291 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1292
1293 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1294 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1295
1296 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1297 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1298
1299 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1300 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1301
1302 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1303 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1304
1305 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1306 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1307
1308 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1309 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1310
1311 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1312 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1313
1314 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
1315 public static extern string GetConfiguration(string str);
1316
1317 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1318 public static extern IntPtr HashSpaceCreate(IntPtr space);
1319
1320 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1321 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1322
1323 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1324 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1325
1326 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1327 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1328
1329 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1330 public static extern void InitODE();
1331
1332 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
1333 public static extern int InitODE2(uint ODEInitFlags);
1334
1335 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1336 public static extern int IsPositiveDefinite(ref dReal A, int n);
1337
1338 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1339 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1340
1341 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1342 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1343
1344 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1345 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1346
1347 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1348 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1349
1350 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1351 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1352
1353 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1354 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1355
1356 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1357 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1358
1359 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1360 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1361
1362 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1363 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1364
1365 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1366 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1367
1368 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1369 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1370 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1371 public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
1372
1373 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1374 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1375
1376 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1377 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1378
1379 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1380 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1381
1382 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1383 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1384
1385 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1386 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1387
1388 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1389 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1390
1391 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1392 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1393
1394 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1395 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1396
1397 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1398 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1399
1400 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1401 public static extern void JointDestroy(IntPtr j);
1402
1403 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1404 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1405
1406 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1407 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1408
1409 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1410 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1411
1412 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1413 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1414
1415 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1416 public static extern int JointGetAMotorMode(IntPtr j);
1417
1418 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1419 public static extern int JointGetAMotorNumAxes(IntPtr j);
1420
1421 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1422 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1423
1424 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1425 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1426
1427 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1428 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1429
1430 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1431 public static extern IntPtr JointGetBody(IntPtr j);
1432
1433 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1434 public static extern IntPtr JointGetData(IntPtr j);
1435
1436 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1437 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1438 public static JointFeedback JointGetFeedback(IntPtr j)
1439 {
1440 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1441 }
1442
1443 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1444 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1445
1446 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1447 public static extern dReal JointGetHingeAngle(IntPtr j);
1448
1449 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1450 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1451
1452 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1453 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1454
1455 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1456 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1457
1458 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1459 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1460
1461 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1462 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1463
1464 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1465 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1466
1467 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1468 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1469
1470 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1471 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1472
1473 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1474 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1475
1476 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1477 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1478
1479 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1480 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1481
1482 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1483 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1484
1485 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1486 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1487
1488 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1489 public static extern int JointGetLMotorNumAxes(IntPtr j);
1490
1491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1492 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1493
1494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1495 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1496
1497 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1498 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1499
1500 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1501 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1502
1503 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1504 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1505
1506 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1507 public static extern dReal JointGetPRPosition(IntPtr j);
1508
1509 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1510 public static extern dReal JointGetPRPositionRate(IntPtr j);
1511
1512 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1513 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1514
1515 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1516 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1517
1518 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1519 public static extern dReal JointGetSliderPosition(IntPtr j);
1520
1521 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1522 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1523
1524 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1525 public static extern JointType JointGetType(IntPtr j);
1526
1527 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1528 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1529
1530 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1531 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1532
1533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1534 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1535
1536 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1537 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1538
1539 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1540 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1541
1542 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1543 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1544
1545 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1546 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1547
1548 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1549 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1550
1551 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1552 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1553
1554 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1555 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1556
1557 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1558 public static extern IntPtr JointGroupCreate(int max_size);
1559
1560 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1561 public static extern void JointGroupDestroy(IntPtr group);
1562
1563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1564 public static extern void JointGroupEmpty(IntPtr group);
1565
1566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1567 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1568
1569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1570 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1571
1572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1573 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1574
1575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1576 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1577
1578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1579 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1580
1581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1582 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1583
1584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1585 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1586
1587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1588 public static extern void JointSetData(IntPtr j, IntPtr data);
1589
1590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1591 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1592
1593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1594 public static extern void JointSetFixed(IntPtr j);
1595
1596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1597 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1598
1599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1600 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1601
1602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1603 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1604
1605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1606 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1607
1608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1609 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1610
1611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1612 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1613
1614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1615 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1616
1617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1618 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1619
1620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1621 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1622
1623 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1624 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1625
1626 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1627 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1628
1629 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1630 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1631
1632 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1633 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1634
1635 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1636 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1637
1638 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1639 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1640
1641 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1642 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1643
1644 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1645 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1646
1647 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1648 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1649
1650 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1651 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1652
1653 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1654 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1655
1656 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1657 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1658
1659 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1660 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1661
1662 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1663 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1664
1665 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1666 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1667
1668 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1669 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1670
1671 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1672 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1673
1674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1675 public static extern void MassAdd(ref Mass a, ref Mass b);
1676
1677 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1678 public static extern void MassAdjust(ref Mass m, dReal newmass);
1679
1680 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1681 public static extern bool MassCheck(ref Mass m);
1682
1683 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1684 public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
1685
1686 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1687 public static extern void MassRotate(ref Mass mass, ref dReal M00);
1688
1689 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1690 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1691
1692 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1693 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1694
1695 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1696 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1697
1698 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1699 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1700
1701 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1702 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1703
1704 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1705 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1706
1707 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1708 public static extern void MassSetParameters(out Mass mass, dReal themass,
1709 dReal cgx, dReal cgy, dReal cgz,
1710 dReal i11, dReal i22, dReal i33,
1711 dReal i12, dReal i13, dReal i23);
1712
1713 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1714 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1715
1716 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1717 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1718
1719 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1720 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1721
1722 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1723 public static extern void MassSetZero(out Mass mass);
1724
1725 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1726 public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
1727
1728 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1729 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1730
1731 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1732 private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
1733 public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
1734 {
1735 MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
1736 }
1737
1738 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1739 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1740
1741 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1742 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1743
1744 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1745 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1746
1747 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1748 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1749
1750 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1751 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1752
1753 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1754 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1755
1756 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1757 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1758
1759 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1760 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1761
1762 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1763 public static extern void QSetIdentity(out Quaternion q);
1764
1765 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1766 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1767
1768 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1769 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1770
1771 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1772 public static extern dReal RandReal();
1773
1774 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1775 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1776
1777 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1778 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1779
1780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1781 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1782
1783 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1784 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1785
1786 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1787 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1788
1789 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1790 public static extern void RSetIdentity(out Matrix3 R);
1791
1792 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1793 public static extern void SetValue(out dReal a, int n);
1794
1795 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1796 public static extern void SetZero(out dReal a, int n);
1797
1798 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1799 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1800
1801 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1802 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1803
1804 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1805 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1806
1807 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1808 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1809
1810 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1811 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1812
1813 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1814 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1815
1816 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
1817 public static extern bool SpaceLockQuery(IntPtr space);
1818
1819 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1820 public static extern void SpaceClean(IntPtr space);
1821
1822 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1823 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1824
1825 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1826 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1827
1828 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1829 public static extern void SpaceDestroy(IntPtr space);
1830
1831 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1832 public static extern bool SpaceGetCleanup(IntPtr space);
1833
1834 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1835 public static extern int SpaceGetNumGeoms(IntPtr space);
1836
1837 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1838 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1839
1840 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
1841 public static extern int SpaceGetSublevel(IntPtr space);
1842
1843 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1844 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1845
1846 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1847 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1848
1849 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1850 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1851
1852 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
1853 public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
1854
1855 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
1856 public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
1857
1858 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1859 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1860
1861 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1862 public static extern IntPtr WorldCreate();
1863
1864 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1865 public static extern void WorldDestroy(IntPtr world);
1866
1867 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1868 public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
1869
1870 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1871 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1872
1873 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1874 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1875
1876 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1877 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1878
1879 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1880 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1881
1882 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1883 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1884
1885 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1886 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1887
1888 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1889 public static extern dReal WorldGetCFM(IntPtr world);
1890
1891 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1892 public static extern dReal WorldGetERP(IntPtr world);
1893
1894 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1895 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1896
1897 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1898 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1899
1900 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1901 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1902
1903 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1904 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1905
1906 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
1907 public static extern dReal WorldGetAngularDamping(IntPtr world);
1908
1909 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1910 public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
1911
1912 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
1913 public static extern dReal WorldGetLinearDamping(IntPtr world);
1914
1915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1916 public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
1917
1918 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1919 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1920
1921 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1922 public static extern dReal WorldGetQuickStepW(IntPtr world);
1923
1924 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1925 public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
1926
1927 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1928 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1929
1930 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1931 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1932
1933 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1934 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1935
1936 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
1937 public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
1938
1939 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1940 public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
1941
1942 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1943 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1944
1945 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1946 public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
1947
1948 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1949 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1950
1951 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1952 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1953
1954 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1955 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1956
1957 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1958 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1959
1960 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1961 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1962
1963 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1964 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1965
1966 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1967 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1968
1969 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1970 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1971
1972 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
1973 public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
1974
1975 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1976 public static extern void WorldSetERP(IntPtr world, dReal erp);
1977
1978 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1979 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
1980
1981 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
1982 public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
1983
1984 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1985 public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
1986
1987 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1988 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
1989
1990 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
1991 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
1992
1993 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1994 public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
1995
1996 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
1997 public static extern void WorldStep(IntPtr world, dReal stepsize);
1998
1999 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
2000 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
2001
2002 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
2003 public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
2004 }
2005}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..d32188e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
@@ -0,0 +1,90 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Threading;
33using System.IO;
34using System.Diagnostics;
35using log4net;
36using Nini.Config;
37using OdeAPI;
38using OpenSim.Framework;
39using OpenSim.Region.Physics.Manager;
40using OpenMetaverse;
41
42namespace OpenSim.Region.Physics.OdePlugin
43{
44 /// <summary>
45 /// ODE plugin
46 /// </summary>
47 public class OdePlugin : IPhysicsPlugin
48 {
49 //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
50
51 private OdeScene m_scene;
52
53 public bool Init()
54 {
55 if (Util.IsWindows())
56 Util.LoadArchSpecificWindowsDll("ode.dll");
57
58 if (d.InitODE2(0) != 0)
59 {
60 if (d.AllocateODEDataForThread(~0U) == 0)
61 {
62 d.CloseODE();
63 return false;
64 }
65 return true;
66 }
67 return false;
68 }
69
70 public PhysicsScene GetScene(String sceneIdentifier)
71 {
72 if (m_scene == null)
73 {
74
75 m_scene = new OdeScene(sceneIdentifier);
76 }
77 return (m_scene);
78 }
79
80 public string GetName()
81 {
82 return ("UbitODE");
83 }
84
85 public void Dispose()
86 {
87 d.CloseODE();
88 }
89 }
90} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
new file mode 100644
index 0000000..286c7f0
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2835 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28//#define SPAM
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using OdeAPI;
40using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43
44namespace OpenSim.Region.Physics.OdePlugin
45{
46 public enum StatusIndicators : int
47 {
48 Generic = 0,
49 Start = 1,
50 End = 2
51 }
52
53 public struct sCollisionData
54 {
55 public uint ColliderLocalId;
56 public uint CollidedWithLocalId;
57 public int NumberOfCollisions;
58 public int CollisionType;
59 public int StatusIndicator;
60 public int lastframe;
61 }
62
63 // colision flags of things others can colide with
64 // rays, sensors, probes removed since can't be colided with
65 // The top space where things are placed provided further selection
66 // ie physical are in active space nonphysical in static
67 // this should be exclusive as possible
68
69 [Flags]
70 public enum CollisionCategories : uint
71 {
72 Disabled = 0,
73 //by 'things' types
74 Space = 0x01,
75 Geom = 0x02, // aka prim/part
76 Character = 0x04,
77 Land = 0x08,
78 Water = 0x010,
79
80 // by state
81 Phantom = 0x01000,
82 VolumeDtc = 0x02000,
83 Selected = 0x04000,
84 NoShape = 0x08000,
85
86
87 All = 0xffffffff
88 }
89
90 /// <summary>
91 /// Material type for a primitive
92 /// </summary>
93 public enum Material : int
94 {
95 /// <summary></summary>
96 Stone = 0,
97 /// <summary></summary>
98 Metal = 1,
99 /// <summary></summary>
100 Glass = 2,
101 /// <summary></summary>
102 Wood = 3,
103 /// <summary></summary>
104 Flesh = 4,
105 /// <summary></summary>
106 Plastic = 5,
107 /// <summary></summary>
108 Rubber = 6,
109
110 light = 7 // compatibility with old viewers
111 }
112
113 public enum changes : int
114 {
115 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
116 Remove,
117 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
118 // or removes from a object if arg is null
119 DeLink,
120 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
121 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
122 PosOffset, // not in use
123 // arg Vector3 new position in local coords. Changes prim position in object
124 OriOffset, // not in use
125 // arg Vector3 new position in local coords. Changes prim position in object
126 Velocity,
127 AngVelocity,
128 Acceleration,
129 Force,
130 Torque,
131 Momentum,
132
133 AddForce,
134 AddAngForce,
135 AngLock,
136
137 Size,
138 Shape,
139
140 CollidesWater,
141 VolumeDtc,
142
143 Physical,
144 Phantom,
145 Selected,
146 disabled,
147 building,
148
149 VehicleType,
150 VehicleFloatParam,
151 VehicleVectorParam,
152 VehicleRotationParam,
153 VehicleFlags,
154 SetVehicle,
155
156 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
157 }
158
159 public struct ODEchangeitem
160 {
161 public PhysicsActor actor;
162 public OdeCharacter character;
163 public changes what;
164 public Object arg;
165 }
166
167 public class OdeScene : PhysicsScene
168 {
169 private readonly ILog m_log;
170 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
171
172 public bool OdeUbitLib = false;
173// private int threadid = 0;
174 private Random fluidRandomizer = new Random(Environment.TickCount);
175
176 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
177 const float MaxERP = 0.8f;
178 const float minERP = 0.1f;
179 const float comumContactCFM = 0.0001f;
180
181 float frictionMovementMult = 0.8f;
182
183 float TerrainBounce = 0.1f;
184 float TerrainFriction = 0.3f;
185
186 public float AvatarFriction = 0;// 0.9f * 0.5f;
187
188 private const uint m_regionWidth = Constants.RegionSize;
189 private const uint m_regionHeight = Constants.RegionSize;
190
191 public float ODE_STEPSIZE = 0.020f;
192 public float HalfOdeStep = 0.01f;
193 public int odetimestepMS = 20; // rounded
194 private float metersInSpace = 25.6f;
195 private float m_timeDilation = 1.0f;
196
197 DateTime m_lastframe;
198
199 public float gravityx = 0f;
200 public float gravityy = 0f;
201 public float gravityz = -9.8f;
202
203 private float waterlevel = 0f;
204 private int framecount = 0;
205
206 private IntPtr WaterGeom = IntPtr.Zero;
207 private IntPtr WaterHeightmapData = IntPtr.Zero;
208 private GCHandle WaterMapHandler = new GCHandle();
209
210 public float avPIDD = 2200f; // make it visible
211 public float avPIDP = 900f; // make it visible
212 private float avCapRadius = 0.37f;
213 private float avDensity = 3f;
214 private float avMovementDivisorWalk = 1.3f;
215 private float avMovementDivisorRun = 0.8f;
216 private float minimumGroundFlightOffset = 3f;
217 public float maximumMassObject = 10000.01f;
218
219 public bool meshSculptedPrim = true;
220 public bool forceSimplePrimMeshing = false;
221
222 public float meshSculptLOD = 32;
223 public float MeshSculptphysicalLOD = 32;
224
225 public float geomDefaultDensity = 10.000006836f;
226
227 public int geomContactPointsStartthrottle = 3;
228 public int geomUpdatesPerThrottledUpdate = 15;
229
230 public float bodyPIDD = 35f;
231 public float bodyPIDG = 25;
232
233// public int geomCrossingFailuresBeforeOutofbounds = 6;
234
235 public int bodyFramesAutoDisable = 5;
236
237
238 private d.NearCallback nearCallback;
239
240 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
241 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
242 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
243 private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
244
245 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
246
247 /// <summary>
248 /// A list of actors that should receive collision events.
249 /// </summary>
250 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
251 private readonly List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
252
253 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
254 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
255 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
256
257 private float contactsurfacelayer = 0.002f;
258
259 private int contactsPerCollision = 80;
260 internal IntPtr ContactgeomsArray = IntPtr.Zero;
261 private IntPtr GlobalContactsArray = IntPtr.Zero;
262
263 const int maxContactsbeforedeath = 4000;
264 private volatile int m_global_contactcount = 0;
265
266
267 private readonly IntPtr contactgroup;
268
269 public ContactData[] m_materialContactsData = new ContactData[8];
270
271 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
272 private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
273 private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
274
275 private int m_physicsiterations = 10;
276 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
277 private readonly PhysicsActor PANull = new NullPhysicsActor();
278 private float step_time = 0.0f;
279
280 public IntPtr world;
281
282 private uint obj2LocalID = 0;
283 private OdeCharacter cc1;
284 private OdePrim cp1;
285 private OdeCharacter cc2;
286 private OdePrim cp2;
287
288 // split the spaces acording to contents type
289 // ActiveSpace contains characters and active prims
290 // StaticSpace contains land and other that is mostly static in enviroment
291 // this can contain subspaces, like the grid in staticspace
292 // as now space only contains this 2 top spaces
293
294 public IntPtr TopSpace; // the global space
295 public IntPtr ActiveSpace; // space for active prims
296 public IntPtr StaticSpace; // space for the static things around
297
298 // some speedup variables
299 private int spaceGridMaxX;
300 private int spaceGridMaxY;
301 private float spacesPerMeter;
302
303 // split static geometry collision into a grid as before
304 private IntPtr[,] staticPrimspace;
305
306 public Object OdeLock;
307 private static Object SimulationLock;
308
309 public IMesher mesher;
310
311 private IConfigSource m_config;
312
313 public bool physics_logging = false;
314 public int physics_logging_interval = 0;
315 public bool physics_logging_append_existing_logfile = false;
316
317 private Vector3 m_worldOffset = Vector3.Zero;
318 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
319 private PhysicsScene m_parentScene = null;
320
321 private ODERayCastRequestManager m_rayCastManager;
322
323
324/* maybe needed if ode uses tls
325 private void checkThread()
326 {
327
328 int th = Thread.CurrentThread.ManagedThreadId;
329 if(th != threadid)
330 {
331 threadid = th;
332 d.AllocateODEDataForThread(~0U);
333 }
334 }
335 */
336 /// <summary>
337 /// Initiailizes the scene
338 /// Sets many properties that ODE requires to be stable
339 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
340 /// </summary>
341 public OdeScene(string sceneIdentifier)
342 {
343 m_log
344 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
345
346// checkThread();
347 Name = sceneIdentifier;
348
349 OdeLock = new Object();
350 SimulationLock = new Object();
351
352 nearCallback = near;
353
354 m_rayCastManager = new ODERayCastRequestManager(this);
355 lock (OdeLock)
356 {
357 // Create the world and the first space
358 try
359 {
360 world = d.WorldCreate();
361 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
362
363 // now the major subspaces
364 ActiveSpace = d.HashSpaceCreate(TopSpace);
365 StaticSpace = d.HashSpaceCreate(TopSpace);
366 }
367 catch
368 {
369 // i must RtC#FM
370 }
371
372 d.HashSpaceSetLevels(TopSpace, -2, 8);
373 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
374 d.HashSpaceSetLevels(StaticSpace, -2, 8);
375
376 // demote to second level
377 d.SpaceSetSublevel(ActiveSpace, 1);
378 d.SpaceSetSublevel(StaticSpace, 1);
379
380 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
381 CollisionCategories.Geom |
382 CollisionCategories.Character |
383 CollisionCategories.Phantom |
384 CollisionCategories.VolumeDtc
385 ));
386 d.GeomSetCollideBits(ActiveSpace, 0);
387 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
388 CollisionCategories.Geom |
389 CollisionCategories.Land |
390 CollisionCategories.Water |
391 CollisionCategories.Phantom |
392 CollisionCategories.VolumeDtc
393 ));
394 d.GeomSetCollideBits(StaticSpace, 0);
395
396 contactgroup = d.JointGroupCreate(0);
397 //contactgroup
398
399 d.WorldSetAutoDisableFlag(world, false);
400 }
401 }
402
403 // Initialize the mesh plugin
404// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
405 public override void Initialise(IMesher meshmerizer, IConfigSource config)
406 {
407// checkThread();
408 mesher = meshmerizer;
409 m_config = config;
410/*
411 string ode_config = d.GetConfiguration("ODE");
412 if (ode_config != null && ode_config != "")
413 {
414 m_log.WarnFormat("ODE configuration: {0}", ode_config);
415
416 if (ode_config.Contains("ODE_Ubit"))
417 {
418 OdeUbitLib = true;
419 }
420 }
421*/
422 /*
423 if (region != null)
424 {
425 WorldExtents.X = region.RegionSizeX;
426 WorldExtents.Y = region.RegionSizeY;
427 }
428 */
429
430 // Defaults
431
432 int contactsPerCollision = 80;
433
434 if (m_config != null)
435 {
436 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
437 if (physicsconfig != null)
438 {
439 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
440 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
441 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
442
443 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
444
445 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
446
447 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
448 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
449
450 avDensity = physicsconfig.GetFloat("av_density", avDensity);
451 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
452 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
453 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
454
455 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
456
457 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
458 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
459// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
460
461 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
462 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
463/*
464 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
465 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
466*/
467 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
468 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
469 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
470 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
471/*
472 if (Environment.OSVersion.Platform == PlatformID.Unix)
473 {
474 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
475 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
476 }
477 else
478 {
479
480 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
481 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
482 }
483*/
484 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
485 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
486 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
487
488 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
489 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
490 }
491 }
492
493 HalfOdeStep = ODE_STEPSIZE * 0.5f;
494 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
495
496 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
497 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
498
499 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
500 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
501
502 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
503 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
504
505 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
506 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
507
508 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
509 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
510
511 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
512 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
513
514 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
515 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
516
517 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
518 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
519
520 m_materialContactsData[(int)Material.light].mu = 0.0f;
521 m_materialContactsData[(int)Material.light].bounce = 0.0f;
522
523 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
524
525 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
526 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
527
528 d.WorldSetLinearDamping(world, 0.001f);
529 d.WorldSetAngularDamping(world, 0.001f);
530 d.WorldSetAngularDampingThreshold(world, 0f);
531 d.WorldSetLinearDampingThreshold(world, 0f);
532 d.WorldSetMaxAngularSpeed(world, 100f);
533
534 d.WorldSetCFM(world,1e-6f); // a bit harder than default
535 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
536 d.WorldSetERP(world, 0.6f); // higher than original
537
538 // Set how many steps we go without running collision testing
539 // This is in addition to the step size.
540 // Essentially Steps * m_physicsiterations
541 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
542
543 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
544
545 spacesPerMeter = 1 / metersInSpace;
546 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
547 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
548
549 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
550
551 // create all spaces now
552 int i, j;
553 IntPtr newspace;
554 for (i = 0; i < spaceGridMaxX; i++)
555 for (j = 0; j < spaceGridMaxY; j++)
556 {
557 newspace = d.HashSpaceCreate(StaticSpace);
558 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
559 waitForSpaceUnlock(newspace);
560 d.SpaceSetSublevel(newspace, 2);
561 d.HashSpaceSetLevels(newspace, -2, 8);
562 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
563 CollisionCategories.Geom |
564 CollisionCategories.Land |
565 CollisionCategories.Water |
566 CollisionCategories.Phantom |
567 CollisionCategories.VolumeDtc
568 ));
569 d.GeomSetCollideBits(newspace, 0);
570
571 staticPrimspace[i, j] = newspace;
572 }
573 // let this now be real maximum values
574 spaceGridMaxX--;
575 spaceGridMaxY--;
576 m_lastframe = DateTime.UtcNow;
577 }
578
579 internal void waitForSpaceUnlock(IntPtr space)
580 {
581 //if (space != IntPtr.Zero)
582 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
583 }
584
585 #region Collision Detection
586
587 // sets a global contact for a joint for contactgeom , and base contact description)
588
589 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
590 {
591 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
592 return IntPtr.Zero;
593
594 float erp = contactGeom.depth;
595 erp *= erpscale;
596 if (erp < minERP)
597 erp = minERP;
598 else if (erp > MaxERP)
599 erp = MaxERP;
600
601 float depth = contactGeom.depth * dscale;
602 if (depth > 0.5f)
603 depth = 0.5f;
604
605 d.Contact newcontact = new d.Contact();
606 newcontact.geom.depth = depth;
607 newcontact.geom.g1 = contactGeom.g1;
608 newcontact.geom.g2 = contactGeom.g2;
609 newcontact.geom.pos = contactGeom.pos;
610 newcontact.geom.normal = contactGeom.normal;
611 newcontact.geom.side1 = contactGeom.side1;
612 newcontact.geom.side2 = contactGeom.side2;
613
614 // this needs bounce also
615 newcontact.surface.mode = comumContactFlags;
616 newcontact.surface.mu = mu;
617 newcontact.surface.bounce = bounce;
618 newcontact.surface.soft_cfm = cfm;
619 newcontact.surface.soft_erp = erp;
620
621 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
622 Marshal.StructureToPtr(newcontact, contact, true);
623 return d.JointCreateContactPtr(world, contactgroup, contact);
624 }
625
626 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
627 {
628 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
629 return false;
630
631 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
632 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
633 return true;
634 }
635
636 /// <summary>
637 /// This is our near callback. A geometry is near a body
638 /// </summary>
639 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
640 /// <param name="g1">a geometry or space</param>
641 /// <param name="g2">another geometry or space</param>
642 ///
643
644 private void near(IntPtr space, IntPtr g1, IntPtr g2)
645 {
646 // no lock here! It's invoked from within Simulate(), which is thread-locked
647
648 if (m_global_contactcount >= maxContactsbeforedeath)
649 return;
650
651 // Test if we're colliding a geom with a space.
652 // If so we have to drill down into the space recursively
653
654 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
655 return;
656
657 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
658 {
659 // We'll be calling near recursivly if one
660 // of them is a space to find all of the
661 // contact points in the space
662 try
663 {
664 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
665 }
666 catch (AccessViolationException)
667 {
668 m_log.Warn("[PHYSICS]: Unable to collide test a space");
669 return;
670 }
671 //here one should check collisions of geoms inside a space
672 // but on each space we only should have geoms that not colide amoung each other
673 // so we don't dig inside spaces
674 return;
675 }
676
677 // get geom bodies to check if we already a joint contact
678 // guess this shouldn't happen now
679 IntPtr b1 = d.GeomGetBody(g1);
680 IntPtr b2 = d.GeomGetBody(g2);
681
682 // d.GeomClassID id = d.GeomGetClass(g1);
683
684 // Figure out how many contact points we have
685 int count = 0;
686 try
687 {
688 // Colliding Geom To Geom
689 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
690
691 if (g1 == g2)
692 return; // Can't collide with yourself
693
694 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
695 return;
696
697 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
698 }
699 catch (SEHException)
700 {
701 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
702// ode.drelease(world);
703 base.TriggerPhysicsBasedRestart();
704 }
705 catch (Exception e)
706 {
707 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
708 return;
709 }
710
711 // id contacts done
712 if (count == 0)
713 return;
714
715 // try get physical actors
716 PhysicsActor p1;
717 PhysicsActor p2;
718
719 if (!actor_name_map.TryGetValue(g1, out p1))
720 {
721 p1 = PANull;
722 }
723
724 if (!actor_name_map.TryGetValue(g2, out p2))
725 {
726 p2 = PANull;
727 }
728
729 // update actors collision score
730 if (p1.CollisionScore >= float.MaxValue - count)
731 p1.CollisionScore = 0;
732 p1.CollisionScore += count;
733
734 if (p2.CollisionScore >= float.MaxValue - count)
735 p2.CollisionScore = 0;
736 p2.CollisionScore += count;
737
738
739 // get first contact
740 d.ContactGeom curContact = new d.ContactGeom();
741 if (!GetCurContactGeom(0, ref curContact))
742 return;
743 // for now it's the one with max depth
744 ContactPoint maxDepthContact = new ContactPoint(
745 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
746 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
747 curContact.depth
748 );
749 // do volume detection case
750 if (
751 (p1.IsVolumeDtc || p2.IsVolumeDtc))
752 {
753 collision_accounting_events(p1, p2, maxDepthContact);
754 return;
755 }
756
757 // big messy collision analises
758
759 Vector3 normoverride = Vector3.Zero; //damm c#
760
761 float mu = 0;
762 float bounce = 0;
763 float cfm = 0.0001f;
764 float erpscale = 1.0f;
765 float dscale = 1.0f;
766 bool IgnoreNegSides = false;
767
768 ContactData contactdata1 = new ContactData(0, 0, false);
769 ContactData contactdata2 = new ContactData(0, 0, false);
770
771 String name = null;
772 bool dop1foot = false;
773 bool dop2foot = false;
774 bool ignore = false;
775 bool AvanormOverride = false;
776
777 switch (p1.PhysicsActorType)
778 {
779 case (int)ActorTypes.Agent:
780 {
781 AvanormOverride = true;
782 Vector3 tmp = p2.Position - p1.Position;
783 normoverride = p2.Velocity - p1.Velocity;
784 mu = normoverride.LengthSquared();
785
786 if (mu > 1e-6)
787 {
788 mu = 1.0f / (float)Math.Sqrt(mu);
789 normoverride *= mu;
790 mu = Vector3.Dot(tmp, normoverride);
791 if (mu > 0)
792 normoverride *= -1;
793 }
794 else
795 {
796 tmp.Normalize();
797 normoverride = -tmp;
798 }
799
800 switch (p2.PhysicsActorType)
801 {
802 case (int)ActorTypes.Agent:
803/*
804 p1.getContactData(ref contactdata1);
805 p2.getContactData(ref contactdata2);
806
807 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
808
809 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
810 mu *= frictionMovementMult;
811*/
812 p1.CollidingObj = true;
813 p2.CollidingObj = true;
814 break;
815 case (int)ActorTypes.Prim:
816/*
817 p1.getContactData(ref contactdata1);
818 p2.getContactData(ref contactdata2);
819
820
821 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
822
823 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
824 mu *= frictionMovementMult;
825 */
826 if (p2.Velocity.LengthSquared() > 0.0f)
827 p2.CollidingObj = true;
828
829 dop1foot = true;
830 break;
831 default:
832 ignore = true; // avatar to terrain and water ignored
833 break;
834 }
835 break;
836 }
837
838 case (int)ActorTypes.Prim:
839 switch (p2.PhysicsActorType)
840 {
841 case (int)ActorTypes.Agent:
842 // p1.getContactData(ref contactdata1);
843 // p2.getContactData(ref contactdata2);
844
845 AvanormOverride = true;
846
847 Vector3 tmp = p2.Position - p1.Position;
848 normoverride = p2.Velocity - p1.Velocity;
849 mu = normoverride.LengthSquared();
850 if (mu > 1e-6)
851 {
852 mu = 1.0f / (float)Math.Sqrt(mu);
853 normoverride *= mu;
854 mu = Vector3.Dot(tmp, normoverride);
855 if (mu > 0)
856 normoverride *= -1;
857 }
858 else
859 {
860 tmp.Normalize();
861 normoverride = -tmp;
862 }
863
864 bounce = 0;
865 mu = 0;
866 cfm = 0.0001f;
867 /*
868 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
869
870 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
871 mu *= frictionMovementMult;
872 */
873 dop2foot = true;
874 if (p1.Velocity.LengthSquared() > 0.0f)
875 p1.CollidingObj = true;
876 break;
877 case (int)ActorTypes.Prim:
878 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
879 {
880 p1.CollidingObj = true;
881 p2.CollidingObj = true;
882 }
883 p1.getContactData(ref contactdata1);
884 p2.getContactData(ref contactdata2);
885 bounce = contactdata1.bounce * contactdata2.bounce;
886 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
887
888 cfm = p1.Mass;
889 if (cfm > p2.Mass)
890 cfm = p2.Mass;
891 dscale = 10 / cfm;
892 dscale = (float)Math.Sqrt(dscale);
893 if (dscale > 1.0f)
894 dscale = 1.0f;
895 erpscale = cfm * 0.01f;
896 cfm = 0.0001f / cfm;
897 if (cfm > 0.01f)
898 cfm = 0.01f;
899
900 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
901 mu *= frictionMovementMult;
902
903 break;
904 default:
905 if (geom_name_map.TryGetValue(g2, out name))
906 {
907 if (name == "Terrain")
908 {
909 p1.getContactData(ref contactdata1);
910 bounce = contactdata1.bounce * TerrainBounce;
911 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
912 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
913 mu *= frictionMovementMult;
914 p1.CollidingGround = true;
915
916 cfm = p1.Mass;
917 dscale = 10 / cfm;
918 dscale = (float)Math.Sqrt(dscale);
919 if (dscale > 1.0f)
920 dscale = 1.0f;
921 erpscale = cfm * 0.01f;
922 cfm = 0.0001f / cfm;
923 if (cfm > 0.01f)
924 cfm = 0.01f;
925
926 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
927 {
928 if (curContact.side1 > 0)
929 IgnoreNegSides = true;
930 }
931
932 }
933 else if (name == "Water")
934 {
935 ignore = true;
936 }
937 }
938 else
939 ignore = true;
940 break;
941 }
942 break;
943
944 default:
945 if (geom_name_map.TryGetValue(g1, out name))
946 {
947 if (name == "Terrain")
948 {
949 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
950 {
951 p2.CollidingGround = true;
952 p2.getContactData(ref contactdata2);
953 bounce = contactdata2.bounce * TerrainBounce;
954 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
955
956 cfm = p2.Mass;
957 dscale = 10 / cfm;
958 dscale = (float)Math.Sqrt(dscale);
959
960 if (dscale > 1.0f)
961 dscale = 1.0f;
962
963 erpscale = cfm * 0.01f;
964 cfm = 0.0001f / cfm;
965 if (cfm > 0.01f)
966 cfm = 0.01f;
967
968 if (curContact.side1 > 0) // should be 2 ?
969 IgnoreNegSides = true;
970
971 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
972 mu *= frictionMovementMult;
973 }
974 else
975 ignore = true;
976
977 }
978 else if (name == "Water" &&
979 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
980 {
981 ignore = true;
982 }
983 }
984 else
985 ignore = true;
986 break;
987 }
988
989 if (ignore)
990 return;
991
992 IntPtr Joint;
993
994 int i = 0;
995 while(true)
996 {
997
998 if (IgnoreNegSides && curContact.side1 < 0)
999 {
1000 if (++i >= count)
1001 break;
1002
1003 if (!GetCurContactGeom(i, ref curContact))
1004 break;
1005 }
1006 else
1007
1008 {
1009
1010 if (AvanormOverride)
1011 {
1012 if (curContact.depth > 0.3f)
1013 {
1014 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
1015 p1.IsColliding = true;
1016 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
1017 p2.IsColliding = true;
1018 curContact.normal.X = normoverride.X;
1019 curContact.normal.Y = normoverride.Y;
1020 curContact.normal.Z = normoverride.Z;
1021 }
1022
1023 else
1024 {
1025 if (dop1foot)
1026 {
1027 float sz = p1.Size.Z;
1028 Vector3 vtmp = p1.Position;
1029 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1030 if (ppos > 0f)
1031 {
1032 if (!p1.Flying)
1033 {
1034 d.AABB aabb;
1035 d.GeomGetAABB(g2, out aabb);
1036 float tmp = vtmp.Z - sz * .25f;
1037
1038 if (aabb.MaxZ < tmp)
1039 {
1040 vtmp.X = curContact.pos.X - vtmp.X;
1041 vtmp.Y = curContact.pos.Y - vtmp.Y;
1042 vtmp.Z = -0.2f;
1043 vtmp.Normalize();
1044 curContact.normal.X = vtmp.X;
1045 curContact.normal.Y = vtmp.Y;
1046 curContact.normal.Z = vtmp.Z;
1047 }
1048 }
1049 }
1050 else
1051 p1.IsColliding = true;
1052
1053 }
1054
1055 if (dop2foot)
1056 {
1057 float sz = p2.Size.Z;
1058 Vector3 vtmp = p2.Position;
1059 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1060 if (ppos > 0f)
1061 {
1062 if (!p2.Flying)
1063 {
1064 d.AABB aabb;
1065 d.GeomGetAABB(g1, out aabb);
1066 float tmp = vtmp.Z - sz * .25f;
1067
1068 if (aabb.MaxZ < tmp)
1069 {
1070 vtmp.X = curContact.pos.X - vtmp.X;
1071 vtmp.Y = curContact.pos.Y - vtmp.Y;
1072 vtmp.Z = -0.2f;
1073 vtmp.Normalize();
1074 curContact.normal.X = vtmp.X;
1075 curContact.normal.Y = vtmp.Y;
1076 curContact.normal.Z = vtmp.Z;
1077 }
1078 }
1079 }
1080 else
1081 p2.IsColliding = true;
1082
1083 }
1084 }
1085 }
1086
1087 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1088 d.JointAttach(Joint, b1, b2);
1089
1090 if (++m_global_contactcount >= maxContactsbeforedeath)
1091 break;
1092
1093 if (++i >= count)
1094 break;
1095
1096 if (!GetCurContactGeom(i, ref curContact))
1097 break;
1098
1099 if (curContact.depth > maxDepthContact.PenetrationDepth)
1100 {
1101 maxDepthContact.Position.X = curContact.pos.X;
1102 maxDepthContact.Position.Y = curContact.pos.Y;
1103 maxDepthContact.Position.Z = curContact.pos.Z;
1104 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1105 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1106 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1107 maxDepthContact.PenetrationDepth = curContact.depth;
1108 }
1109 }
1110 }
1111
1112 collision_accounting_events(p1, p2, maxDepthContact);
1113
1114/*
1115 if (notskipedcount > geomContactPointsStartthrottle)
1116 {
1117 // If there are more then 3 contact points, it's likely
1118 // that we've got a pile of objects, so ...
1119 // We don't want to send out hundreds of terse updates over and over again
1120 // so lets throttle them and send them again after it's somewhat sorted out.
1121 this needs checking so out for now
1122 if (b1 != IntPtr.Zero)
1123 p1.ThrottleUpdates = true;
1124 if (b2 != IntPtr.Zero)
1125 p2.ThrottleUpdates = true;
1126
1127 }
1128 */
1129 }
1130
1131 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1132 {
1133 obj2LocalID = 0;
1134 bool p1events = p1.SubscribedEvents();
1135 bool p2events = p2.SubscribedEvents();
1136
1137 if (p1.IsVolumeDtc)
1138 p2events = false;
1139 if (p2.IsVolumeDtc)
1140 p1events = false;
1141
1142 if (!(p2events || p1events))
1143 return;
1144
1145 if (p1events)
1146 AddCollisionEventReporting(p1);
1147
1148 if (p2events)
1149 AddCollisionEventReporting(p2);
1150
1151 Vector3 vel = Vector3.Zero;
1152 if (p2 != null && p2.IsPhysical)
1153 vel = p2.Velocity;
1154
1155 if (p1 != null && p1.IsPhysical)
1156 vel -= p1.Velocity;
1157
1158 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1159
1160 switch ((ActorTypes)p1.PhysicsActorType)
1161 {
1162 case ActorTypes.Agent:
1163 cc1 = (OdeCharacter)p1;
1164 switch ((ActorTypes)p2.PhysicsActorType)
1165 {
1166 case ActorTypes.Agent:
1167 cc2 = (OdeCharacter)p2;
1168 obj2LocalID = cc2.m_localID;
1169 if (p2events)
1170 cc2.AddCollisionEvent(cc1.m_localID, contact);
1171 break;
1172
1173 case ActorTypes.Prim:
1174 if (p2 is OdePrim)
1175 {
1176 cp2 = (OdePrim)p2;
1177 obj2LocalID = cp2.m_localID;
1178 if (p2events)
1179 cp2.AddCollisionEvent(cc1.m_localID, contact);
1180 }
1181 break;
1182
1183 case ActorTypes.Ground:
1184 case ActorTypes.Unknown:
1185 default:
1186 obj2LocalID = 0;
1187 break;
1188 }
1189 if (p1events)
1190 {
1191 contact.SurfaceNormal = -contact.SurfaceNormal;
1192 cc1.AddCollisionEvent(obj2LocalID, contact);
1193 }
1194 break;
1195
1196 case ActorTypes.Prim:
1197
1198 if (p1 is OdePrim)
1199 {
1200 cp1 = (OdePrim)p1;
1201
1202 // obj1LocalID = cp2.m_localID;
1203 switch ((ActorTypes)p2.PhysicsActorType)
1204 {
1205 case ActorTypes.Agent:
1206 if (p2 is OdeCharacter)
1207 {
1208 cc2 = (OdeCharacter)p2;
1209 obj2LocalID = cc2.m_localID;
1210 if (p2events)
1211 cc2.AddCollisionEvent(cp1.m_localID, contact);
1212 }
1213 break;
1214 case ActorTypes.Prim:
1215
1216 if (p2 is OdePrim)
1217 {
1218 cp2 = (OdePrim)p2;
1219 obj2LocalID = cp2.m_localID;
1220 if (p2events)
1221 cp2.AddCollisionEvent(cp1.m_localID, contact);
1222 }
1223 break;
1224
1225 case ActorTypes.Ground:
1226 case ActorTypes.Unknown:
1227 default:
1228 obj2LocalID = 0;
1229 break;
1230 }
1231 if (p1events)
1232 {
1233 contact.SurfaceNormal = -contact.SurfaceNormal;
1234 cp1.AddCollisionEvent(obj2LocalID, contact);
1235 }
1236 }
1237 break;
1238 case ActorTypes.Ground:
1239 case ActorTypes.Unknown:
1240 default:
1241 switch ((ActorTypes)p2.PhysicsActorType)
1242 {
1243 case ActorTypes.Agent:
1244 if (p2 is OdeCharacter)
1245 {
1246 cc2 = (OdeCharacter)p2;
1247 obj2LocalID = cc2.m_localID;
1248 if (p2events)
1249 cc2.AddCollisionEvent(0, contact);
1250 }
1251 break;
1252 case ActorTypes.Prim:
1253 if (p2 is OdePrim)
1254 {
1255 cp2 = (OdePrim)p2;
1256 obj2LocalID = cp2.m_localID;
1257 if (p2events)
1258 cp2.AddCollisionEvent(0, contact);
1259 }
1260 break;
1261 }
1262 break;
1263 }
1264 }
1265
1266 /// <summary>
1267 /// This is our collision testing routine in ODE
1268 /// </summary>
1269 /// <param name="timeStep"></param>
1270 private void collision_optimized()
1271 {
1272 lock (_characters)
1273 {
1274 try
1275 {
1276 foreach (OdeCharacter chr in _characters)
1277 {
1278 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1279 continue;
1280
1281 chr.IsColliding = false;
1282 // chr.CollidingGround = false; not done here
1283 chr.CollidingObj = false;
1284 // do colisions with static space
1285 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1286 }
1287 }
1288 catch (AccessViolationException)
1289 {
1290 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1291 }
1292
1293 }
1294
1295 lock (_activeprims)
1296 {
1297 foreach (OdePrim aprim in _activeprims)
1298 {
1299 aprim.CollisionScore = 0;
1300 aprim.IsColliding = false;
1301 }
1302 }
1303
1304 // collide active prims with static enviroment
1305 lock (_activegroups)
1306 {
1307 try
1308 {
1309 foreach (OdePrim prm in _activegroups)
1310 {
1311 if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds)
1312 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1313 }
1314 }
1315 catch (AccessViolationException)
1316 {
1317 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1318 }
1319 }
1320 // finally colide active things amoung them
1321 try
1322 {
1323 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1324 }
1325 catch (AccessViolationException)
1326 {
1327 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1328 }
1329// _perloopContact.Clear();
1330 }
1331
1332 #endregion
1333 /// <summary>
1334 /// Add actor to the list that should receive collision events in the simulate loop.
1335 /// </summary>
1336 /// <param name="obj"></param>
1337 public void AddCollisionEventReporting(PhysicsActor obj)
1338 {
1339 if (!_collisionEventPrim.Contains(obj))
1340 _collisionEventPrim.Add(obj);
1341 }
1342
1343 /// <summary>
1344 /// Remove actor from the list that should receive collision events in the simulate loop.
1345 /// </summary>
1346 /// <param name="obj"></param>
1347 public void RemoveCollisionEventReporting(PhysicsActor obj)
1348 {
1349 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1350 _collisionEventPrimRemove.Add(obj);
1351 }
1352
1353
1354 #region Add/Remove Entities
1355
1356 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1357 {
1358 Vector3 pos;
1359 pos.X = position.X;
1360 pos.Y = position.Y;
1361 pos.Z = position.Z;
1362 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1363 newAv.Flying = isFlying;
1364 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1365
1366 return newAv;
1367 }
1368
1369 public void AddCharacter(OdeCharacter chr)
1370 {
1371 lock (_characters)
1372 {
1373 if (!_characters.Contains(chr))
1374 {
1375 _characters.Add(chr);
1376 if (chr.bad)
1377 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1378 }
1379 }
1380 }
1381
1382 public void RemoveCharacter(OdeCharacter chr)
1383 {
1384 lock (_characters)
1385 {
1386 if (_characters.Contains(chr))
1387 {
1388 _characters.Remove(chr);
1389 }
1390 }
1391 }
1392
1393 public void BadCharacter(OdeCharacter chr)
1394 {
1395 lock (_badCharacter)
1396 {
1397 if (!_badCharacter.Contains(chr))
1398 _badCharacter.Add(chr);
1399 }
1400 }
1401
1402 public override void RemoveAvatar(PhysicsActor actor)
1403 {
1404 //m_log.Debug("[PHYSICS]:ODELOCK");
1405 ((OdeCharacter) actor).Destroy();
1406 }
1407
1408 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1409 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1410 {
1411 Vector3 pos = position;
1412 Vector3 siz = size;
1413 Quaternion rot = rotation;
1414
1415 OdePrim newPrim;
1416 lock (OdeLock)
1417 {
1418 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);
1419
1420 lock (_prims)
1421 _prims.Add(newPrim);
1422 }
1423 return newPrim;
1424 }
1425
1426 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1427 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
1428 {
1429 Vector3 pos = position;
1430 Vector3 siz = size;
1431 Quaternion rot = rotation;
1432
1433 OdePrim newPrim;
1434 lock (OdeLock)
1435 {
1436 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);
1437
1438 lock (_prims)
1439 _prims.Add(newPrim);
1440 }
1441 return newPrim;
1442 }
1443
1444 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1445 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1446 {
1447 Vector3 pos = position;
1448 Vector3 siz = size;
1449 Quaternion rot = rotation;
1450
1451 OdePrim newPrim;
1452 lock (OdeLock)
1453 {
1454 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);
1455
1456 lock (_prims)
1457 _prims.Add(newPrim);
1458 }
1459 return newPrim;
1460 }
1461
1462 public void addActivePrim(OdePrim activatePrim)
1463 {
1464 // adds active prim..
1465 lock (_activeprims)
1466 {
1467 if (!_activeprims.Contains(activatePrim))
1468 _activeprims.Add(activatePrim);
1469 }
1470 }
1471
1472 public void addActiveGroups(OdePrim activatePrim)
1473 {
1474 lock (_activegroups)
1475 {
1476 if (!_activegroups.Contains(activatePrim))
1477 _activegroups.Add(activatePrim);
1478 }
1479 }
1480
1481 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1482 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1483 {
1484 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid);
1485 }
1486
1487
1488 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1489 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1490 {
1491#if SPAM
1492 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1493#endif
1494
1495 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1496 }
1497
1498 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1499 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1500 {
1501#if SPAM
1502 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1503#endif
1504
1505 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1506 }
1507
1508 public override float TimeDilation
1509 {
1510 get { return m_timeDilation; }
1511 }
1512
1513 public override bool SupportsNINJAJoints
1514 {
1515 get { return false; }
1516 }
1517
1518
1519 public void remActivePrim(OdePrim deactivatePrim)
1520 {
1521 lock (_activeprims)
1522 {
1523 _activeprims.Remove(deactivatePrim);
1524 }
1525 }
1526 public void remActiveGroup(OdePrim deactivatePrim)
1527 {
1528 lock (_activegroups)
1529 {
1530 _activegroups.Remove(deactivatePrim);
1531 }
1532 }
1533
1534 public override void RemovePrim(PhysicsActor prim)
1535 {
1536 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1537 // removed in the next physics simulate pass.
1538 if (prim is OdePrim)
1539 {
1540// lock (OdeLock)
1541 {
1542
1543 OdePrim p = (OdePrim)prim;
1544 p.setPrimForRemoval();
1545 }
1546 }
1547 }
1548 /// <summary>
1549 /// This is called from within simulate but outside the locked portion
1550 /// We need to do our own locking here
1551 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1552 /// Simulate() -- justincc).
1553 ///
1554 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1555 ///
1556 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1557 /// that the space was using.
1558 /// </summary>
1559 /// <param name="prim"></param>
1560 public void RemovePrimThreadLocked(OdePrim prim)
1561 {
1562 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1563 lock (prim)
1564 {
1565 RemoveCollisionEventReporting(prim);
1566 lock (_prims)
1567 _prims.Remove(prim);
1568 }
1569
1570 }
1571 #endregion
1572
1573 #region Space Separation Calculation
1574
1575 /// <summary>
1576 /// Called when a static prim moves or becomes static
1577 /// Places the prim in a space one the static sub-spaces grid
1578 /// </summary>
1579 /// <param name="geom">the pointer to the geom that moved</param>
1580 /// <param name="pos">the position that the geom moved to</param>
1581 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1582 /// <returns>a pointer to the new space it's in</returns>
1583 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1584 {
1585 // moves a prim into another static sub-space or from another space into a static sub-space
1586
1587 // Called ODEPrim so
1588 // it's already in locked space.
1589
1590 if (geom == IntPtr.Zero) // shouldn't happen
1591 return IntPtr.Zero;
1592
1593 // get the static sub-space for current position
1594 IntPtr newspace = calculateSpaceForGeom(pos);
1595
1596 if (newspace == currentspace) // if we are there all done
1597 return newspace;
1598
1599 // else remove it from its current space
1600 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1601 {
1602 if (d.GeomIsSpace(currentspace))
1603 {
1604 waitForSpaceUnlock(currentspace);
1605 d.SpaceRemove(currentspace, geom);
1606
1607 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1608 {
1609 d.SpaceDestroy(currentspace);
1610 }
1611 }
1612 else
1613 {
1614 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1615 " Geom:" + geom);
1616 }
1617 }
1618 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1619 {
1620 currentspace = d.GeomGetSpace(geom);
1621 if (currentspace != IntPtr.Zero)
1622 {
1623 if (d.GeomIsSpace(currentspace))
1624 {
1625 waitForSpaceUnlock(currentspace);
1626 d.SpaceRemove(currentspace, geom);
1627
1628 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1629 {
1630 d.SpaceDestroy(currentspace);
1631 }
1632
1633 }
1634 }
1635 }
1636
1637 // put the geom in the newspace
1638 waitForSpaceUnlock(newspace);
1639 d.SpaceAdd(newspace, geom);
1640
1641 // let caller know this newspace
1642 return newspace;
1643 }
1644
1645 /// <summary>
1646 /// Calculates the space the prim should be in by its position
1647 /// </summary>
1648 /// <param name="pos"></param>
1649 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1650 public IntPtr calculateSpaceForGeom(Vector3 pos)
1651 {
1652 int x, y;
1653 x = (int)(pos.X * spacesPerMeter);
1654 if (x < 0)
1655 x = 0;
1656 else if (x > spaceGridMaxX)
1657 x = spaceGridMaxX;
1658
1659 y = (int)(pos.Y * spacesPerMeter);
1660 if (y < 0)
1661 y = 0;
1662 else if (y >spaceGridMaxY)
1663 y = spaceGridMaxY;
1664
1665 IntPtr tmpSpace = staticPrimspace[x, y];
1666 return tmpSpace;
1667 }
1668
1669 #endregion
1670
1671 /// <summary>
1672 /// Routine to figure out if we need to mesh this prim with our mesher
1673 /// </summary>
1674 /// <param name="pbs"></param>
1675 /// <returns></returns>
1676 public bool needsMeshing(PrimitiveBaseShape pbs)
1677 {
1678 // check sculpts or meshs
1679 if (pbs.SculptEntry)
1680 {
1681 if (meshSculptedPrim)
1682 return true;
1683
1684 if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
1685 return true;
1686
1687 return false;
1688 }
1689
1690 if (forceSimplePrimMeshing)
1691 return true;
1692
1693 // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
1694
1695 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1696 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1697 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1698 {
1699
1700 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1701 && pbs.ProfileHollow == 0
1702 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1703 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1704 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1705 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1706 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1707 {
1708#if SPAM
1709 m_log.Warn("NonMesh");
1710#endif
1711 return false;
1712 }
1713 }
1714
1715 // following code doesn't give meshs to boxes and spheres ever
1716 // and it's odd.. so for now just return true if asked to force meshs
1717 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1718
1719 int iPropertiesNotSupportedDefault = 0;
1720
1721 if (pbs.ProfileHollow != 0)
1722 iPropertiesNotSupportedDefault++;
1723
1724 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1725 iPropertiesNotSupportedDefault++;
1726
1727 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1728 iPropertiesNotSupportedDefault++;
1729
1730 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1731 iPropertiesNotSupportedDefault++;
1732
1733 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1734 iPropertiesNotSupportedDefault++;
1735
1736 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1737 iPropertiesNotSupportedDefault++;
1738
1739 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1740 iPropertiesNotSupportedDefault++;
1741
1742 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
1743 iPropertiesNotSupportedDefault++;
1744
1745 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1746 iPropertiesNotSupportedDefault++;
1747
1748 // test for torus
1749 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1750 {
1751 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1752 {
1753 iPropertiesNotSupportedDefault++;
1754 }
1755 }
1756 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1757 {
1758 if (pbs.PathCurve == (byte)Extrusion.Straight)
1759 {
1760 iPropertiesNotSupportedDefault++;
1761 }
1762
1763 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1764 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1765 {
1766 iPropertiesNotSupportedDefault++;
1767 }
1768 }
1769 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1770 {
1771 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1772 {
1773 iPropertiesNotSupportedDefault++;
1774 }
1775 }
1776 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1777 {
1778 if (pbs.PathCurve == (byte)Extrusion.Straight)
1779 {
1780 iPropertiesNotSupportedDefault++;
1781 }
1782 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1783 {
1784 iPropertiesNotSupportedDefault++;
1785 }
1786 }
1787
1788 if (iPropertiesNotSupportedDefault == 0)
1789 {
1790#if SPAM
1791 m_log.Warn("NonMesh");
1792#endif
1793 return false;
1794 }
1795#if SPAM
1796 m_log.Debug("Mesh");
1797#endif
1798 return true;
1799 }
1800
1801 /// <summary>
1802 /// Called to queue a change to a actor
1803 /// to use in place of old taint mechanism so changes do have a time sequence
1804 /// </summary>
1805
1806 public void AddChange(PhysicsActor actor, changes what, Object arg)
1807 {
1808 ODEchangeitem item = new ODEchangeitem();
1809 item.actor = actor;
1810 item.what = what;
1811 item.arg = arg;
1812 ChangesQueue.Enqueue(item);
1813 }
1814
1815 /// <summary>
1816 /// Called after our prim properties are set Scale, position etc.
1817 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1818 /// This assures us that we have no race conditions
1819 /// </summary>
1820 /// <param name="prim"></param>
1821 public override void AddPhysicsActorTaint(PhysicsActor prim)
1822 {
1823 }
1824
1825 /// <summary>
1826 /// This is our main simulate loop
1827 /// It's thread locked by a Mutex in the scene.
1828 /// It holds Collisions, it instructs ODE to step through the physical reactions
1829 /// It moves the objects around in memory
1830 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1831 /// </summary>
1832 /// <param name="timeStep"></param>
1833 /// <returns></returns>
1834 public override float Simulate(float timeStep)
1835 {
1836
1837 DateTime now = DateTime.UtcNow;
1838 TimeSpan SinceLastFrame = now - m_lastframe;
1839 m_lastframe = now;
1840 timeStep = (float)SinceLastFrame.TotalSeconds;
1841
1842 // acumulate time so we can reduce error
1843 step_time += timeStep;
1844
1845 if (step_time < HalfOdeStep)
1846 return 0;
1847
1848 if (framecount < 0)
1849 framecount = 0;
1850
1851 framecount++;
1852
1853 int curphysiteractions;
1854
1855 // if in trouble reduce step resolution
1856 if (step_time >= m_SkipFramesAtms)
1857 curphysiteractions = m_physicsiterations / 2;
1858 else
1859 curphysiteractions = m_physicsiterations;
1860
1861 int nodeframes = 0;
1862
1863// checkThread();
1864
1865 lock (SimulationLock)
1866 lock(OdeLock)
1867 {
1868 // adjust number of iterations per step
1869 try
1870 {
1871 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1872 }
1873 catch (StackOverflowException)
1874 {
1875 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1876// ode.drelease(world);
1877 base.TriggerPhysicsBasedRestart();
1878 }
1879
1880 while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
1881 {
1882 try
1883 {
1884 // clear pointer/counter to contacts to pass into joints
1885 m_global_contactcount = 0;
1886
1887 ODEchangeitem item;
1888
1889 if(ChangesQueue.Count >0)
1890 {
1891 int ttmpstart = Util.EnvironmentTickCount();
1892 int ttmp;
1893
1894 while(ChangesQueue.Dequeue(out item))
1895 {
1896 if (item.actor != null)
1897 {
1898 try
1899 {
1900 if (item.actor is OdeCharacter)
1901 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1902 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1903 RemovePrimThreadLocked((OdePrim)item.actor);
1904 }
1905 catch
1906 {
1907 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1908 };
1909 }
1910 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1911 if (ttmp > 20)
1912 break;
1913 }
1914 }
1915
1916 // Move characters
1917 lock (_characters)
1918 {
1919 List<OdeCharacter> defects = new List<OdeCharacter>();
1920 foreach (OdeCharacter actor in _characters)
1921 {
1922 if (actor != null)
1923 actor.Move(ODE_STEPSIZE, defects);
1924 }
1925 if (defects.Count != 0)
1926 {
1927 foreach (OdeCharacter defect in defects)
1928 {
1929 RemoveCharacter(defect);
1930 }
1931 }
1932 }
1933
1934 // Move other active objects
1935 lock (_activegroups)
1936 {
1937 foreach (OdePrim aprim in _activegroups)
1938 {
1939 aprim.Move();
1940 }
1941 }
1942
1943 //if ((framecount % m_randomizeWater) == 0)
1944 // randomizeWater(waterlevel);
1945
1946 m_rayCastManager.ProcessQueuedRequests();
1947
1948 collision_optimized();
1949
1950 foreach (PhysicsActor obj in _collisionEventPrim)
1951 {
1952 if (obj == null)
1953 continue;
1954
1955 switch ((ActorTypes)obj.PhysicsActorType)
1956 {
1957 case ActorTypes.Agent:
1958 OdeCharacter cobj = (OdeCharacter)obj;
1959 cobj.AddCollisionFrameTime((int)(odetimestepMS));
1960 cobj.SendCollisions();
1961 break;
1962
1963 case ActorTypes.Prim:
1964 OdePrim pobj = (OdePrim)obj;
1965 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1966 {
1967 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1968 pobj.SendCollisions();
1969 }
1970 break;
1971 }
1972 }
1973
1974 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1975 _collisionEventPrim.Remove(obj);
1976
1977 _collisionEventPrimRemove.Clear();
1978
1979 // do a ode simulation step
1980 d.WorldQuickStep(world, ODE_STEPSIZE);
1981 d.JointGroupEmpty(contactgroup);
1982
1983 // update managed ideia of physical data and do updates to core
1984 /*
1985 lock (_characters)
1986 {
1987 foreach (OdeCharacter actor in _characters)
1988 {
1989 if (actor != null)
1990 {
1991 if (actor.bad)
1992 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1993
1994 actor.UpdatePositionAndVelocity();
1995 }
1996 }
1997 }
1998 */
1999
2000 lock (_activegroups)
2001 {
2002 {
2003 foreach (OdePrim actor in _activegroups)
2004 {
2005 if (actor.IsPhysical)
2006 {
2007 actor.UpdatePositionAndVelocity();
2008 }
2009 }
2010 }
2011 }
2012 }
2013 catch (Exception e)
2014 {
2015 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
2016// ode.dunlock(world);
2017 }
2018
2019
2020 step_time -= ODE_STEPSIZE;
2021 nodeframes++;
2022 }
2023
2024 lock (_badCharacter)
2025 {
2026 if (_badCharacter.Count > 0)
2027 {
2028 foreach (OdeCharacter chr in _badCharacter)
2029 {
2030 RemoveCharacter(chr);
2031 }
2032
2033 _badCharacter.Clear();
2034 }
2035 }
2036
2037 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
2038 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
2039 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
2040 int nbodies = d.NTotalBodies;
2041 int ngeoms = d.NTotalGeoms;
2042
2043 // Finished with all sim stepping. If requested, dump world state to file for debugging.
2044 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
2045 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
2046 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
2047 {
2048 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
2049 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
2050
2051 if (physics_logging_append_existing_logfile)
2052 {
2053 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
2054 TextWriter fwriter = File.AppendText(fname);
2055 fwriter.WriteLine(header);
2056 fwriter.Close();
2057 }
2058
2059 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
2060 }
2061
2062 // think time dilation as to do with dinamic step size that we dont' have
2063 // even so tell something to world
2064 if (nodeframes < 10) // we did the requested loops
2065 m_timeDilation = 1.0f;
2066 else if (step_time > 0)
2067 {
2068 m_timeDilation = timeStep / step_time;
2069 if (m_timeDilation > 1)
2070 m_timeDilation = 1;
2071 if (step_time > m_SkipFramesAtms)
2072 step_time = 0;
2073 }
2074 }
2075
2076// return nodeframes * ODE_STEPSIZE; // return real simulated time
2077 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
2078 }
2079
2080 /// <summary>
2081 public override void GetResults()
2082 {
2083 }
2084
2085 public override bool IsThreaded
2086 {
2087 // for now we won't be multithreaded
2088 get { return (false); }
2089 }
2090
2091 public float GetTerrainHeightAtXY(float x, float y)
2092 {
2093
2094
2095 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2096 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2097
2098
2099 IntPtr heightFieldGeom = IntPtr.Zero;
2100
2101 // get region map
2102 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
2103 return 0f;
2104
2105 if (heightFieldGeom == IntPtr.Zero)
2106 return 0f;
2107
2108 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
2109 return 0f;
2110
2111 // TerrainHeightField for ODE as offset 1m
2112 x += 1f - offsetX;
2113 y += 1f - offsetY;
2114
2115 // make position fit into array
2116 if (x < 0)
2117 x = 0;
2118 if (y < 0)
2119 y = 0;
2120
2121 // integer indexs
2122 int ix;
2123 int iy;
2124 // interpolators offset
2125 float dx;
2126 float dy;
2127
2128 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
2129
2130 if (OdeUbitLib)
2131 {
2132 if (x < regsize - 1)
2133 {
2134 ix = (int)x;
2135 dx = x - (float)ix;
2136 }
2137 else // out world use external height
2138 {
2139 ix = regsize - 2;
2140 dx = 0;
2141 }
2142 if (y < regsize - 1)
2143 {
2144 iy = (int)y;
2145 dy = y - (float)iy;
2146 }
2147 else
2148 {
2149 iy = regsize - 2;
2150 dy = 0;
2151 }
2152 }
2153
2154 else
2155 {
2156 // we still have square fixed size regions
2157 // also flip x and y because of how map is done for ODE fliped axis
2158 // so ix,iy,dx and dy are inter exchanged
2159 if (x < regsize - 1)
2160 {
2161 iy = (int)x;
2162 dy = x - (float)iy;
2163 }
2164 else // out world use external height
2165 {
2166 iy = regsize - 2;
2167 dy = 0;
2168 }
2169 if (y < regsize - 1)
2170 {
2171 ix = (int)y;
2172 dx = y - (float)ix;
2173 }
2174 else
2175 {
2176 ix = regsize - 2;
2177 dx = 0;
2178 }
2179 }
2180
2181 float h0;
2182 float h1;
2183 float h2;
2184
2185 iy *= regsize;
2186 iy += ix; // all indexes have iy + ix
2187
2188 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2189 /*
2190 if ((dx + dy) <= 1.0f)
2191 {
2192 h0 = ((float)heights[iy]); // 0,0 vertice
2193 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2194 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2195 }
2196 else
2197 {
2198 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2199 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2200 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2201 }
2202 */
2203 h0 = ((float)heights[iy]); // 0,0 vertice
2204
2205 if ((dy > dx))
2206 {
2207 iy += regsize;
2208 h2 = (float)heights[iy]; // 0,1 vertice
2209 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2210 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2211 }
2212 else
2213 {
2214 iy++;
2215 h2 = (float)heights[iy]; // vertice 1,0
2216 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2217 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2218 }
2219
2220 return h0 + h1 + h2;
2221 }
2222
2223
2224 public override void SetTerrain(float[] heightMap)
2225 {
2226 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2227 {
2228 if (m_parentScene is OdeScene)
2229 {
2230 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2231 }
2232 }
2233 else
2234 {
2235 SetTerrain(heightMap, m_worldOffset);
2236 }
2237 }
2238
2239 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2240 {
2241 SetTerrain(heightMap, pOffset);
2242 }
2243
2244 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2245 {
2246 if (OdeUbitLib)
2247 UbitSetTerrain(heightMap, pOffset);
2248 else
2249 OriSetTerrain(heightMap, pOffset);
2250 }
2251
2252 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2253 {
2254 // assumes 1m size grid and constante size square regions
2255 // needs to know about sims around in future
2256
2257 float[] _heightmap;
2258
2259 uint heightmapWidth = Constants.RegionSize + 2;
2260 uint heightmapHeight = Constants.RegionSize + 2;
2261
2262 uint heightmapWidthSamples = heightmapWidth + 1;
2263 uint heightmapHeightSamples = heightmapHeight + 1;
2264
2265 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2266
2267 const float scale = 1.0f;
2268 const float offset = 0.0f;
2269 const float thickness = 10f;
2270 const int wrap = 0;
2271
2272 uint regionsize = Constants.RegionSize;
2273
2274 float hfmin = float.MaxValue;
2275 float hfmax = float.MinValue;
2276 float val;
2277 uint xx;
2278 uint yy;
2279
2280 uint maxXXYY = regionsize - 1;
2281 // flipping map adding one margin all around so things don't fall in edges
2282
2283 uint xt = 0;
2284 xx = 0;
2285
2286 for (uint x = 0; x < heightmapWidthSamples; x++)
2287 {
2288 if (x > 1 && xx < maxXXYY)
2289 xx++;
2290 yy = 0;
2291 for (uint y = 0; y < heightmapHeightSamples; y++)
2292 {
2293 if (y > 1 && y < maxXXYY)
2294 yy += regionsize;
2295
2296 val = heightMap[yy + xx];
2297 if (val < 0.0f)
2298 val = 0.0f; // no neg terrain as in chode
2299 _heightmap[xt + y] = val;
2300
2301 if (hfmin > val)
2302 hfmin = val;
2303 if (hfmax < val)
2304 hfmax = val;
2305 }
2306 xt += heightmapHeightSamples;
2307 }
2308 lock (OdeLock)
2309 {
2310 IntPtr GroundGeom = IntPtr.Zero;
2311 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2312 {
2313 RegionTerrain.Remove(pOffset);
2314 if (GroundGeom != IntPtr.Zero)
2315 {
2316 d.GeomDestroy(GroundGeom);
2317
2318 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2319 {
2320 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2321 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2322 TerrainHeightFieldHeights.Remove(GroundGeom);
2323 }
2324 }
2325 }
2326 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2327
2328 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2329
2330 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2331 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2332 offset, thickness, wrap);
2333
2334 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2335 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2336 if (GroundGeom != IntPtr.Zero)
2337 {
2338 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2339 d.GeomSetCollideBits(GroundGeom, 0);
2340
2341 }
2342 geom_name_map[GroundGeom] = "Terrain";
2343
2344 d.Matrix3 R = new d.Matrix3();
2345
2346 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2347 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2348
2349
2350 q1 = q1 * q2;
2351
2352 Vector3 v3;
2353 float angle;
2354 q1.GetAxisAngle(out v3, out angle);
2355
2356 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2357 d.GeomSetRotation(GroundGeom, ref R);
2358 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2359 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2360// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2361 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2362 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2363
2364 }
2365 }
2366
2367 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2368 {
2369 // assumes 1m size grid and constante size square regions
2370 // needs to know about sims around in future
2371
2372 float[] _heightmap;
2373
2374 uint heightmapWidth = Constants.RegionSize + 2;
2375 uint heightmapHeight = Constants.RegionSize + 2;
2376
2377 uint heightmapWidthSamples = heightmapWidth + 1;
2378 uint heightmapHeightSamples = heightmapHeight + 1;
2379
2380 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2381
2382
2383 uint regionsize = Constants.RegionSize;
2384
2385 float hfmin = float.MaxValue;
2386// float hfmax = float.MinValue;
2387 float val;
2388
2389
2390 uint maxXXYY = regionsize - 1;
2391 // adding one margin all around so things don't fall in edges
2392
2393 uint xx;
2394 uint yy = 0;
2395 uint yt = 0;
2396
2397 for (uint y = 0; y < heightmapHeightSamples; y++)
2398 {
2399 if (y > 1 && y < maxXXYY)
2400 yy += regionsize;
2401 xx = 0;
2402 for (uint x = 0; x < heightmapWidthSamples; x++)
2403 {
2404 if (x > 1 && x < maxXXYY)
2405 xx++;
2406
2407 val = heightMap[yy + xx];
2408 if (val < 0.0f)
2409 val = 0.0f; // no neg terrain as in chode
2410 _heightmap[yt + x] = val;
2411
2412 if (hfmin > val)
2413 hfmin = val;
2414// if (hfmax < val)
2415// hfmax = val;
2416 }
2417 yt += heightmapWidthSamples;
2418 }
2419 lock (OdeLock)
2420 {
2421 IntPtr GroundGeom = IntPtr.Zero;
2422 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2423 {
2424 RegionTerrain.Remove(pOffset);
2425 if (GroundGeom != IntPtr.Zero)
2426 {
2427 d.GeomDestroy(GroundGeom);
2428
2429 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2430 {
2431 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2432 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2433 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2434 TerrainHeightFieldHeights.Remove(GroundGeom);
2435 }
2436 }
2437 }
2438 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2439
2440 const int wrap = 0;
2441 float thickness = hfmin;
2442 if (thickness < 0)
2443 thickness = 1;
2444
2445 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2446
2447 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2448 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2449 thickness, wrap);
2450
2451// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2452 GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1);
2453 if (GroundGeom != IntPtr.Zero)
2454 {
2455 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2456 d.GeomSetCollideBits(GroundGeom, 0);
2457
2458 }
2459 geom_name_map[GroundGeom] = "Terrain";
2460
2461 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2462 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2463 // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2464 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2465 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2466 }
2467 }
2468
2469
2470 public override void DeleteTerrain()
2471 {
2472 }
2473
2474 public float GetWaterLevel()
2475 {
2476 return waterlevel;
2477 }
2478
2479 public override bool SupportsCombining()
2480 {
2481 return true;
2482 }
2483/*
2484 public override void UnCombine(PhysicsScene pScene)
2485 {
2486 IntPtr localGround = IntPtr.Zero;
2487// float[] localHeightfield;
2488 bool proceed = false;
2489 List<IntPtr> geomDestroyList = new List<IntPtr>();
2490
2491 lock (OdeLock)
2492 {
2493 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2494 {
2495 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2496 {
2497 if (geom == localGround)
2498 {
2499// localHeightfield = TerrainHeightFieldHeights[geom];
2500 proceed = true;
2501 }
2502 else
2503 {
2504 geomDestroyList.Add(geom);
2505 }
2506 }
2507
2508 if (proceed)
2509 {
2510 m_worldOffset = Vector3.Zero;
2511 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2512 m_parentScene = null;
2513
2514 foreach (IntPtr g in geomDestroyList)
2515 {
2516 // removingHeightField needs to be done or the garbage collector will
2517 // collect the terrain data before we tell ODE to destroy it causing
2518 // memory corruption
2519 if (TerrainHeightFieldHeights.ContainsKey(g))
2520 {
2521// float[] removingHeightField = TerrainHeightFieldHeights[g];
2522 TerrainHeightFieldHeights.Remove(g);
2523
2524 if (RegionTerrain.ContainsKey(g))
2525 {
2526 RegionTerrain.Remove(g);
2527 }
2528
2529 d.GeomDestroy(g);
2530 //removingHeightField = new float[0];
2531 }
2532 }
2533
2534 }
2535 else
2536 {
2537 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2538 }
2539 }
2540 }
2541 }
2542*/
2543 public override void SetWaterLevel(float baseheight)
2544 {
2545 waterlevel = baseheight;
2546 randomizeWater(waterlevel);
2547 }
2548
2549 public void randomizeWater(float baseheight)
2550 {
2551 const uint heightmapWidth = Constants.RegionSize + 2;
2552 const uint heightmapHeight = Constants.RegionSize + 2;
2553 const uint heightmapWidthSamples = heightmapWidth + 1;
2554 const uint heightmapHeightSamples = heightmapHeight + 1;
2555
2556 const float scale = 1.0f;
2557 const float offset = 0.0f;
2558 const int wrap = 0;
2559
2560 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
2561
2562 float maxheigh = float.MinValue;
2563 float minheigh = float.MaxValue;
2564 float val;
2565 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
2566 {
2567
2568 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
2569 _watermap[i] = val;
2570 if (maxheigh < val)
2571 maxheigh = val;
2572 if (minheigh > val)
2573 minheigh = val;
2574 }
2575
2576 float thickness = minheigh;
2577
2578 lock (OdeLock)
2579 {
2580 if (WaterGeom != IntPtr.Zero)
2581 {
2582 d.GeomDestroy(WaterGeom);
2583 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2584 WaterGeom = IntPtr.Zero;
2585 WaterHeightmapData = IntPtr.Zero;
2586 if(WaterMapHandler.IsAllocated)
2587 WaterMapHandler.Free();
2588 }
2589
2590 WaterHeightmapData = d.GeomHeightfieldDataCreate();
2591
2592 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2593
2594 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
2595 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2596 offset, thickness, wrap);
2597 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
2598 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
2599 if (WaterGeom != IntPtr.Zero)
2600 {
2601 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
2602 d.GeomSetCollideBits(WaterGeom, 0);
2603
2604 geom_name_map[WaterGeom] = "Water";
2605
2606 d.Matrix3 R = new d.Matrix3();
2607
2608 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2609 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2610
2611 q1 = q1 * q2;
2612 Vector3 v3;
2613 float angle;
2614 q1.GetAxisAngle(out v3, out angle);
2615
2616 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2617 d.GeomSetRotation(WaterGeom, ref R);
2618 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
2619 }
2620 }
2621 }
2622
2623 public override void Dispose()
2624 {
2625 m_rayCastManager.Dispose();
2626 m_rayCastManager = null;
2627
2628 lock (OdeLock)
2629 {
2630 lock (_prims)
2631 {
2632 foreach (OdePrim prm in _prims)
2633 {
2634 RemovePrim(prm);
2635 }
2636 }
2637
2638 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2639 {
2640 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2641 {
2642 if (gch.IsAllocated)
2643 gch.Free();
2644 }
2645 }
2646
2647 if (WaterGeom != IntPtr.Zero)
2648 {
2649 d.GeomDestroy(WaterGeom);
2650 WaterGeom = IntPtr.Zero;
2651 if (WaterHeightmapData != IntPtr.Zero)
2652 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2653 WaterHeightmapData = IntPtr.Zero;
2654
2655 if (WaterMapHandler.IsAllocated)
2656 WaterMapHandler.Free();
2657 }
2658
2659
2660 if (ContactgeomsArray != IntPtr.Zero)
2661 Marshal.FreeHGlobal(ContactgeomsArray);
2662 if (GlobalContactsArray != IntPtr.Zero)
2663 Marshal.FreeHGlobal(GlobalContactsArray);
2664
2665
2666 d.WorldDestroy(world);
2667 //d.CloseODE();
2668 }
2669 }
2670
2671 public override Dictionary<uint, float> GetTopColliders()
2672 {
2673 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2674 int cnt = 0;
2675 lock (_prims)
2676 {
2677 foreach (OdePrim prm in _prims)
2678 {
2679 if (prm.CollisionScore > 0)
2680 {
2681 returncolliders.Add(prm.m_localID, prm.CollisionScore);
2682 cnt++;
2683 prm.CollisionScore = 0f;
2684 if (cnt > 25)
2685 {
2686 break;
2687 }
2688 }
2689 }
2690 }
2691 return returncolliders;
2692 }
2693
2694 public override bool SupportsRayCast()
2695 {
2696 return true;
2697 }
2698
2699 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2700 {
2701 if (retMethod != null)
2702 {
2703 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2704 }
2705 }
2706
2707 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2708 {
2709 if (retMethod != null)
2710 {
2711 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2712 }
2713 }
2714
2715 // don't like this
2716 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2717 {
2718 ContactResult[] ourResults = null;
2719 RayCallback retMethod = delegate(List<ContactResult> results)
2720 {
2721 ourResults = new ContactResult[results.Count];
2722 results.CopyTo(ourResults, 0);
2723 };
2724 int waitTime = 0;
2725 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2726 while (ourResults == null && waitTime < 1000)
2727 {
2728 Thread.Sleep(1);
2729 waitTime++;
2730 }
2731 if (ourResults == null)
2732 return new List<ContactResult>();
2733 return new List<ContactResult>(ourResults);
2734 }
2735
2736 public override bool SuportsRaycastWorldFiltered()
2737 {
2738 return true;
2739 }
2740
2741 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2742 {
2743 object SyncObject = new object();
2744 List<ContactResult> ourresults = new List<ContactResult>();
2745
2746 RayCallback retMethod = delegate(List<ContactResult> results)
2747 {
2748 lock (SyncObject)
2749 {
2750 ourresults = results;
2751 Monitor.PulseAll(SyncObject);
2752 }
2753 };
2754
2755 lock (SyncObject)
2756 {
2757 m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod);
2758 if (!Monitor.Wait(SyncObject, 500))
2759 return null;
2760 else
2761 return ourresults;
2762 }
2763 }
2764
2765 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2766 {
2767 if (retMethod != null && actor !=null)
2768 {
2769 IntPtr geom;
2770 if (actor is OdePrim)
2771 geom = ((OdePrim)actor).prim_geom;
2772 else if (actor is OdeCharacter)
2773 geom = ((OdePrim)actor).prim_geom;
2774 else
2775 return;
2776 if (geom == IntPtr.Zero)
2777 return;
2778 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2779 }
2780 }
2781
2782 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2783 {
2784 if (retMethod != null && actor != null)
2785 {
2786 IntPtr geom;
2787 if (actor is OdePrim)
2788 geom = ((OdePrim)actor).prim_geom;
2789 else if (actor is OdeCharacter)
2790 geom = ((OdePrim)actor).prim_geom;
2791 else
2792 return;
2793 if (geom == IntPtr.Zero)
2794 return;
2795
2796 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2797 }
2798 }
2799
2800 // don't like this
2801 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2802 {
2803 if (actor != null)
2804 {
2805 IntPtr geom;
2806 if (actor is OdePrim)
2807 geom = ((OdePrim)actor).prim_geom;
2808 else if (actor is OdeCharacter)
2809 geom = ((OdePrim)actor).prim_geom;
2810 else
2811 return new List<ContactResult>();
2812 if (geom == IntPtr.Zero)
2813 return new List<ContactResult>();
2814
2815 ContactResult[] ourResults = null;
2816 RayCallback retMethod = delegate(List<ContactResult> results)
2817 {
2818 ourResults = new ContactResult[results.Count];
2819 results.CopyTo(ourResults, 0);
2820 };
2821 int waitTime = 0;
2822 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2823 while (ourResults == null && waitTime < 1000)
2824 {
2825 Thread.Sleep(1);
2826 waitTime++;
2827 }
2828 if (ourResults == null)
2829 return new List<ContactResult>();
2830 return new List<ContactResult>(ourResults);
2831 }
2832 return new List<ContactResult>();
2833 }
2834 }
2835}