aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs1438
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs4082
-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.cs38
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsScene.cs29
-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.cs26
-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.cs1454
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs993
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs3725
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs443
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs1960
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs90
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2356
34 files changed, 26704 insertions, 37 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..2945199
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -0,0 +1,1438 @@
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 // unique UUID of this character object
144 public UUID m_uuid;
145 public bool bad = false;
146 private Object m_syncRoot = new Object();
147
148 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)
149 {
150 m_uuid = UUID.Random();
151
152 if (pos.IsFinite())
153 {
154 if (pos.Z > 9999999f)
155 {
156 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
157 }
158 if (pos.Z < -90000f)
159 {
160 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
161 }
162 _position = pos;
163 m_taintPosition.X = pos.X;
164 m_taintPosition.Y = pos.Y;
165 m_taintPosition.Z = pos.Z;
166 }
167 else
168 {
169 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
170 m_taintPosition.X = _position.X;
171 m_taintPosition.Y = _position.Y;
172 m_taintPosition.Z = _position.Z;
173 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
174 }
175
176 _parent_scene = parent_scene;
177
178 PID_D = pid_d;
179 PID_P = pid_p;
180 CAPSULE_RADIUS = capsule_radius;
181 m_tensor = tensor;
182 m_density = density;
183 heightFudgeFactor = height_fudge_factor;
184 walkDivisor = walk_divisor;
185 runDivisor = rundivisor;
186
187 // m_StandUpRotation =
188 // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
189 // 0.5f);
190
191 for (int i = 0; i < 11; i++)
192 {
193 m_colliderarr[i] = false;
194 }
195 CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
196 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
197 m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
198
199 m_isPhysical = false; // current status: no ODE information exists
200 m_tainted_isPhysical = true; // new tainted status: need to create ODE information
201
202 _parent_scene.AddPhysicsActorTaint(this);
203
204 m_name = avName;
205 }
206
207 public override int PhysicsActorType
208 {
209 get { return (int) ActorTypes.Agent; }
210 set { return; }
211 }
212
213 /// <summary>
214 /// If this is set, the avatar will move faster
215 /// </summary>
216 public override bool SetAlwaysRun
217 {
218 get { return m_alwaysRun; }
219 set { m_alwaysRun = value; }
220 }
221
222 public override uint LocalID
223 {
224 set { m_localID = value; }
225 }
226
227 public override bool Grabbed
228 {
229 set { return; }
230 }
231
232 public override bool Selected
233 {
234// set { return; }
235 set { jumping = value; } // add for jumping flag
236 }
237
238 public override float Buoyancy
239 {
240 get { return m_buoyancy; }
241 set { m_buoyancy = value; }
242 }
243
244 public override bool FloatOnWater
245 {
246 set { return; }
247 }
248
249 public override bool IsPhysical
250 {
251 get { return false; }
252 set { return; }
253 }
254
255 public override bool ThrottleUpdates
256 {
257 get { return false; }
258 set { return; }
259 }
260
261 public override bool Flying
262 {
263 get { return flying; }
264 set { flying = value; }
265 }
266
267 /// <summary>
268 /// Returns if the avatar is colliding in general.
269 /// This includes the ground and objects and avatar.
270 /// </summary>
271 public override bool IsColliding
272 {
273//#@ get { return m_iscolliding; }
274 get { //##
275//Console.WriteLine(">>>>>>>>>>>> IC get = {0}", m_iscolliding); //##
276 return m_iscolliding; } //##
277 set
278 {
279 int i;
280 int truecount = 0;
281 int falsecount = 0;
282
283 if (m_colliderarr.Length >= 10)
284 {
285 for (i = 0; i < 10; i++)
286 {
287 m_colliderarr[i] = m_colliderarr[i + 1];
288 }
289 }
290 m_colliderarr[10] = value;
291
292 for (i = 0; i < 11; i++)
293 {
294 if (m_colliderarr[i])
295 {
296 truecount++;
297 }
298 else
299 {
300 falsecount++;
301 }
302 }
303
304 // Equal truecounts and false counts means we're colliding with something.
305
306 if (falsecount > 1.2*truecount)
307 {
308 m_iscolliding = false;
309 }
310 else
311 {
312 m_iscolliding = true;
313 }
314// ## Console.WriteLine("IC SET = {0} t{1} f{2} i {3}", value, truecount, falsecount, m_iscolliding);
315 if (m_wascolliding != m_iscolliding)
316 {
317 //base.SendCollisionUpdate(new CollisionEventUpdate());
318 }
319 m_wascolliding = m_iscolliding;
320 }
321 }
322
323 /// <summary>
324 /// Returns if an avatar is colliding with the ground
325 /// </summary>
326 public override bool CollidingGround
327 {
328 get { return m_iscollidingGround; }
329 set
330 {
331 // Collisions against the ground are not really reliable
332 // So, to get a consistant value we have to average the current result over time
333 // Currently we use 1 second = 10 calls to this.
334 int i;
335 int truecount = 0;
336 int falsecount = 0;
337
338 if (m_colliderGroundarr.Length >= 10)
339 {
340 for (i = 0; i < 10; i++)
341 {
342 m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
343 }
344 }
345 m_colliderGroundarr[10] = value;
346
347 for (i = 0; i < 11; i++)
348 {
349 if (m_colliderGroundarr[i])
350 {
351 truecount++;
352 }
353 else
354 {
355 falsecount++;
356 }
357 }
358
359 // Equal truecounts and false counts means we're colliding with something.
360
361 if (falsecount > 1.2*truecount)
362 {
363 m_iscollidingGround = false;
364 }
365 else
366 {
367 m_iscollidingGround = true;
368 }
369 if (m_wascollidingGround != m_iscollidingGround)
370 {
371 //base.SendCollisionUpdate(new CollisionEventUpdate());
372 }
373 m_wascollidingGround = m_iscollidingGround;
374 }
375 }
376
377 /// <summary>
378 /// Returns if the avatar is colliding with an object
379 /// </summary>
380 public override bool CollidingObj
381 {
382 get { return m_iscollidingObj; }
383 set
384 {
385 m_iscollidingObj = value;
386 if (value)
387 m_pidControllerActive = false;
388 else
389 m_pidControllerActive = true;
390 }
391 }
392
393 /// <summary>
394 /// turn the PID controller on or off.
395 /// The PID Controller will turn on all by itself in many situations
396 /// </summary>
397 /// <param name="status"></param>
398 public void SetPidStatus(bool status)
399 {
400 m_pidControllerActive = status;
401 }
402
403 public override bool Stopped
404 {
405 get { return _zeroFlag; }
406 }
407
408 /// <summary>
409 /// This 'puts' an avatar somewhere in the physics space.
410 /// Not really a good choice unless you 'know' it's a good
411 /// spot otherwise you're likely to orbit the avatar.
412 /// </summary>
413 public override Vector3 Position
414 {
415 get { return _position; }
416 set
417 {
418 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
419 {
420 if (value.IsFinite())
421 {
422 if (value.Z > 9999999f)
423 {
424 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
425 }
426 if (value.Z < -90000f)
427 {
428 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
429 }
430
431 _position.X = value.X;
432 _position.Y = value.Y;
433 _position.Z = value.Z;
434
435 m_taintPosition.X = value.X;
436 m_taintPosition.Y = value.Y;
437 m_taintPosition.Z = value.Z;
438 _parent_scene.AddPhysicsActorTaint(this);
439 }
440 else
441 {
442 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
443 }
444 }
445 }
446 }
447
448 public override Vector3 RotationalVelocity
449 {
450 get { return m_rotationalVelocity; }
451 set { m_rotationalVelocity = value; }
452 }
453
454 /// <summary>
455 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
456 /// and use it to offset landings properly
457 /// </summary>
458 public override Vector3 Size
459 {
460 get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
461 set
462 {
463 if (value.IsFinite())
464 {
465 m_pidControllerActive = true;
466
467 Vector3 SetSize = value;
468 m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
469 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
470
471 Velocity = Vector3.Zero;
472 m_taintPosition = _position; // update the stale taint position
473 _parent_scene.AddPhysicsActorTaint(this);
474 }
475 else
476 {
477 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
478 }
479 }
480 }
481
482 private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
483 {
484 movementVector.Z = 0f;
485 float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
486 if (magnitude < 0.1f) return;
487
488 // normalize the velocity vector
489 float invMagnitude = 1.0f / magnitude;
490 movementVector.X *= invMagnitude;
491 movementVector.Y *= invMagnitude;
492
493 // if we change the capsule heading too often, the capsule can fall down
494 // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
495 // meaning only 4 possible capsule tilt orientations
496 if (movementVector.X > 0)
497 {
498 // east
499 if (movementVector.Y > 0)
500 {
501 // northeast
502 movementVector.X = (float)Math.Sqrt(2.0);
503 movementVector.Y = (float)Math.Sqrt(2.0);
504 }
505 else
506 {
507 // southeast
508 movementVector.X = (float)Math.Sqrt(2.0);
509 movementVector.Y = -(float)Math.Sqrt(2.0);
510 }
511 }
512 else
513 {
514 // west
515 if (movementVector.Y > 0)
516 {
517 // northwest
518 movementVector.X = -(float)Math.Sqrt(2.0);
519 movementVector.Y = (float)Math.Sqrt(2.0);
520 }
521 else
522 {
523 // southwest
524 movementVector.X = -(float)Math.Sqrt(2.0);
525 movementVector.Y = -(float)Math.Sqrt(2.0);
526 }
527 }
528
529
530 // movementVector.Z is zero
531
532 // calculate tilt components based on desired amount of tilt and current (snapped) heading.
533 // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
534 float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
535 float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
536
537 //m_log.Debug("[PHYSICS] changing avatar tilt");
538 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
539 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
540 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
541 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
542 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
543 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
544 }
545
546 /// <summary>
547 /// This creates the Avatar's physical Surrogate at the position supplied
548 /// </summary>
549 /// <param name="npositionX"></param>
550 /// <param name="npositionY"></param>
551 /// <param name="npositionZ"></param>
552
553 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
554 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
555 // place that is safe to call this routine AvatarGeomAndBodyCreation.
556 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
557 {
558 //CAPSULE_LENGTH = -5;
559 //CAPSULE_RADIUS = -5;
560 int dAMotorEuler = 1;
561 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
562 if (CAPSULE_LENGTH <= 0)
563 {
564 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
565 CAPSULE_LENGTH = 0.01f;
566
567 }
568
569 if (CAPSULE_RADIUS <= 0)
570 {
571 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
572 CAPSULE_RADIUS = 0.01f;
573
574 }
575
576 if(Shell != IntPtr.Zero)
577 {
578 try
579 {
580 d.GeomDestroy(Shell);
581 }
582 catch (System.AccessViolationException)
583 {
584 m_log.Error("[PHYSICS]: PrimGeom dead");
585 }
586 // Remove any old entries
587//string tShell;
588//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
589//Console.WriteLine("**** Remove {0}", tShell);
590 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
591 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
592 }
593
594 Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
595 _parent_scene.geom_name_map[Shell] = m_name;
596 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
597//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);
598
599 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
600 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
601
602 d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
603 Body = d.BodyCreate(_parent_scene.world);
604 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
605
606 _position.X = npositionX;
607 _position.Y = npositionY;
608 _position.Z = npositionZ;
609
610
611 m_taintPosition.X = npositionX;
612 m_taintPosition.Y = npositionY;
613 m_taintPosition.Z = npositionZ;
614
615 d.BodySetMass(Body, ref ShellMass);
616 d.Matrix3 m_caprot;
617 // 90 Stand up on the cap of the capped cyllinder
618 if (_parent_scene.IsAvCapsuleTilted)
619 {
620 d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
621 }
622 else
623 {
624 d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
625 }
626
627
628 d.GeomSetRotation(Shell, ref m_caprot);
629 d.BodySetRotation(Body, ref m_caprot);
630
631 d.GeomSetBody(Shell, Body);
632
633
634 // The purpose of the AMotor here is to keep the avatar's physical
635 // surrogate from rotating while moving
636 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
637 d.JointAttach(Amotor, Body, IntPtr.Zero);
638 d.JointSetAMotorMode(Amotor, dAMotorEuler);
639 d.JointSetAMotorNumAxes(Amotor, 3);
640 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
641 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
642 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
643 d.JointSetAMotorAngle(Amotor, 0, 0);
644 d.JointSetAMotorAngle(Amotor, 1, 0);
645 d.JointSetAMotorAngle(Amotor, 2, 0);
646
647 // These lowstops and high stops are effectively (no wiggle room)
648 if (_parent_scene.IsAvCapsuleTilted)
649 {
650 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
651 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
652 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
653 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
654 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
655 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
656 }
657 else
658 {
659 #region Documentation of capsule motor LowStop and HighStop parameters
660 // Intentionally introduce some tilt into the capsule by setting
661 // the motor stops to small epsilon values. This small tilt prevents
662 // the capsule from falling into the terrain; a straight-up capsule
663 // (with -0..0 motor stops) falls into the terrain for reasons yet
664 // to be comprehended in their entirety.
665 #endregion
666 AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
667 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
668 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
669 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
670 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
671 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
672 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
673 }
674
675 // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the
676 // capped cyllinder will fall over
677 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
678 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
679
680 //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
681 //d.QfromR(
682 //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
683 //
684 //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
685 //standupStraight();
686 }
687
688 //
689 /// <summary>
690 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
691 /// This may be used in calculations in the scene/scenepresence
692 /// </summary>
693 public override float Mass
694 {
695 get
696 {
697 float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
698 return m_density*AVvolume;
699 }
700 }
701 public override void link(PhysicsActor obj)
702 {
703
704 }
705
706 public override void delink()
707 {
708
709 }
710
711 public override void LockAngularMotion(Vector3 axis)
712 {
713
714 }
715
716// This code is very useful. Written by DanX0r. We're just not using it right now.
717// Commented out to prevent a warning.
718//
719// private void standupStraight()
720// {
721// // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
722// // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
723// // change appearance and when you enter the simulator
724// // After this routine is done, the amotor stabilizes much quicker
725// d.Vector3 feet;
726// d.Vector3 head;
727// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
728// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
729// float posture = head.Z - feet.Z;
730
731// // restoring force proportional to lack of posture:
732// float servo = (2.5f - posture) * POSTURE_SERVO;
733// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
734// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
735// //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
736// //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
737// }
738
739 public override Vector3 Force
740 {
741 get { return _target_velocity; }
742 set { return; }
743 }
744
745 public override int VehicleType
746 {
747 get { return 0; }
748 set { return; }
749 }
750
751 public override void VehicleFloatParam(int param, float value)
752 {
753
754 }
755
756 public override void VehicleVectorParam(int param, Vector3 value)
757 {
758
759 }
760
761 public override void VehicleRotationParam(int param, Quaternion rotation)
762 {
763
764 }
765
766 public override void VehicleFlags(int flags, bool remove)
767 {
768 }
769
770 public override void SetVolumeDetect(int param)
771 {
772
773 }
774
775 public override Vector3 CenterOfMass
776 {
777 get { return Vector3.Zero; }
778 }
779
780 public override Vector3 GeometricCenter
781 {
782 get { return Vector3.Zero; }
783 }
784
785 public override PrimitiveBaseShape Shape
786 {
787 set { return; }
788 }
789
790 public override Vector3 Velocity
791 {
792 get {
793 // There's a problem with Vector3.Zero! Don't Use it Here!
794 if (_zeroFlag)
795 return Vector3.Zero;
796 m_lastUpdateSent = false;
797 return _velocity;
798 }
799 set
800 {
801 if (value.IsFinite())
802 {
803 m_pidControllerActive = true;
804 _target_velocity = value;
805 }
806 else
807 {
808 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
809 }
810 }
811 }
812
813 public override Vector3 Torque
814 {
815 get { return Vector3.Zero; }
816 set { return; }
817 }
818
819 public override float CollisionScore
820 {
821 get { return 0f; }
822 set { }
823 }
824
825 public override bool Kinematic
826 {
827 get { return false; }
828 set { }
829 }
830
831 public override Quaternion Orientation
832 {
833 get { return Quaternion.Identity; }
834 set {
835 //Matrix3 or = Orientation.ToRotationMatrix();
836 //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
837 //d.BodySetRotation(Body, ref ord);
838 }
839 }
840
841 public override Vector3 Acceleration
842 {
843 get { return _acceleration; }
844 set { _acceleration = value; }
845 }
846
847 public void SetAcceleration(Vector3 accel)
848 {
849 m_pidControllerActive = true;
850 _acceleration = accel;
851 }
852
853 /// <summary>
854 /// Adds the force supplied to the Target Velocity
855 /// The PID controller takes this target velocity and tries to make it a reality
856 /// </summary>
857 /// <param name="force"></param>
858 public override void AddForce(Vector3 force, bool pushforce)
859 {
860 if (force.IsFinite())
861 {
862 if (pushforce)
863 {
864 m_pidControllerActive = false;
865 force *= 100f;
866//Console.WriteLine("DF 1"); // ##
867 if (!force.ApproxEquals(Vector3.Zero, 0.01f))
868 doForce(force);
869 // If uncommented, things get pushed off world
870 //
871 // m_log.Debug("Push!");
872 // _target_velocity.X += force.X;
873 // _target_velocity.Y += force.Y;
874 // _target_velocity.Z += force.Z;
875 }
876 else
877 {
878 m_pidControllerActive = true;
879 _target_velocity.X += force.X;
880 _target_velocity.Y += force.Y;
881 _target_velocity.Z += force.Z;
882 }
883 }
884 else
885 {
886 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
887 }
888 //m_lastUpdateSent = false;
889 }
890
891 public override void AddAngularForce(Vector3 force, bool pushforce)
892 {
893
894 }
895
896 /// <summary>
897 /// After all of the forces add up with 'add force' we apply them with doForce
898 /// </summary>
899 /// <param name="force"></param>
900 public void doForce(Vector3 force)
901 {
902 if (!collidelock)
903 {
904 d.BodyAddForce(Body, force.X, force.Y, force.Z);
905 //d.BodySetRotation(Body, ref m_StandUpRotation);
906 //standupStraight();
907 d.Vector3 vel = d.BodyGetLinearVel(Body); //##
908//Console.WriteLine("AvVel <{0},{1},{2}>", vel.X, vel.Y, vel.Z); //##
909 }
910 }
911
912 public override void SetMomentum(Vector3 momentum)
913 {
914 }
915
916
917 /// <summary>
918 /// Called from Simulate
919 /// This is the avatar's movement control + PID Controller
920 /// </summary>
921 /// <param name="timeStep"></param>
922 public void Move(float timeStep, List<OdeCharacter> defects)
923 {
924 // no lock; for now it's only called from within Simulate()
925
926 // If the PID Controller isn't active then we set our force
927 // calculating base velocity to the current position
928
929 if (Body == IntPtr.Zero)
930 return;
931
932 if (m_pidControllerActive == false)
933 {
934 _zeroPosition = d.BodyGetPosition(Body);
935 }
936 //PidStatus = true;
937
938 d.Vector3 localpos = d.BodyGetPosition(Body);
939 Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
940
941 if (!localPos.IsFinite())
942 {
943
944 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
945 defects.Add(this);
946 // _parent_scene.RemoveCharacter(this);
947
948 // destroy avatar capsule and related ODE data
949 if (Amotor != IntPtr.Zero)
950 {
951 // Kill the Amotor
952 d.JointDestroy(Amotor);
953 Amotor = IntPtr.Zero;
954 }
955
956 //kill the Geometry
957 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
958
959 if (Body != IntPtr.Zero)
960 {
961 //kill the body
962 d.BodyDestroy(Body);
963
964 Body = IntPtr.Zero;
965 }
966
967 if(Shell != IntPtr.Zero)
968 {
969 try
970 {
971 d.GeomDestroy(Shell);
972 }
973 catch (System.AccessViolationException)
974 {
975 m_log.Error("[PHYSICS]: PrimGeom dead");
976 }
977 // Remove any old entries
978//string tShell;
979//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
980//Console.WriteLine("**** Remove {0}", tShell);
981
982 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
983 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
984 Shell = IntPtr.Zero;
985 }
986
987 return;
988 }
989
990 Vector3 vec = Vector3.Zero;
991 d.Vector3 vel = d.BodyGetLinearVel(Body);
992
993 float movementdivisor = 1f;
994
995 if (!m_alwaysRun)
996 {
997 movementdivisor = walkDivisor;
998 }
999 else
1000 {
1001 movementdivisor = runDivisor;
1002 }
1003
1004 // if velocity is zero, use position control; otherwise, velocity control
1005 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
1006 {
1007 // keep track of where we stopped. No more slippin' & slidin'
1008 if (!_zeroFlag)
1009 {
1010 _zeroFlag = true;
1011 _zeroPosition = d.BodyGetPosition(Body);
1012 }
1013 if (m_pidControllerActive)
1014 {
1015 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1016 // react to the physics scene by moving it's position.
1017 // Avatar to Avatar collisions
1018 // Prim to avatar collisions
1019
1020 d.Vector3 pos = d.BodyGetPosition(Body);
1021 float errX = _zeroPosition.X - pos.X;
1022 float errY = _zeroPosition.Y - pos.Y;
1023 if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) )
1024 {
1025 vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2);
1026 vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2);
1027 }
1028 else
1029 { // close, jump to lateral destination
1030 d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z);
1031 }
1032// if (flying)
1033 if (flying || jumping) // add for jumping
1034 {
1035 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1036 }
1037 }
1038 //PidStatus = true;
1039 }
1040 else
1041 {
1042 m_pidControllerActive = true;
1043 _zeroFlag = false;
1044 if (m_iscolliding && !flying)
1045 {
1046 // We're standing on something
1047 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
1048 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
1049 }
1050 else if (m_iscolliding && flying)
1051 {
1052 // We're flying and colliding with something
1053 vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
1054 vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
1055 }
1056 else if (!m_iscolliding && flying)
1057 {
1058 // we're in mid air suspended
1059 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
1060 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
1061 }
1062
1063 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
1064 {
1065 // We're colliding with something and we're not flying but we're moving
1066 // This means we're walking or running.
1067 d.Vector3 pos = d.BodyGetPosition(Body);
1068 vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
1069 if (_target_velocity.X > 0)
1070 {
1071 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1072 }
1073 if (_target_velocity.Y > 0)
1074 {
1075 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1076 }
1077 }
1078 else if (!m_iscolliding && !flying)
1079 {
1080 // we're not colliding and we're not flying so that means we're falling!
1081 // m_iscolliding includes collisions with the ground.
1082
1083 // d.Vector3 pos = d.BodyGetPosition(Body);
1084 if (Math.Abs(_target_velocity.X) > 0)
1085 {
1086 vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
1087 }
1088 if (Math.Abs(_target_velocity.Y) > 0)
1089 {
1090 vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
1091 }
1092 }
1093
1094 if (flying)
1095 {
1096 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
1097 }
1098 }
1099 if (flying)
1100 {
1101 vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
1102
1103 //Added for auto fly height. Kitto Flora
1104 //d.Vector3 pos = d.BodyGetPosition(Body);
1105 float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
1106
1107 if (_position.Z < target_altitude)
1108 {
1109 vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
1110 }
1111 // end add Kitto Flora
1112 }
1113 if (vec.IsFinite())
1114 {
1115 if (!vec.ApproxEquals(Vector3.Zero, 0.02f)) // 0.01 allows 0.002 !!
1116 {
1117//Console.WriteLine("DF 2"); // ##
1118
1119 doForce(vec);
1120 if (!_zeroFlag)
1121 {
1122// AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
1123 }
1124 }
1125 }
1126 else
1127 {
1128 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1129 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1130 defects.Add(this);
1131 // _parent_scene.RemoveCharacter(this);
1132 // destroy avatar capsule and related ODE data
1133 if (Amotor != IntPtr.Zero)
1134 {
1135 // Kill the Amotor
1136 d.JointDestroy(Amotor);
1137 Amotor = IntPtr.Zero;
1138 }
1139 //kill the Geometry
1140 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1141
1142 if (Body != IntPtr.Zero)
1143 {
1144 //kill the body
1145 d.BodyDestroy(Body);
1146
1147 Body = IntPtr.Zero;
1148 }
1149
1150 if(Shell != IntPtr.Zero)
1151 {
1152 try
1153 {
1154 d.GeomDestroy(Shell);
1155 }
1156 catch (System.AccessViolationException)
1157 {
1158 m_log.Error("[PHYSICS]: PrimGeom dead");
1159 }
1160 // Remove any old entries
1161//string tShell;
1162//_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1163//Console.WriteLine("**** Remove {0}", tShell);
1164
1165 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1166 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1167 Shell = IntPtr.Zero;
1168 }
1169 }
1170 }
1171
1172 /// <summary>
1173 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1174 /// </summary>
1175 public void UpdatePositionAndVelocity()
1176 {
1177 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1178 d.Vector3 vec;
1179 try
1180 {
1181 vec = d.BodyGetPosition(Body);
1182 }
1183 catch (NullReferenceException)
1184 {
1185 bad = true;
1186 _parent_scene.BadCharacter(this);
1187 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1188 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1189 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1190 }
1191
1192
1193 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
1194 if (vec.X < 0.0f) vec.X = 0.0f;
1195 if (vec.Y < 0.0f) vec.Y = 0.0f;
1196 if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
1197 if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
1198
1199 _position.X = vec.X;
1200 _position.Y = vec.Y;
1201 _position.Z = vec.Z;
1202
1203 // Did we move last? = zeroflag
1204 // This helps keep us from sliding all over
1205
1206 if (_zeroFlag)
1207 {
1208 _velocity.X = 0.0f;
1209 _velocity.Y = 0.0f;
1210 _velocity.Z = 0.0f;
1211
1212 // Did we send out the 'stopped' message?
1213 if (!m_lastUpdateSent)
1214 {
1215 m_lastUpdateSent = true;
1216 //base.RequestPhysicsterseUpdate();
1217
1218 }
1219 }
1220 else
1221 {
1222 m_lastUpdateSent = false;
1223 try
1224 {
1225 vec = d.BodyGetLinearVel(Body);
1226 }
1227 catch (NullReferenceException)
1228 {
1229 vec.X = _velocity.X;
1230 vec.Y = _velocity.Y;
1231 vec.Z = _velocity.Z;
1232 }
1233 _velocity.X = (vec.X);
1234 _velocity.Y = (vec.Y);
1235
1236 _velocity.Z = (vec.Z);
1237
1238 if (_velocity.Z < -6 && !m_hackSentFall)
1239 {
1240 m_hackSentFall = true;
1241 m_pidControllerActive = false;
1242 }
1243 else if (flying && !m_hackSentFly)
1244 {
1245 //m_hackSentFly = true;
1246 //base.SendCollisionUpdate(new CollisionEventUpdate());
1247 }
1248 else
1249 {
1250 m_hackSentFly = false;
1251 m_hackSentFall = false;
1252 }
1253 }
1254 }
1255
1256 /// <summary>
1257 /// Cleanup the things we use in the scene.
1258 /// </summary>
1259 public void Destroy()
1260 {
1261 m_tainted_isPhysical = false;
1262 _parent_scene.AddPhysicsActorTaint(this);
1263 }
1264
1265 public override void CrossingFailure()
1266 {
1267 }
1268
1269 public override Vector3 PIDTarget { set { return; } }
1270 public override bool PIDActive { set { return; } }
1271 public override float PIDTau { set { return; } }
1272
1273 public override float PIDHoverHeight { set { return; } }
1274 public override bool PIDHoverActive { set { return; } }
1275 public override PIDHoverType PIDHoverType { set { return; } }
1276 public override float PIDHoverTau { set { return; } }
1277
1278 public override Quaternion APIDTarget{ set { return; } }
1279
1280 public override bool APIDActive{ set { return; } }
1281
1282 public override float APIDStrength{ set { return; } }
1283
1284 public override float APIDDamping{ set { return; } }
1285
1286
1287 public override void SubscribeEvents(int ms)
1288 {
1289 m_requestedUpdateFrequency = ms;
1290 m_eventsubscription = ms;
1291 _parent_scene.addCollisionEventReporting(this);
1292 }
1293 public override void UnSubscribeEvents()
1294 {
1295 _parent_scene.remCollisionEventReporting(this);
1296 m_requestedUpdateFrequency = 0;
1297 m_eventsubscription = 0;
1298 }
1299 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1300 {
1301 if (m_eventsubscription > 0)
1302 {
1303 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1304 }
1305 }
1306
1307 public void SendCollisions()
1308 {
1309 if (m_eventsubscription > m_requestedUpdateFrequency)
1310 {
1311 if (CollisionEventsThisFrame != null)
1312 {
1313 base.SendCollisionUpdate(CollisionEventsThisFrame);
1314 }
1315 CollisionEventsThisFrame = new CollisionEventUpdate();
1316 m_eventsubscription = 0;
1317 }
1318 }
1319 public override bool SubscribedEvents()
1320 {
1321 if (m_eventsubscription > 0)
1322 return true;
1323 return false;
1324 }
1325
1326 public void ProcessTaints(float timestep)
1327 {
1328 lock (m_syncRoot)
1329 {
1330
1331 if (m_tainted_isPhysical != m_isPhysical)
1332 {
1333 if (m_tainted_isPhysical)
1334 {
1335 // Create avatar capsule and related ODE data
1336 if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
1337 {
1338 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1339 + (Shell!=IntPtr.Zero ? "Shell ":"")
1340 + (Body!=IntPtr.Zero ? "Body ":"")
1341 + (Amotor!=IntPtr.Zero ? "Amotor ":""));
1342 }
1343 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
1344 _parent_scene.AddCharacter(this);
1345 }
1346 else
1347 {
1348 _parent_scene.RemoveCharacter(this);
1349 // destroy avatar capsule and related ODE data
1350 if (Amotor != IntPtr.Zero)
1351 {
1352 // Kill the Amotor
1353 d.JointDestroy(Amotor);
1354 Amotor = IntPtr.Zero;
1355 }
1356 //kill the Geometry
1357 _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1358
1359 if (Body != IntPtr.Zero)
1360 {
1361 //kill the body
1362 d.BodyDestroy(Body);
1363 Body = IntPtr.Zero;
1364 }
1365
1366 if(Shell != IntPtr.Zero)
1367 {
1368 try
1369 {
1370 d.GeomDestroy(Shell);
1371 }
1372 catch (System.AccessViolationException)
1373 {
1374 m_log.Error("[PHYSICS]: PrimGeom dead");
1375 }
1376 // Remove any old entries
1377 //string tShell;
1378 //_parent_scene.geom_name_map.TryGetValue(Shell, out tShell);
1379 //Console.WriteLine("**** Remove {0}", tShell);
1380
1381 if(_parent_scene.geom_name_map.ContainsKey(Shell)) _parent_scene.geom_name_map.Remove(Shell);
1382 if(_parent_scene.actor_name_map.ContainsKey(Shell)) _parent_scene.actor_name_map.Remove(Shell);
1383 Shell = IntPtr.Zero;
1384 }
1385 }
1386
1387 m_isPhysical = m_tainted_isPhysical;
1388 }
1389
1390 if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
1391 {
1392 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1393 {
1394
1395 m_pidControllerActive = true;
1396 // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
1397 d.JointDestroy(Amotor);
1398 float prevCapsule = CAPSULE_LENGTH;
1399 CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
1400 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
1401 d.BodyDestroy(Body);
1402 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1403 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
1404 Velocity = Vector3.Zero;
1405 }
1406 else
1407 {
1408 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1409 + (Shell==IntPtr.Zero ? "Shell ":"")
1410 + (Body==IntPtr.Zero ? "Body ":"")
1411 + (Amotor==IntPtr.Zero ? "Amotor ":""));
1412 }
1413 }
1414
1415 if (!m_taintPosition.ApproxEquals(_position, 0.05f))
1416 {
1417 if (Body != IntPtr.Zero)
1418 {
1419 d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
1420
1421 _position.X = m_taintPosition.X;
1422 _position.Y = m_taintPosition.Y;
1423 _position.Z = m_taintPosition.Z;
1424 }
1425 }
1426
1427 }
1428 }
1429
1430 internal void AddCollisionFrameTime(int p)
1431 {
1432 // protect it from overflow crashing
1433 if (m_eventsubscription + p >= int.MaxValue)
1434 m_eventsubscription = 0;
1435 m_eventsubscription += p;
1436 }
1437 }
1438}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..3e2b71c
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,4082 @@
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 byte m_shapetype;
183 private byte m_taintshapetype;
184
185 public bool _zeroFlag; // if body has been stopped
186 private bool m_lastUpdateSent;
187
188 public IntPtr Body = IntPtr.Zero;
189 public String m_primName;
190 private Vector3 _target_velocity;
191 public d.Mass pMass;
192
193 public int m_eventsubscription;
194 private CollisionEventUpdate CollisionEventsThisFrame;
195
196 private IntPtr m_linkJoint = IntPtr.Zero;
197
198 public volatile bool childPrim;
199
200 internal int m_material = (int)Material.Wood;
201
202 private IntPtr m_body = IntPtr.Zero;
203
204 // Vehicle properties ============================================================================================
205 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
206 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
207 private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings:
208 // HOVER_TERRAIN_ONLY
209 // HOVER_GLOBAL_HEIGHT
210 // NO_DEFLECTION_UP
211 // HOVER_WATER_ONLY
212 // HOVER_UP_ONLY
213 // LIMIT_MOTOR_UP
214 // LIMIT_ROLL_ONLY
215
216 // Linear properties
217 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
218 //requested by LSL
219 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
220 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
221 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
222
223 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
224 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
225 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
226
227 //Angular properties
228 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
229
230 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
231 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
232 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
233
234 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
235 // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
236 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
237
238 //Deflection properties
239 // private float m_angularDeflectionEfficiency = 0;
240 // private float m_angularDeflectionTimescale = 0;
241 // private float m_linearDeflectionEfficiency = 0;
242 // private float m_linearDeflectionTimescale = 0;
243
244 //Banking properties
245 // private float m_bankingEfficiency = 0;
246 // private float m_bankingMix = 0;
247 // private float m_bankingTimescale = 0;
248
249 //Hover and Buoyancy properties
250 private float m_VhoverHeight = 0f;
251 // private float m_VhoverEfficiency = 0f;
252 private float m_VhoverTimescale = 0f;
253 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
254 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
255 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
256 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
257 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
258
259 //Attractor properties
260 private float m_verticalAttractionEfficiency = 1.0f; // damped
261 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
262
263// SerialControl m_taintserial = null;
264 object m_taintvehicledata = null;
265
266 public void DoSetVehicle()
267 {
268 VehicleData vd = (VehicleData)m_taintvehicledata;
269
270 m_type = vd.m_type;
271 m_flags = vd.m_flags;
272
273 // Linear properties
274 m_linearMotorDirection = vd.m_linearMotorDirection;
275 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
276 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
277 m_linearMotorTimescale = vd.m_linearMotorTimescale;
278// m_linearMotorOffset = vd.m_linearMotorOffset;
279
280 //Angular properties
281 m_angularMotorDirection = vd.m_angularMotorDirection;
282 m_angularMotorTimescale = vd.m_angularMotorTimescale;
283 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
284 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
285
286 //Deflection properties
287// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
288// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
289// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
290// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
291
292 //Banking properties
293// m_bankingEfficiency = vd.m_bankingEfficiency;
294// m_bankingMix = vd.m_bankingMix;
295// m_bankingTimescale = vd.m_bankingTimescale;
296
297 //Hover and Buoyancy properties
298 m_VhoverHeight = vd.m_VhoverHeight;
299// m_VhoverEfficiency = vd.m_VhoverEfficiency;
300 m_VhoverTimescale = vd.m_VhoverTimescale;
301 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
302
303 //Attractor properties
304 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
305 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
306
307 // Axis
308// m_referenceFrame = vd.m_referenceFrame;
309
310
311 m_taintvehicledata = null;
312 }
313
314 public override void SetVehicle(object vdata)
315 {
316 m_taintvehicledata = vdata;
317 _parent_scene.AddPhysicsActorTaint(this);
318 }
319
320 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
321 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical,
322 bool pisPhantom,byte shapetype, CollisionLocker dode, uint localid)
323 {
324 m_localID = localid;
325 ode = dode;
326 if (!pos.IsFinite())
327 {
328 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
329 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
330 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
331 }
332
333 _position = pos;
334 m_taintposition = pos;
335 PID_D = parent_scene.bodyPIDD;
336 PID_G = parent_scene.bodyPIDG;
337 m_density = parent_scene.geomDefaultDensity;
338 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
339 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
340
341 prim_geom = IntPtr.Zero;
342 // prev_geom = IntPtr.Zero;
343
344 if (!pos.IsFinite())
345 {
346 size = new Vector3(0.5f, 0.5f, 0.5f);
347 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
348 }
349
350 if (size.X <= 0) size.X = 0.01f;
351 if (size.Y <= 0) size.Y = 0.01f;
352 if (size.Z <= 0) size.Z = 0.01f;
353
354 _size = size;
355 m_taintsize = _size;
356
357 if (!QuaternionIsFinite(rotation))
358 {
359 rotation = Quaternion.Identity;
360 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
361 }
362
363 _orientation = rotation;
364 m_taintrot = _orientation;
365 _mesh = mesh;
366 _pbs = pbs;
367 m_shapetype = shapetype;
368 m_taintshapetype = shapetype;
369
370 _parent_scene = parent_scene;
371 m_targetSpace = (IntPtr)0;
372
373 // if (pos.Z < 0)
374 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
375 m_isphysical = false;
376 else
377 {
378 m_isphysical = pisPhysical;
379 // If we're physical, we need to be in the master space for now.
380 // linksets *should* be in a space together.. but are not currently
381 if (m_isphysical)
382 m_targetSpace = _parent_scene.space;
383 }
384
385 m_isphantom = pisPhantom;
386 m_taintphantom = pisPhantom;
387
388 _triMeshData = IntPtr.Zero;
389 m_NoColide = false;
390
391// m_taintserial = null;
392 m_primName = primName;
393 m_taintadd = true;
394 _parent_scene.AddPhysicsActorTaint(this);
395 // don't do .add() here; old geoms get recycled with the same hash
396 }
397
398 public override int PhysicsActorType
399 {
400 get { return (int)ActorTypes.Prim; }
401 set { return; }
402 }
403
404 public override bool SetAlwaysRun
405 {
406 get { return false; }
407 set { return; }
408 }
409
410 public override uint LocalID
411 {
412 set
413 {
414 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
415 m_localID = value;
416 }
417 }
418
419 public override bool Grabbed
420 {
421 set { return; }
422 }
423
424 public override bool Selected
425 {
426 set
427 {
428 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
429 // This only makes the object not collidable if the object
430 // is physical or the object is modified somehow *IN THE FUTURE*
431 // without this, if an avatar selects prim, they can walk right
432 // through it while it's selected
433 m_collisionscore = 0;
434 if ((m_isphysical && !_zeroFlag) || !value)
435 {
436 m_taintselected = value;
437 _parent_scene.AddPhysicsActorTaint(this);
438 }
439 else
440 {
441 m_taintselected = value;
442 m_isSelected = value;
443 }
444 if (m_isSelected) disableBodySoft();
445 }
446 }
447
448 public override bool IsPhysical
449 {
450 get { return m_isphysical; }
451 set
452 {
453 m_isphysical = value;
454 if (!m_isphysical)
455 { // Zero the remembered last velocity
456 m_lastVelocity = Vector3.Zero;
457 if (m_type != Vehicle.TYPE_NONE) Halt();
458 }
459 }
460 }
461
462 public override bool Phantom
463 {
464 get { return m_isphantom; }
465 set
466 {
467 m_isphantom = value;
468 }
469 }
470
471 public void setPrimForRemoval()
472 {
473 m_taintremove = true;
474 }
475
476 public override bool Flying
477 {
478 // no flying prims for you
479 get { return false; }
480 set { }
481 }
482
483 public override bool IsColliding
484 {
485 get { return iscolliding; }
486 set { iscolliding = value; }
487 }
488
489 public override bool CollidingGround
490 {
491 get { return false; }
492 set { return; }
493 }
494
495 public override bool CollidingObj
496 {
497 get { return false; }
498 set { return; }
499 }
500
501 public override bool ThrottleUpdates
502 {
503 get { return m_throttleUpdates; }
504 set { m_throttleUpdates = value; }
505 }
506
507 public override bool Stopped
508 {
509 get { return _zeroFlag; }
510 }
511
512 public override Vector3 Position
513 {
514 get { return _position; }
515
516 set
517 {
518 _position = value;
519 //m_log.Info("[PHYSICS]: " + _position.ToString());
520 }
521 }
522
523 public override Vector3 Size
524 {
525 get { return _size; }
526 set
527 {
528 if (value.IsFinite())
529 {
530 _size = value;
531 }
532 else
533 {
534 m_log.Warn("[PHYSICS]: Got NaN Size on object");
535 }
536 }
537 }
538
539 public override float Mass
540 {
541 get { return CalculateMass(); }
542 }
543
544 public override Vector3 Force
545 {
546 //get { return Vector3.Zero; }
547 get { return m_force; }
548 set
549 {
550 if (value.IsFinite())
551 {
552 m_force = value;
553 }
554 else
555 {
556 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
557 }
558 }
559 }
560
561 public override int VehicleType
562 {
563 get { return (int)m_type; }
564 set { ProcessTypeChange((Vehicle)value); }
565 }
566
567 public override void VehicleFloatParam(int param, float value)
568 {
569 ProcessFloatVehicleParam((Vehicle)param, value);
570 }
571
572 public override void VehicleVectorParam(int param, Vector3 value)
573 {
574 ProcessVectorVehicleParam((Vehicle)param, value);
575 }
576
577 public override void VehicleRotationParam(int param, Quaternion rotation)
578 {
579 ProcessRotationVehicleParam((Vehicle)param, rotation);
580 }
581
582 public override void VehicleFlags(int param, bool remove)
583 {
584 ProcessVehicleFlags(param, remove);
585 }
586
587 public override void SetVolumeDetect(int param)
588 {
589 lock (_parent_scene.OdeLock)
590 {
591 m_isVolumeDetect = (param != 0);
592 }
593 }
594
595 public override Vector3 CenterOfMass
596 {
597 get { return Vector3.Zero; }
598 }
599
600 public override Vector3 GeometricCenter
601 {
602 get { return Vector3.Zero; }
603 }
604
605 public override PrimitiveBaseShape Shape
606 {
607 set
608 {
609 _pbs = value;
610 m_taintshape = true;
611 }
612 }
613
614 public override byte PhysicsShapeType
615 {
616 get
617 {
618 return m_shapetype;
619 }
620 set
621 {
622 m_taintshapetype = value;
623 _parent_scene.AddPhysicsActorTaint(this);
624 }
625 }
626
627 public override Vector3 Velocity
628 {
629 get
630 {
631 // Averate previous velocity with the new one so
632 // client object interpolation works a 'little' better
633 if (_zeroFlag)
634 return Vector3.Zero;
635
636 Vector3 returnVelocity = Vector3.Zero;
637 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
638 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
639 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
640 return returnVelocity;
641 }
642 set
643 {
644 if (value.IsFinite())
645 {
646 _velocity = value;
647 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
648 _acceleration = Vector3.Zero;
649
650 m_taintVelocity = value;
651 _parent_scene.AddPhysicsActorTaint(this);
652 }
653 else
654 {
655 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
656 }
657
658 }
659 }
660
661 public override Vector3 Torque
662 {
663 get
664 {
665 if (!m_isphysical || Body == IntPtr.Zero)
666 return Vector3.Zero;
667
668 return _torque;
669 }
670
671 set
672 {
673 if (value.IsFinite())
674 {
675 m_taintTorque = value;
676 _parent_scene.AddPhysicsActorTaint(this);
677 }
678 else
679 {
680 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
681 }
682 }
683 }
684
685 public override float CollisionScore
686 {
687 get { return m_collisionscore; }
688 set { m_collisionscore = value; }
689 }
690
691 public override bool Kinematic
692 {
693 get { return false; }
694 set { }
695 }
696
697 public override Quaternion Orientation
698 {
699 get { return _orientation; }
700 set
701 {
702 if (QuaternionIsFinite(value))
703 {
704 _orientation = value;
705 }
706 else
707 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
708
709 }
710 }
711
712 public override bool FloatOnWater
713 {
714 set
715 {
716 m_taintCollidesWater = value;
717 _parent_scene.AddPhysicsActorTaint(this);
718 }
719 }
720
721 public override void SetMomentum(Vector3 momentum)
722 {
723 }
724
725 public override Vector3 PIDTarget
726 {
727 set
728 {
729 if (value.IsFinite())
730 {
731 m_PIDTarget = value;
732 }
733 else
734 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
735 }
736 }
737 public override bool PIDActive { set { m_usePID = value; } }
738 public override float PIDTau { set { m_PIDTau = value; } }
739
740 // For RotLookAt
741 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
742 public override bool APIDActive { set { m_useAPID = value; } }
743 public override float APIDStrength { set { m_APIDStrength = value; } }
744 public override float APIDDamping { set { m_APIDDamping = value; } }
745
746 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
747 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
748 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
749 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
750
751 internal static bool QuaternionIsFinite(Quaternion q)
752 {
753 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
754 return false;
755 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
756 return false;
757 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
758 return false;
759 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
760 return false;
761 return true;
762 }
763
764 public override Vector3 Acceleration // client updates read data via here
765 {
766 get
767 {
768 if (_zeroFlag)
769 {
770 return Vector3.Zero;
771 }
772 return _acceleration;
773 }
774 set { _acceleration = value; }
775 }
776
777
778 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
779 {
780 _acceleration = accel;
781 }
782
783 public override void AddForce(Vector3 force, bool pushforce)
784 {
785 if (force.IsFinite())
786 {
787 lock (m_forcelist)
788 m_forcelist.Add(force);
789
790 m_taintforce = true;
791 }
792 else
793 {
794 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
795 }
796 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
797 }
798
799 public override void AddAngularForce(Vector3 force, bool pushforce)
800 {
801 if (force.IsFinite())
802 {
803 m_angularforcelist.Add(force);
804 m_taintaddangularforce = true;
805 }
806 else
807 {
808 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
809 }
810 }
811
812 public override Vector3 RotationalVelocity
813 {
814 get
815 {
816 return m_rotationalVelocity;
817 }
818 set
819 {
820 if (value.IsFinite())
821 {
822 m_rotationalVelocity = value;
823 }
824 else
825 {
826 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
827 }
828 }
829 }
830
831 public override void CrossingFailure()
832 {
833 if (m_outofBounds)
834 {
835 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
836 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
837 _position.Z = Util.Clip(_position.Z, -100f, 50000f);
838 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
839
840 m_lastposition = _position;
841
842 _velocity = Vector3.Zero;
843 m_lastVelocity = _velocity;
844
845
846 if (m_type != Vehicle.TYPE_NONE)
847 Halt();
848
849 d.BodySetLinearVel(Body, 0, 0, 0);
850 base.RequestPhysicsterseUpdate();
851 m_outofBounds = false;
852 }
853 /*
854 int tmp = Interlocked.Increment(ref m_crossingfailures);
855 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
856 {
857 base.RaiseOutOfBounds(_position);
858 return;
859 }
860 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
861 {
862 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
863 }
864 */
865 }
866
867 public override float Buoyancy
868 {
869 get { return m_buoyancy; }
870 set { m_buoyancy = value; }
871 }
872
873 public override void link(PhysicsActor obj)
874 {
875 m_taintparent = obj;
876 }
877
878 public override void delink()
879 {
880 m_taintparent = null;
881 }
882
883 public override void LockAngularMotion(Vector3 axis)
884 {
885 // This is actually ROTATION ENABLE, not a lock.
886 // default is <1,1,1> which is all enabled.
887 // The lock value is updated inside Move(), no point in using the taint system.
888 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
889 if (axis.IsFinite())
890 {
891 axis.X = (axis.X > 0) ? 1f : 0f;
892 axis.Y = (axis.Y > 0) ? 1f : 0f;
893 axis.Z = (axis.Z > 0) ? 1f : 0f;
894 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
895 m_rotateEnableRequest = axis;
896 m_rotateEnableUpdate = true;
897 }
898 else
899 {
900 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
901 }
902 }
903
904 public void SetGeom(IntPtr geom)
905 {
906 if (prim_geom != IntPtr.Zero)
907 {
908 // Remove any old entries
909 //string tPA;
910 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
911 //Console.WriteLine("**** Remove {0}", tPA);
912 if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
913 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
914 d.GeomDestroy(prim_geom);
915 }
916
917 prim_geom = geom;
918 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
919 if (prim_geom != IntPtr.Zero)
920 {
921 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
922 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
923 //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
924 if (m_NoColide)
925 {
926 d.GeomSetCategoryBits(prim_geom, 0);
927 if (m_isphysical && !m_isVolumeDetect)
928 {
929 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
930 }
931 else
932 {
933 d.GeomSetCollideBits(prim_geom, 0);
934 d.GeomDisable(prim_geom);
935 }
936 }
937 else
938 {
939 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
940 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
941 }
942 }
943
944 if (childPrim)
945 {
946 if (_parent != null && _parent is OdePrim)
947 {
948 OdePrim parent = (OdePrim)_parent;
949 //Console.WriteLine("SetGeom calls ChildSetGeom");
950 parent.ChildSetGeom(this);
951 }
952 }
953 //m_log.Warn("Setting Geom to: " + prim_geom);
954 }
955
956 public void enableBodySoft()
957 {
958 if (!childPrim)
959 {
960 if (m_isphysical && Body != IntPtr.Zero)
961 {
962 d.BodyEnable(Body);
963 if (m_type != Vehicle.TYPE_NONE)
964 Enable(Body, _parent_scene);
965 }
966
967 m_disabled = false;
968 }
969 }
970
971 public void disableBodySoft()
972 {
973 m_disabled = true;
974
975 if (m_isphysical && Body != IntPtr.Zero)
976 {
977 d.BodyDisable(Body);
978 Halt();
979 }
980 }
981
982 public void enableBody()
983 {
984 // Don't enable this body if we're a child prim
985 // this should be taken care of in the parent function not here
986 if (!childPrim)
987 {
988 // Sets the geom to a body
989 Body = d.BodyCreate(_parent_scene.world);
990
991 setMass();
992 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
993 d.Quaternion myrot = new d.Quaternion();
994 myrot.X = _orientation.X;
995 myrot.Y = _orientation.Y;
996 myrot.Z = _orientation.Z;
997 myrot.W = _orientation.W;
998 d.BodySetQuaternion(Body, ref myrot);
999 d.GeomSetBody(prim_geom, Body);
1000
1001 m_collisionCategories |= CollisionCategories.Body;
1002 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1003
1004 if (m_NoColide)
1005 {
1006 d.GeomSetCategoryBits(prim_geom, 0);
1007 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1008 }
1009 else
1010 {
1011 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1012 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1013 }
1014
1015 d.BodySetAutoDisableFlag(Body, true);
1016 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1017
1018 // disconnect from world gravity so we can apply buoyancy
1019 d.BodySetGravityMode(Body, false);
1020
1021 m_interpenetrationcount = 0;
1022 m_collisionscore = 0;
1023 m_disabled = false;
1024
1025 if (m_type != Vehicle.TYPE_NONE)
1026 {
1027 Enable(Body, _parent_scene);
1028 }
1029
1030 _parent_scene.addActivePrim(this);
1031 }
1032 }
1033
1034 #region Mass Calculation
1035
1036 private float CalculateMass()
1037 {
1038 float volume = _size.X * _size.Y * _size.Z; // default
1039 float tmp;
1040
1041 float returnMass = 0;
1042 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1043 float hollowVolume = hollowAmount * hollowAmount;
1044
1045 switch (_pbs.ProfileShape)
1046 {
1047 case ProfileShape.Square:
1048 // default box
1049
1050 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1051 {
1052 if (hollowAmount > 0.0)
1053 {
1054 switch (_pbs.HollowShape)
1055 {
1056 case HollowShape.Square:
1057 case HollowShape.Same:
1058 break;
1059
1060 case HollowShape.Circle:
1061
1062 hollowVolume *= 0.78539816339f;
1063 break;
1064
1065 case HollowShape.Triangle:
1066
1067 hollowVolume *= (0.5f * .5f);
1068 break;
1069
1070 default:
1071 hollowVolume = 0;
1072 break;
1073 }
1074 volume *= (1.0f - hollowVolume);
1075 }
1076 }
1077
1078 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1079 {
1080 //a tube
1081
1082 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1083 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1084 volume -= volume * tmp * tmp;
1085
1086 if (hollowAmount > 0.0)
1087 {
1088 hollowVolume *= hollowAmount;
1089
1090 switch (_pbs.HollowShape)
1091 {
1092 case HollowShape.Square:
1093 case HollowShape.Same:
1094 break;
1095
1096 case HollowShape.Circle:
1097 hollowVolume *= 0.78539816339f; ;
1098 break;
1099
1100 case HollowShape.Triangle:
1101 hollowVolume *= 0.5f * 0.5f;
1102 break;
1103 default:
1104 hollowVolume = 0;
1105 break;
1106 }
1107 volume *= (1.0f - hollowVolume);
1108 }
1109 }
1110
1111 break;
1112
1113 case ProfileShape.Circle:
1114
1115 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1116 {
1117 volume *= 0.78539816339f; // elipse base
1118
1119 if (hollowAmount > 0.0)
1120 {
1121 switch (_pbs.HollowShape)
1122 {
1123 case HollowShape.Same:
1124 case HollowShape.Circle:
1125 break;
1126
1127 case HollowShape.Square:
1128 hollowVolume *= 0.5f * 2.5984480504799f;
1129 break;
1130
1131 case HollowShape.Triangle:
1132 hollowVolume *= .5f * 1.27323954473516f;
1133 break;
1134
1135 default:
1136 hollowVolume = 0;
1137 break;
1138 }
1139 volume *= (1.0f - hollowVolume);
1140 }
1141 }
1142
1143 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1144 {
1145 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1146 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1147 volume *= (1.0f - tmp * tmp);
1148
1149 if (hollowAmount > 0.0)
1150 {
1151
1152 // calculate the hollow volume by it's shape compared to the prim shape
1153 hollowVolume *= hollowAmount;
1154
1155 switch (_pbs.HollowShape)
1156 {
1157 case HollowShape.Same:
1158 case HollowShape.Circle:
1159 break;
1160
1161 case HollowShape.Square:
1162 hollowVolume *= 0.5f * 2.5984480504799f;
1163 break;
1164
1165 case HollowShape.Triangle:
1166 hollowVolume *= .5f * 1.27323954473516f;
1167 break;
1168
1169 default:
1170 hollowVolume = 0;
1171 break;
1172 }
1173 volume *= (1.0f - hollowVolume);
1174 }
1175 }
1176 break;
1177
1178 case ProfileShape.HalfCircle:
1179 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1180 {
1181 volume *= 0.52359877559829887307710723054658f;
1182 }
1183 break;
1184
1185 case ProfileShape.EquilateralTriangle:
1186
1187 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1188 {
1189 volume *= 0.32475953f;
1190
1191 if (hollowAmount > 0.0)
1192 {
1193
1194 // calculate the hollow volume by it's shape compared to the prim shape
1195 switch (_pbs.HollowShape)
1196 {
1197 case HollowShape.Same:
1198 case HollowShape.Triangle:
1199 hollowVolume *= .25f;
1200 break;
1201
1202 case HollowShape.Square:
1203 hollowVolume *= 0.499849f * 3.07920140172638f;
1204 break;
1205
1206 case HollowShape.Circle:
1207 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1208 // Cyllinder hollow volume calculation
1209
1210 hollowVolume *= 0.1963495f * 3.07920140172638f;
1211 break;
1212
1213 default:
1214 hollowVolume = 0;
1215 break;
1216 }
1217 volume *= (1.0f - hollowVolume);
1218 }
1219 }
1220 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1221 {
1222 volume *= 0.32475953f;
1223 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1224 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1225 volume *= (1.0f - tmp * tmp);
1226
1227 if (hollowAmount > 0.0)
1228 {
1229
1230 hollowVolume *= hollowAmount;
1231
1232 switch (_pbs.HollowShape)
1233 {
1234 case HollowShape.Same:
1235 case HollowShape.Triangle:
1236 hollowVolume *= .25f;
1237 break;
1238
1239 case HollowShape.Square:
1240 hollowVolume *= 0.499849f * 3.07920140172638f;
1241 break;
1242
1243 case HollowShape.Circle:
1244
1245 hollowVolume *= 0.1963495f * 3.07920140172638f;
1246 break;
1247
1248 default:
1249 hollowVolume = 0;
1250 break;
1251 }
1252 volume *= (1.0f - hollowVolume);
1253 }
1254 }
1255 break;
1256
1257 default:
1258 break;
1259 }
1260
1261
1262
1263 float taperX1;
1264 float taperY1;
1265 float taperX;
1266 float taperY;
1267 float pathBegin;
1268 float pathEnd;
1269 float profileBegin;
1270 float profileEnd;
1271
1272 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1273 {
1274 taperX1 = _pbs.PathScaleX * 0.01f;
1275 if (taperX1 > 1.0f)
1276 taperX1 = 2.0f - taperX1;
1277 taperX = 1.0f - taperX1;
1278
1279 taperY1 = _pbs.PathScaleY * 0.01f;
1280 if (taperY1 > 1.0f)
1281 taperY1 = 2.0f - taperY1;
1282 taperY = 1.0f - taperY1;
1283 }
1284 else
1285 {
1286 taperX = _pbs.PathTaperX * 0.01f;
1287 if (taperX < 0.0f)
1288 taperX = -taperX;
1289 taperX1 = 1.0f - taperX;
1290
1291 taperY = _pbs.PathTaperY * 0.01f;
1292 if (taperY < 0.0f)
1293 taperY = -taperY;
1294 taperY1 = 1.0f - taperY;
1295
1296 }
1297
1298
1299 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1300
1301 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1302 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1303 volume *= (pathEnd - pathBegin);
1304
1305 // this is crude aproximation
1306 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1307 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1308 volume *= (profileEnd - profileBegin);
1309
1310 returnMass = m_density * volume;
1311
1312 if (returnMass <= 0)
1313 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1314 // else if (returnMass > _parent_scene.maximumMassObject)
1315 // returnMass = _parent_scene.maximumMassObject;
1316
1317
1318
1319
1320 // Recursively calculate mass
1321 bool HasChildPrim = false;
1322 lock (childrenPrim)
1323 {
1324 if (childrenPrim.Count > 0)
1325 {
1326 HasChildPrim = true;
1327 }
1328
1329 }
1330 if (HasChildPrim)
1331 {
1332 OdePrim[] childPrimArr = new OdePrim[0];
1333
1334 lock (childrenPrim)
1335 childPrimArr = childrenPrim.ToArray();
1336
1337 for (int i = 0; i < childPrimArr.Length; i++)
1338 {
1339 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1340 returnMass += childPrimArr[i].CalculateMass();
1341 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1342 if (i > 256)
1343 break;
1344 }
1345 }
1346 if (returnMass > _parent_scene.maximumMassObject)
1347 returnMass = _parent_scene.maximumMassObject;
1348 return returnMass;
1349 }// end CalculateMass
1350
1351 #endregion
1352
1353 public void setMass()
1354 {
1355 if (Body != (IntPtr)0)
1356 {
1357 float newmass = CalculateMass();
1358
1359 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1360
1361 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1362 d.BodySetMass(Body, ref pMass);
1363 }
1364 }
1365
1366 public void disableBody()
1367 {
1368 //this kills the body so things like 'mesh' can re-create it.
1369 lock (this)
1370 {
1371 if (!childPrim)
1372 {
1373 if (Body != IntPtr.Zero)
1374 {
1375 _parent_scene.remActivePrim(this);
1376 m_collisionCategories &= ~CollisionCategories.Body;
1377 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1378
1379 if (prim_geom != IntPtr.Zero)
1380 {
1381 if (m_NoColide)
1382 {
1383 d.GeomSetCategoryBits(prim_geom, 0);
1384 d.GeomSetCollideBits(prim_geom, 0);
1385 d.GeomDisable(prim_geom);
1386 }
1387 else
1388 {
1389 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1390 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1391 }
1392 }
1393
1394 d.BodyDestroy(Body);
1395 lock (childrenPrim)
1396 {
1397 if (childrenPrim.Count > 0)
1398 {
1399 foreach (OdePrim prm in childrenPrim)
1400 {
1401 if (prm.m_NoColide && prm.prim_geom != IntPtr.Zero)
1402 {
1403 d.GeomSetCategoryBits(prm.prim_geom, 0);
1404 d.GeomSetCollideBits(prm.prim_geom, 0);
1405 d.GeomDisable(prm.prim_geom);
1406 }
1407
1408 _parent_scene.remActivePrim(prm);
1409 prm.Body = IntPtr.Zero;
1410 }
1411 }
1412 }
1413 Body = IntPtr.Zero;
1414 }
1415 }
1416 else
1417 {
1418 _parent_scene.remActivePrim(this);
1419
1420 m_collisionCategories &= ~CollisionCategories.Body;
1421 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1422
1423 if (prim_geom != IntPtr.Zero)
1424 {
1425 if (m_NoColide)
1426 {
1427 d.GeomSetCategoryBits(prim_geom, 0);
1428 d.GeomSetCollideBits(prim_geom, 0);
1429 d.GeomDisable(prim_geom);
1430 }
1431 else
1432 {
1433 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1434 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1435 }
1436 }
1437
1438 Body = IntPtr.Zero;
1439 }
1440 }
1441 m_disabled = true;
1442 m_collisionscore = 0;
1443 }
1444
1445// private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1446
1447 public bool setMesh(OdeScene parent_scene, IMesh mesh)
1448 {
1449 //Kill Body so that mesh can re-make the geom
1450 if (IsPhysical && Body != IntPtr.Zero)
1451 {
1452 if (childPrim)
1453 {
1454 if (_parent != null)
1455 {
1456 OdePrim parent = (OdePrim)_parent;
1457 parent.ChildDelink(this);
1458 }
1459 }
1460 else
1461 {
1462 disableBody();
1463 }
1464 }
1465
1466 IntPtr vertices, indices;
1467 int vertexCount, indexCount;
1468 int vertexStride, triStride;
1469 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1470 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1471
1472 // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
1473 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1474
1475 if (vertexCount == 0 || indexCount == 0)
1476 {
1477 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());
1478 return false;
1479 }
1480
1481 IntPtr geo = IntPtr.Zero;
1482 try
1483 {
1484 _triMeshData = d.GeomTriMeshDataCreate();
1485 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1486 d.GeomTriMeshDataPreprocess(_triMeshData);
1487
1488 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1489
1490 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1491 }
1492 catch (Exception e)
1493 {
1494 m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);
1495
1496 if (_triMeshData != IntPtr.Zero)
1497 {
1498 d.GeomTriMeshDataDestroy(_triMeshData);
1499 _triMeshData = IntPtr.Zero;
1500 }
1501 return false;
1502 }
1503
1504 SetGeom(geo);
1505
1506 return true;
1507 }
1508
1509 public void ProcessTaints(float timestep) //=============================================================================
1510 {
1511 if (m_taintadd)
1512 {
1513 changeadd(timestep);
1514 }
1515
1516 if (m_taintremove)
1517 return;
1518
1519 if (prim_geom != IntPtr.Zero)
1520 {
1521 if (!_position.ApproxEquals(m_taintposition, 0f))
1522 {
1523 changemove(timestep);
1524 }
1525 if (m_taintrot != _orientation)
1526 {
1527 if (childPrim && IsPhysical) // For physical child prim...
1528 {
1529 rotate(timestep);
1530 // KF: ODE will also rotate the parent prim!
1531 // so rotate the root back to where it was
1532 OdePrim parent = (OdePrim)_parent;
1533 parent.rotate(timestep);
1534 }
1535 else
1536 {
1537 //Just rotate the prim
1538 rotate(timestep);
1539 }
1540 }
1541 //
1542 if (m_taintphantom != m_isphantom )
1543 {
1544 changePhantomStatus();
1545 }//
1546
1547 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1548 {
1549 changePhysicsStatus(timestep);
1550 }//
1551
1552
1553 if (!_size.ApproxEquals(m_taintsize, 0f))
1554 changesize(timestep);
1555 //
1556
1557 if(m_taintshapetype != m_shapetype)
1558 {
1559 m_shapetype = m_taintshapetype;
1560 changeshape(timestep);
1561 }
1562
1563 if (m_taintshape)
1564 changeshape(timestep);
1565 //
1566
1567 if (m_taintforce)
1568 changeAddForce(timestep);
1569
1570 if (m_taintaddangularforce)
1571 changeAddAngularForce(timestep);
1572
1573 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1574 changeSetTorque(timestep);
1575
1576 if (m_taintdisable)
1577 changedisable(timestep);
1578
1579 if (m_taintselected != m_isSelected)
1580 changeSelectedStatus();
1581
1582 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1583 changevelocity(timestep);
1584
1585 if (m_taintparent != _parent)
1586 changelink(timestep);
1587
1588 if (m_taintCollidesWater != m_collidesWater)
1589 changefloatonwater(timestep);
1590
1591 if (m_taintvehicledata != null)
1592 DoSetVehicle();
1593
1594 /* obsolete
1595 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1596 changeAngularLock(timestep);
1597 */
1598 }
1599
1600 else
1601 {
1602 m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom");
1603
1604 // not sure this will not flame...
1605 m_taintremove = true;
1606 _parent_scene.AddPhysicsActorTaint(this);
1607 }
1608
1609 }
1610
1611 private void changelink(float timestep)
1612 {
1613 // If the newly set parent is not null
1614 // create link
1615 if (_parent == null && m_taintparent != null)
1616 {
1617 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1618 {
1619 OdePrim obj = (OdePrim)m_taintparent;
1620 obj.ParentPrim(this);
1621 }
1622 }
1623 // If the newly set parent is null
1624 // destroy link
1625 else if (_parent != null && m_taintparent == null)
1626 {
1627 if (_parent is OdePrim)
1628 {
1629 OdePrim obj = (OdePrim)_parent;
1630 obj.ChildDelink(this);
1631 childPrim = false;
1632 }
1633 }
1634
1635 _parent = m_taintparent;
1636 m_taintPhysics = m_isphysical;
1637 }
1638
1639 // I'm the parent
1640 // prim is the child
1641 public void ParentPrim(OdePrim prim)
1642 {
1643 if (this.m_localID != prim.m_localID)
1644 {
1645 if (Body == IntPtr.Zero)
1646 {
1647 Body = d.BodyCreate(_parent_scene.world);
1648 // disconnect from world gravity so we can apply buoyancy
1649 d.BodySetGravityMode(Body, false);
1650
1651 setMass();
1652 }
1653 if (Body != IntPtr.Zero)
1654 {
1655 lock (childrenPrim)
1656 {
1657 if (!childrenPrim.Contains(prim))
1658 {
1659 childrenPrim.Add(prim);
1660
1661 foreach (OdePrim prm in childrenPrim)
1662 {
1663 d.Mass m2;
1664 d.MassSetZero(out m2);
1665 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1666
1667
1668 d.Quaternion quat = new d.Quaternion();
1669 quat.W = prm._orientation.W;
1670 quat.X = prm._orientation.X;
1671 quat.Y = prm._orientation.Y;
1672 quat.Z = prm._orientation.Z;
1673
1674 d.Matrix3 mat = new d.Matrix3();
1675 d.RfromQ(out mat, ref quat);
1676 d.MassRotate(ref m2, ref mat);
1677 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1678 d.MassAdd(ref pMass, ref m2);
1679 }
1680 foreach (OdePrim prm in childrenPrim)
1681 {
1682 if (m_isphantom && !prm.m_isVolumeDetect)
1683 {
1684 prm.m_collisionCategories = 0;
1685 prm.m_collisionFlags = CollisionCategories.Land;
1686 }
1687 else
1688 {
1689 prm.m_collisionCategories |= CollisionCategories.Body;
1690 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1691 }
1692 if (prm.prim_geom == IntPtr.Zero)
1693 {
1694 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1695 continue;
1696 }
1697
1698 if (prm.m_NoColide)
1699 {
1700 d.GeomSetCategoryBits(prm.prim_geom, 0);
1701 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1702 }
1703 else
1704 {
1705 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1706 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1707 }
1708
1709 d.Quaternion quat = new d.Quaternion();
1710 quat.W = prm._orientation.W;
1711 quat.X = prm._orientation.X;
1712 quat.Y = prm._orientation.Y;
1713 quat.Z = prm._orientation.Z;
1714
1715 d.Matrix3 mat = new d.Matrix3();
1716 d.RfromQ(out mat, ref quat);
1717 if (Body != IntPtr.Zero)
1718 {
1719 d.GeomSetBody(prm.prim_geom, Body);
1720 prm.childPrim = true;
1721 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1722 //d.GeomSetOffsetPosition(prim.prim_geom,
1723 // (Position.X - prm.Position.X) - pMass.c.X,
1724 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1725 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1726 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1727 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1728 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1729 d.BodySetMass(Body, ref pMass);
1730 }
1731 else
1732 {
1733 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1734 }
1735
1736 prm.m_interpenetrationcount = 0;
1737 prm.m_collisionscore = 0;
1738 prm.m_disabled = false;
1739
1740 prm.Body = Body;
1741
1742 _parent_scene.addActivePrim(prm);
1743 }
1744
1745 if (m_isphantom && !m_isVolumeDetect)
1746 {
1747 m_collisionCategories = 0;
1748 m_collisionFlags = CollisionCategories.Land;
1749 }
1750 else
1751 {
1752 m_collisionCategories |= CollisionCategories.Body;
1753 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1754 }
1755
1756 if (m_NoColide)
1757 {
1758 d.GeomSetCategoryBits(prim_geom, 0);
1759 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1760 }
1761 else
1762 {
1763 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1764 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1765 }
1766
1767 d.Quaternion quat2 = new d.Quaternion();
1768 quat2.W = _orientation.W;
1769 quat2.X = _orientation.X;
1770 quat2.Y = _orientation.Y;
1771 quat2.Z = _orientation.Z;
1772
1773 d.Matrix3 mat2 = new d.Matrix3();
1774 d.RfromQ(out mat2, ref quat2);
1775 d.GeomSetBody(prim_geom, Body);
1776 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1777 //d.GeomSetOffsetPosition(prim.prim_geom,
1778 // (Position.X - prm.Position.X) - pMass.c.X,
1779 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1780 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1781 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1782 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1783 d.BodySetMass(Body, ref pMass);
1784
1785 d.BodySetAutoDisableFlag(Body, true);
1786 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1787
1788 m_interpenetrationcount = 0;
1789 m_collisionscore = 0;
1790 m_disabled = false;
1791
1792 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1793 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1794
1795 _parent_scene.addActivePrim(this);
1796 }
1797 }
1798 }
1799 }
1800 }
1801
1802 private void ChildSetGeom(OdePrim odePrim)
1803 {
1804 lock (childrenPrim)
1805 {
1806 foreach (OdePrim prm in childrenPrim)
1807 {
1808 prm.disableBody();
1809 }
1810 }
1811 disableBody();
1812
1813 if (Body != IntPtr.Zero)
1814 {
1815 _parent_scene.remActivePrim(this);
1816 }
1817
1818 lock (childrenPrim)
1819 {
1820 foreach (OdePrim prm in childrenPrim)
1821 {
1822 ParentPrim(prm);
1823 }
1824 }
1825 }
1826
1827 private void ChildDelink(OdePrim odePrim)
1828 {
1829 // Okay, we have a delinked child.. need to rebuild the body.
1830 lock (childrenPrim)
1831 {
1832 foreach (OdePrim prm in childrenPrim)
1833 {
1834 prm.childPrim = true;
1835 prm.disableBody();
1836 }
1837 }
1838 disableBody();
1839
1840 lock (childrenPrim)
1841 {
1842 childrenPrim.Remove(odePrim);
1843 }
1844
1845 if (Body != IntPtr.Zero)
1846 {
1847 _parent_scene.remActivePrim(this);
1848 }
1849
1850 lock (childrenPrim)
1851 {
1852 foreach (OdePrim prm in childrenPrim)
1853 {
1854 ParentPrim(prm);
1855 }
1856 }
1857 }
1858
1859 private void changePhantomStatus()
1860 {
1861 m_taintphantom = m_isphantom;
1862 changeSelectedStatus();
1863 }
1864
1865/* not in use
1866 private void SetCollider()
1867 {
1868 SetCollider(m_isSelected, m_isphysical, m_isphantom, m_isSelected);
1869 }
1870
1871 private void SetCollider(bool sel, bool phys, bool phan, bool vdtc)
1872 {
1873 if (sel)
1874 {
1875 m_collisionCategories = CollisionCategories.Selected;
1876 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1877 }
1878 else
1879 {
1880 if (phan && !vdtc)
1881 {
1882 m_collisionCategories = 0;
1883 if (phys)
1884 m_collisionFlags = CollisionCategories.Land;
1885 else
1886 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
1887 }
1888 else
1889 {
1890 m_collisionCategories = CollisionCategories.Geom;
1891 if (phys)
1892 m_collisionCategories |= CollisionCategories.Body;
1893
1894 m_collisionFlags = m_default_collisionFlags;
1895
1896 if (m_collidesLand)
1897 m_collisionFlags |= CollisionCategories.Land;
1898 if (m_collidesWater)
1899 m_collisionFlags |= CollisionCategories.Water;
1900 }
1901 }
1902
1903 if (prim_geom != IntPtr.Zero)
1904 {
1905 if (m_NoColide)
1906 {
1907 d.GeomSetCategoryBits(prim_geom, 0);
1908 if (phys)
1909 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1910 else
1911 {
1912 d.GeomSetCollideBits(prim_geom, 0);
1913 d.GeomDisable(prim_geom);
1914 }
1915 }
1916 else
1917 {
1918 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1919 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1920 }
1921 }
1922 }
1923*/
1924
1925 private void changeSelectedStatus()
1926 {
1927 if (m_taintselected)
1928 {
1929 m_collisionCategories = CollisionCategories.Selected;
1930 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1931
1932 // We do the body disable soft twice because 'in theory' a collision could have happened
1933 // in between the disabling and the collision properties setting
1934 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1935 // through the ground.
1936
1937 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1938 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1939 // so that causes the selected part to wake up and continue moving.
1940
1941 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1942 // assembly will stop simulating during the selection, because of the lack of atomicity
1943 // of select operations (their processing could be interrupted by a thread switch, causing
1944 // simulation to continue before all of the selected object notifications trickle down to
1945 // the physics engine).
1946
1947 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1948 // selected and disabled. then, due to a thread switch, the selection processing is
1949 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1950 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1951 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1952 // up, start simulating again, which in turn wakes up the last 50.
1953
1954 if (m_isphysical)
1955 {
1956 disableBodySoft();
1957 }
1958
1959 if (prim_geom != IntPtr.Zero)
1960 {
1961 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1962 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1963 if (m_NoColide)
1964 d.GeomDisable(prim_geom);
1965 }
1966
1967 if (m_isphysical)
1968 {
1969 disableBodySoft();
1970 }
1971 if (Body != IntPtr.Zero)
1972 {
1973 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1974 d.BodySetForce(Body, 0f, 0f, 0f);
1975 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1976 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1977 }
1978 }
1979 else
1980 {
1981 if (m_isphantom && !m_isVolumeDetect)
1982 {
1983 m_collisionCategories = 0;
1984 if (m_isphysical)
1985 m_collisionFlags = CollisionCategories.Land;
1986 else
1987 m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics
1988 }
1989 else
1990 {
1991 m_collisionCategories = CollisionCategories.Geom;
1992 if (m_isphysical)
1993 m_collisionCategories |= CollisionCategories.Body;
1994
1995 m_collisionFlags = m_default_collisionFlags;
1996
1997 if (m_collidesLand)
1998 m_collisionFlags |= CollisionCategories.Land;
1999 if (m_collidesWater)
2000 m_collisionFlags |= CollisionCategories.Water;
2001 }
2002
2003 if (prim_geom != IntPtr.Zero)
2004 {
2005 if (m_NoColide)
2006 {
2007 d.GeomSetCategoryBits(prim_geom, 0);
2008 if (m_isphysical)
2009 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2010 else
2011 {
2012 d.GeomSetCollideBits(prim_geom, 0);
2013 d.GeomDisable(prim_geom);
2014 }
2015 }
2016 else
2017 {
2018 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2019 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2020 }
2021 }
2022 if (Body != IntPtr.Zero)
2023 {
2024 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2025 d.BodySetForce(Body, 0f, 0f, 0f);
2026 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
2027 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
2028 }
2029
2030 if (m_isphysical)
2031 {
2032 if (Body != IntPtr.Zero)
2033 {
2034 enableBodySoft();
2035 }
2036 }
2037 }
2038
2039 resetCollisionAccounting();
2040 m_isSelected = m_taintselected;
2041 }//end changeSelectedStatus
2042
2043 public void ResetTaints()
2044 {
2045 m_taintposition = _position;
2046 m_taintrot = _orientation;
2047 m_taintPhysics = m_isphysical;
2048 m_taintselected = m_isSelected;
2049 m_taintsize = _size;
2050 m_taintshape = false;
2051 m_taintforce = false;
2052 m_taintdisable = false;
2053 m_taintVelocity = Vector3.Zero;
2054 }
2055
2056 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
2057 {
2058 bool gottrimesh = false;
2059
2060 m_NoColide = false; // assume all will go well
2061
2062 if (_triMeshData != IntPtr.Zero)
2063 {
2064 d.GeomTriMeshDataDestroy(_triMeshData);
2065 _triMeshData = IntPtr.Zero;
2066 }
2067
2068 if (_mesh != null)
2069 {
2070 gottrimesh = setMesh(_parent_scene, _mesh);
2071 if (!gottrimesh)
2072 {
2073 // getting a mesh failed,
2074 // lets go on having a basic box or sphere, with prim size but not coliding
2075 // physical colides with land, non with nothing
2076
2077 m_NoColide = true;
2078 }
2079 }
2080
2081 if (!gottrimesh)
2082 { // we will have a basic box or sphere
2083 IntPtr geo = IntPtr.Zero;
2084
2085 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
2086 && _size.X == _size.Y && _size.X == _size.Z)
2087 {
2088 // its a sphere
2089 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2090 try
2091 {
2092 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
2093 }
2094 catch (Exception e)
2095 {
2096 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2097 geo = IntPtr.Zero;
2098 ode.dunlock(_parent_scene.world);
2099 }
2100 }
2101 else // make it a box
2102 {
2103 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2104 try
2105 {
2106 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
2107 }
2108 catch (Exception e)
2109 {
2110 m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
2111 geo = IntPtr.Zero;
2112 ode.dunlock(_parent_scene.world);
2113 }
2114 }
2115
2116 if (geo == IntPtr.Zero) // if this happens it must be fixed
2117 {
2118 // if it does lets stop what we can
2119 // not sure this will not flame...
2120
2121 m_taintremove = true;
2122 _parent_scene.AddPhysicsActorTaint(this);
2123 return;
2124 }
2125
2126 SetGeom(geo); // this processes the m_NoColide
2127 }
2128 }
2129
2130 public void changeadd(float timestep)
2131 {
2132 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2133 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
2134
2135 if (targetspace == IntPtr.Zero)
2136 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2137
2138 m_targetSpace = targetspace;
2139
2140 if (_mesh == null) // && m_meshfailed == false)
2141 {
2142 if (_parent_scene.needsMeshing(_pbs))
2143 {
2144 bool convex;
2145 if (m_shapetype == 2)
2146 convex = true;
2147 else
2148 convex = false;
2149 try
2150 {
2151 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,convex);
2152 }
2153 catch
2154 {
2155 //Don't continuously try to mesh prims when meshing has failed
2156 m_meshfailed = true;
2157 _mesh = null;
2158 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2159 }
2160 }
2161 }
2162
2163 lock (_parent_scene.OdeLock)
2164 {
2165 CreateGeom(m_targetSpace, _mesh);
2166
2167 if (prim_geom != IntPtr.Zero)
2168 {
2169 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2170 d.Quaternion myrot = new d.Quaternion();
2171 myrot.X = _orientation.X;
2172 myrot.Y = _orientation.Y;
2173 myrot.Z = _orientation.Z;
2174 myrot.W = _orientation.W;
2175 d.GeomSetQuaternion(prim_geom, ref myrot);
2176 }
2177
2178 if (m_isphysical && Body == IntPtr.Zero)
2179 {
2180 enableBody();
2181 }
2182 }
2183
2184 changeSelectedStatus();
2185
2186 m_taintadd = false;
2187 }
2188
2189 public void changemove(float timestep)
2190 {
2191 if (m_isphysical)
2192 {
2193 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
2194 if (!m_taintremove && !childPrim)
2195 {
2196 if (Body == IntPtr.Zero)
2197 enableBody();
2198 //Prim auto disable after 20 frames,
2199 //if you move it, re-enable the prim manually.
2200 if (_parent != null)
2201 {
2202 if (m_linkJoint != IntPtr.Zero)
2203 {
2204 d.JointDestroy(m_linkJoint);
2205 m_linkJoint = IntPtr.Zero;
2206 }
2207 }
2208 if (Body != IntPtr.Zero)
2209 {
2210 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
2211
2212 if (_parent != null)
2213 {
2214 OdePrim odParent = (OdePrim)_parent;
2215 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
2216 {
2217 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
2218 Console.WriteLine("ODEPrim JointCreateFixed !!!");
2219 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
2220 d.JointAttach(m_linkJoint, Body, odParent.Body);
2221 d.JointSetFixed(m_linkJoint);
2222 }
2223 }
2224 d.BodyEnable(Body);
2225 if (m_type != Vehicle.TYPE_NONE)
2226 {
2227 Enable(Body, _parent_scene);
2228 }
2229 }
2230 else
2231 {
2232 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
2233 }
2234 }
2235 //else
2236 // {
2237 //m_log.Debug("[BUG]: race!");
2238 //}
2239 }
2240 else
2241 {
2242 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2243 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2244 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2245
2246 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
2247 m_targetSpace = tempspace;
2248
2249 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2250 if (prim_geom != IntPtr.Zero)
2251 {
2252 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2253
2254 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2255 d.SpaceAdd(m_targetSpace, prim_geom);
2256 }
2257 }
2258
2259 changeSelectedStatus();
2260
2261 resetCollisionAccounting();
2262 m_taintposition = _position;
2263 }
2264
2265 public void rotate(float timestep)
2266 {
2267 d.Quaternion myrot = new d.Quaternion();
2268 myrot.X = _orientation.X;
2269 myrot.Y = _orientation.Y;
2270 myrot.Z = _orientation.Z;
2271 myrot.W = _orientation.W;
2272 if (Body != IntPtr.Zero)
2273 {
2274 // KF: If this is a root prim do BodySet
2275 d.BodySetQuaternion(Body, ref myrot);
2276 }
2277 else
2278 {
2279 // daughter prim, do Geom set
2280 d.GeomSetQuaternion(prim_geom, ref myrot);
2281 }
2282
2283 resetCollisionAccounting();
2284 m_taintrot = _orientation;
2285 }
2286
2287 private void resetCollisionAccounting()
2288 {
2289 m_collisionscore = 0;
2290 m_interpenetrationcount = 0;
2291 m_disabled = false;
2292 }
2293
2294 public void changedisable(float timestep)
2295 {
2296 m_disabled = true;
2297 if (Body != IntPtr.Zero)
2298 {
2299 d.BodyDisable(Body);
2300 Body = IntPtr.Zero;
2301 }
2302
2303 m_taintdisable = false;
2304 }
2305
2306 public void changePhysicsStatus(float timestep)
2307 {
2308 if (m_isphysical == true)
2309 {
2310 if (Body == IntPtr.Zero)
2311 {
2312 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2313 {
2314 changeshape(2f);
2315 }
2316 else
2317 {
2318 enableBody();
2319 }
2320 }
2321 }
2322 else
2323 {
2324 if (Body != IntPtr.Zero)
2325 {
2326 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2327 {
2328 _mesh = null;
2329 changeadd(2f);
2330 }
2331 if (childPrim)
2332 {
2333 if (_parent != null)
2334 {
2335 OdePrim parent = (OdePrim)_parent;
2336 parent.ChildDelink(this);
2337 }
2338 }
2339 else
2340 {
2341 disableBody();
2342 }
2343 }
2344 }
2345
2346 changeSelectedStatus();
2347
2348 resetCollisionAccounting();
2349 m_taintPhysics = m_isphysical;
2350 }
2351
2352 public void changesize(float timestamp)
2353 {
2354
2355 string oldname = _parent_scene.geom_name_map[prim_geom];
2356
2357 if (_size.X <= 0) _size.X = 0.01f;
2358 if (_size.Y <= 0) _size.Y = 0.01f;
2359 if (_size.Z <= 0) _size.Z = 0.01f;
2360
2361 // Cleanup of old prim geometry
2362 if (_mesh != null)
2363 {
2364 // Cleanup meshing here
2365 }
2366 //kill body to rebuild
2367 if (IsPhysical && Body != IntPtr.Zero)
2368 {
2369 if (childPrim)
2370 {
2371 if (_parent != null)
2372 {
2373 OdePrim parent = (OdePrim)_parent;
2374 parent.ChildDelink(this);
2375 }
2376 }
2377 else
2378 {
2379 disableBody();
2380 }
2381 }
2382 if (d.SpaceQuery(m_targetSpace, prim_geom))
2383 {
2384 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2385 d.SpaceRemove(m_targetSpace, prim_geom);
2386 }
2387 // we don't need to do space calculation because the client sends a position update also.
2388
2389 // Construction of new prim
2390 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2391 {
2392 float meshlod = _parent_scene.meshSculptLOD;
2393
2394 if (IsPhysical)
2395 meshlod = _parent_scene.MeshSculptphysicalLOD;
2396 // Don't need to re-enable body.. it's done in SetMesh
2397
2398 IMesh mesh = null;
2399
2400 try
2401 {
2402 if (_parent_scene.needsMeshing(_pbs))
2403 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
2404 }
2405 catch
2406 {
2407 m_meshfailed = true;
2408 mesh = null;
2409 m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2410 }
2411
2412 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2413 CreateGeom(m_targetSpace, mesh);
2414 }
2415 else
2416 {
2417 _mesh = null;
2418 CreateGeom(m_targetSpace, _mesh);
2419 }
2420
2421 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2422 d.Quaternion myrot = new d.Quaternion();
2423 myrot.X = _orientation.X;
2424 myrot.Y = _orientation.Y;
2425 myrot.Z = _orientation.Z;
2426 myrot.W = _orientation.W;
2427 d.GeomSetQuaternion(prim_geom, ref myrot);
2428
2429 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2430 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2431 {
2432 // Re creates body on size.
2433 // EnableBody also does setMass()
2434 enableBody();
2435 d.BodyEnable(Body);
2436 }
2437
2438 _parent_scene.geom_name_map[prim_geom] = oldname;
2439
2440 changeSelectedStatus();
2441 if (childPrim)
2442 {
2443 if (_parent is OdePrim)
2444 {
2445 OdePrim parent = (OdePrim)_parent;
2446 parent.ChildSetGeom(this);
2447 }
2448 }
2449 resetCollisionAccounting();
2450 m_taintsize = _size;
2451 }
2452
2453
2454
2455 public void changefloatonwater(float timestep)
2456 {
2457 m_collidesWater = m_taintCollidesWater;
2458
2459 if (prim_geom != IntPtr.Zero)
2460 {
2461 if (m_collidesWater)
2462 {
2463 m_collisionFlags |= CollisionCategories.Water;
2464 }
2465 else
2466 {
2467 m_collisionFlags &= ~CollisionCategories.Water;
2468 }
2469 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2470 }
2471 }
2472
2473 public void changeshape(float timestamp)
2474 {
2475 string oldname = _parent_scene.geom_name_map[prim_geom];
2476
2477 // Cleanup of old prim geometry and Bodies
2478 if (IsPhysical && Body != IntPtr.Zero)
2479 {
2480 if (childPrim)
2481 {
2482 if (_parent != null)
2483 {
2484 OdePrim parent = (OdePrim)_parent;
2485 parent.ChildDelink(this);
2486 }
2487 }
2488 else
2489 {
2490 disableBody();
2491 }
2492 }
2493
2494
2495 // we don't need to do space calculation because the client sends a position update also.
2496 if (_size.X <= 0) _size.X = 0.01f;
2497 if (_size.Y <= 0) _size.Y = 0.01f;
2498 if (_size.Z <= 0) _size.Z = 0.01f;
2499 // Construction of new prim
2500
2501 if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false)
2502 {
2503 // Don't need to re-enable body.. it's done in SetMesh
2504 float meshlod = _parent_scene.meshSculptLOD;
2505 IMesh mesh;
2506
2507 if (IsPhysical)
2508 meshlod = _parent_scene.MeshSculptphysicalLOD;
2509
2510 bool convex;
2511 if (m_shapetype == 2)
2512 convex = true;
2513 else
2514 convex = false;
2515
2516 try
2517 {
2518 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, convex);
2519 }
2520 catch
2521 {
2522 mesh = null;
2523 m_meshfailed = true;
2524 m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
2525 }
2526
2527 CreateGeom(m_targetSpace, mesh);
2528
2529 // createmesh returns null when it doesn't mesh.
2530 }
2531 else
2532 {
2533 _mesh = null;
2534 CreateGeom(m_targetSpace, null);
2535 }
2536
2537 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2538 d.Quaternion myrot = new d.Quaternion();
2539 //myrot.W = _orientation.w;
2540 myrot.W = _orientation.W;
2541 myrot.X = _orientation.X;
2542 myrot.Y = _orientation.Y;
2543 myrot.Z = _orientation.Z;
2544 d.GeomSetQuaternion(prim_geom, ref myrot);
2545
2546 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2547 if (IsPhysical && Body == IntPtr.Zero)
2548 {
2549 // Re creates body on size.
2550 // EnableBody also does setMass()
2551 enableBody();
2552 if (Body != IntPtr.Zero)
2553 {
2554 d.BodyEnable(Body);
2555 }
2556 }
2557 _parent_scene.geom_name_map[prim_geom] = oldname;
2558
2559 changeSelectedStatus();
2560 if (childPrim)
2561 {
2562 if (_parent is OdePrim)
2563 {
2564 OdePrim parent = (OdePrim)_parent;
2565 parent.ChildSetGeom(this);
2566 }
2567 }
2568 resetCollisionAccounting();
2569 m_taintshape = false;
2570 }
2571
2572 public void changeAddForce(float timestamp)
2573 {
2574 if (!m_isSelected)
2575 {
2576 lock (m_forcelist)
2577 {
2578 //m_log.Info("[PHYSICS]: dequeing forcelist");
2579 if (IsPhysical)
2580 {
2581 Vector3 iforce = Vector3.Zero;
2582 int i = 0;
2583 try
2584 {
2585 for (i = 0; i < m_forcelist.Count; i++)
2586 {
2587
2588 iforce = iforce + (m_forcelist[i] * 100);
2589 }
2590 }
2591 catch (IndexOutOfRangeException)
2592 {
2593 m_forcelist = new List<Vector3>();
2594 m_collisionscore = 0;
2595 m_interpenetrationcount = 0;
2596 m_taintforce = false;
2597 return;
2598 }
2599 catch (ArgumentOutOfRangeException)
2600 {
2601 m_forcelist = new List<Vector3>();
2602 m_collisionscore = 0;
2603 m_interpenetrationcount = 0;
2604 m_taintforce = false;
2605 return;
2606 }
2607 d.BodyEnable(Body);
2608
2609 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2610 }
2611 m_forcelist.Clear();
2612 }
2613
2614 m_collisionscore = 0;
2615 m_interpenetrationcount = 0;
2616 }
2617
2618 m_taintforce = false;
2619
2620 }
2621
2622
2623
2624 public void changeSetTorque(float timestamp)
2625 {
2626 if (!m_isSelected)
2627 {
2628 if (IsPhysical && Body != IntPtr.Zero)
2629 {
2630 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2631 }
2632 }
2633
2634 m_taintTorque = Vector3.Zero;
2635 }
2636
2637 public void changeAddAngularForce(float timestamp)
2638 {
2639 if (!m_isSelected)
2640 {
2641 lock (m_angularforcelist)
2642 {
2643 //m_log.Info("[PHYSICS]: dequeing forcelist");
2644 if (IsPhysical)
2645 {
2646 Vector3 iforce = Vector3.Zero;
2647 for (int i = 0; i < m_angularforcelist.Count; i++)
2648 {
2649 iforce = iforce + (m_angularforcelist[i] * 100);
2650 }
2651 d.BodyEnable(Body);
2652 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2653
2654 }
2655 m_angularforcelist.Clear();
2656 }
2657
2658 m_collisionscore = 0;
2659 m_interpenetrationcount = 0;
2660 }
2661
2662 m_taintaddangularforce = false;
2663 }
2664
2665 private void changevelocity(float timestep)
2666 {
2667 if (!m_isSelected)
2668 {
2669 Thread.Sleep(20);
2670 if (IsPhysical)
2671 {
2672 if (Body != IntPtr.Zero)
2673 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2674 }
2675
2676 //resetCollisionAccounting();
2677 }
2678 m_taintVelocity = Vector3.Zero;
2679 }
2680
2681 public void UpdatePositionAndVelocity()
2682 {
2683 return; // moved to the Move () method
2684 }
2685
2686 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2687 {
2688 obj.I.M00 = pMat[0, 0];
2689 obj.I.M01 = pMat[0, 1];
2690 obj.I.M02 = pMat[0, 2];
2691 obj.I.M10 = pMat[1, 0];
2692 obj.I.M11 = pMat[1, 1];
2693 obj.I.M12 = pMat[1, 2];
2694 obj.I.M20 = pMat[2, 0];
2695 obj.I.M21 = pMat[2, 1];
2696 obj.I.M22 = pMat[2, 2];
2697 return obj;
2698 }
2699
2700 public override void SubscribeEvents(int ms)
2701 {
2702 m_eventsubscription = ms;
2703 _parent_scene.addCollisionEventReporting(this);
2704 }
2705
2706 public override void UnSubscribeEvents()
2707 {
2708 _parent_scene.remCollisionEventReporting(this);
2709 m_eventsubscription = 0;
2710 }
2711
2712 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2713 {
2714 if (CollisionEventsThisFrame == null)
2715 CollisionEventsThisFrame = new CollisionEventUpdate();
2716 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
2717 }
2718
2719 public void SendCollisions()
2720 {
2721 if (CollisionEventsThisFrame == null)
2722 return;
2723
2724 base.SendCollisionUpdate(CollisionEventsThisFrame);
2725
2726 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
2727 CollisionEventsThisFrame = null;
2728 else
2729 CollisionEventsThisFrame = new CollisionEventUpdate();
2730 }
2731
2732 public override bool SubscribedEvents()
2733 {
2734 if (m_eventsubscription > 0)
2735 return true;
2736 return false;
2737 }
2738
2739 public static Matrix4 Inverse(Matrix4 pMat)
2740 {
2741 if (determinant3x3(pMat) == 0)
2742 {
2743 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2744 }
2745
2746
2747
2748 return (Adjoint(pMat) / determinant3x3(pMat));
2749 }
2750
2751 public static Matrix4 Adjoint(Matrix4 pMat)
2752 {
2753 Matrix4 adjointMatrix = new Matrix4();
2754 for (int i = 0; i < 4; i++)
2755 {
2756 for (int j = 0; j < 4; j++)
2757 {
2758 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2759 }
2760 }
2761
2762 adjointMatrix = Transpose(adjointMatrix);
2763 return adjointMatrix;
2764 }
2765
2766 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2767 {
2768 Matrix4 minor = new Matrix4();
2769 int m = 0, n = 0;
2770 for (int i = 0; i < 4; i++)
2771 {
2772 if (i == iRow)
2773 continue;
2774 n = 0;
2775 for (int j = 0; j < 4; j++)
2776 {
2777 if (j == iCol)
2778 continue;
2779 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2780 n++;
2781 }
2782 m++;
2783 }
2784 return minor;
2785 }
2786
2787 public static Matrix4 Transpose(Matrix4 pMat)
2788 {
2789 Matrix4 transposeMatrix = new Matrix4();
2790 for (int i = 0; i < 4; i++)
2791 for (int j = 0; j < 4; j++)
2792 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
2793 return transposeMatrix;
2794 }
2795
2796 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2797 {
2798 switch (r)
2799 {
2800 case 0:
2801 switch (c)
2802 {
2803 case 0:
2804 pMat.M11 = val;
2805 break;
2806 case 1:
2807 pMat.M12 = val;
2808 break;
2809 case 2:
2810 pMat.M13 = val;
2811 break;
2812 case 3:
2813 pMat.M14 = val;
2814 break;
2815 }
2816
2817 break;
2818 case 1:
2819 switch (c)
2820 {
2821 case 0:
2822 pMat.M21 = val;
2823 break;
2824 case 1:
2825 pMat.M22 = val;
2826 break;
2827 case 2:
2828 pMat.M23 = val;
2829 break;
2830 case 3:
2831 pMat.M24 = val;
2832 break;
2833 }
2834
2835 break;
2836 case 2:
2837 switch (c)
2838 {
2839 case 0:
2840 pMat.M31 = val;
2841 break;
2842 case 1:
2843 pMat.M32 = val;
2844 break;
2845 case 2:
2846 pMat.M33 = val;
2847 break;
2848 case 3:
2849 pMat.M34 = val;
2850 break;
2851 }
2852
2853 break;
2854 case 3:
2855 switch (c)
2856 {
2857 case 0:
2858 pMat.M41 = val;
2859 break;
2860 case 1:
2861 pMat.M42 = val;
2862 break;
2863 case 2:
2864 pMat.M43 = val;
2865 break;
2866 case 3:
2867 pMat.M44 = val;
2868 break;
2869 }
2870
2871 break;
2872 }
2873 }
2874 private static float determinant3x3(Matrix4 pMat)
2875 {
2876 float det = 0;
2877 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2878 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2879 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2880 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2881 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2882 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2883
2884 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2885 return det;
2886
2887 }
2888
2889 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2890 {
2891 dst.c.W = src.c.W;
2892 dst.c.X = src.c.X;
2893 dst.c.Y = src.c.Y;
2894 dst.c.Z = src.c.Z;
2895 dst.mass = src.mass;
2896 dst.I.M00 = src.I.M00;
2897 dst.I.M01 = src.I.M01;
2898 dst.I.M02 = src.I.M02;
2899 dst.I.M10 = src.I.M10;
2900 dst.I.M11 = src.I.M11;
2901 dst.I.M12 = src.I.M12;
2902 dst.I.M20 = src.I.M20;
2903 dst.I.M21 = src.I.M21;
2904 dst.I.M22 = src.I.M22;
2905 }
2906
2907 public override void SetMaterial(int pMaterial)
2908 {
2909 m_material = pMaterial;
2910 }
2911
2912 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2913 {
2914 switch (pParam)
2915 {
2916 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2917 if (pValue < 0.01f) pValue = 0.01f;
2918 // m_angularDeflectionEfficiency = pValue;
2919 break;
2920 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2921 if (pValue < 0.1f) pValue = 0.1f;
2922 // m_angularDeflectionTimescale = pValue;
2923 break;
2924 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2925 if (pValue < 0.3f) pValue = 0.3f;
2926 m_angularMotorDecayTimescale = pValue;
2927 break;
2928 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2929 if (pValue < 0.3f) pValue = 0.3f;
2930 m_angularMotorTimescale = pValue;
2931 break;
2932 case Vehicle.BANKING_EFFICIENCY:
2933 if (pValue < 0.01f) pValue = 0.01f;
2934 // m_bankingEfficiency = pValue;
2935 break;
2936 case Vehicle.BANKING_MIX:
2937 if (pValue < 0.01f) pValue = 0.01f;
2938 // m_bankingMix = pValue;
2939 break;
2940 case Vehicle.BANKING_TIMESCALE:
2941 if (pValue < 0.01f) pValue = 0.01f;
2942 // m_bankingTimescale = pValue;
2943 break;
2944 case Vehicle.BUOYANCY:
2945 if (pValue < -1f) pValue = -1f;
2946 if (pValue > 1f) pValue = 1f;
2947 m_VehicleBuoyancy = pValue;
2948 break;
2949 // case Vehicle.HOVER_EFFICIENCY:
2950 // if (pValue < 0f) pValue = 0f;
2951 // if (pValue > 1f) pValue = 1f;
2952 // m_VhoverEfficiency = pValue;
2953 // break;
2954 case Vehicle.HOVER_HEIGHT:
2955 m_VhoverHeight = pValue;
2956 break;
2957 case Vehicle.HOVER_TIMESCALE:
2958 if (pValue < 0.1f) pValue = 0.1f;
2959 m_VhoverTimescale = pValue;
2960 break;
2961 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2962 if (pValue < 0.01f) pValue = 0.01f;
2963 // m_linearDeflectionEfficiency = pValue;
2964 break;
2965 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2966 if (pValue < 0.01f) pValue = 0.01f;
2967 // m_linearDeflectionTimescale = pValue;
2968 break;
2969 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2970 if (pValue < 0.3f) pValue = 0.3f;
2971 m_linearMotorDecayTimescale = pValue;
2972 break;
2973 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2974 if (pValue < 0.1f) pValue = 0.1f;
2975 m_linearMotorTimescale = pValue;
2976 break;
2977 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2978 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2979 if (pValue > 1.0f) pValue = 1.0f;
2980 m_verticalAttractionEfficiency = pValue;
2981 break;
2982 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2983 if (pValue < 0.1f) pValue = 0.1f;
2984 m_verticalAttractionTimescale = pValue;
2985 break;
2986
2987 // These are vector properties but the engine lets you use a single float value to
2988 // set all of the components to the same value
2989 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2990 if (pValue > 30f) pValue = 30f;
2991 if (pValue < 0.1f) pValue = 0.1f;
2992 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2993 break;
2994 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2995 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2996 UpdateAngDecay();
2997 break;
2998 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2999 if (pValue < 0.1f) pValue = 0.1f;
3000 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
3001 break;
3002 case Vehicle.LINEAR_MOTOR_DIRECTION:
3003 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
3004 UpdateLinDecay();
3005 break;
3006 case Vehicle.LINEAR_MOTOR_OFFSET:
3007 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
3008 break;
3009
3010 }
3011
3012 }//end ProcessFloatVehicleParam
3013
3014 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
3015 {
3016 switch (pParam)
3017 {
3018 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
3019 if (pValue.X > 30f) pValue.X = 30f;
3020 if (pValue.X < 0.1f) pValue.X = 0.1f;
3021 if (pValue.Y > 30f) pValue.Y = 30f;
3022 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3023 if (pValue.Z > 30f) pValue.Z = 30f;
3024 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3025 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3026 break;
3027 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3028 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
3029 // Limit requested angular speed to 2 rps= 4 pi rads/sec
3030 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
3031 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
3032 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
3033 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
3034 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
3035 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
3036 UpdateAngDecay();
3037 break;
3038 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3039 if (pValue.X < 0.1f) pValue.X = 0.1f;
3040 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3041 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3042 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3043 break;
3044 case Vehicle.LINEAR_MOTOR_DIRECTION:
3045 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
3046 UpdateLinDecay();
3047 break;
3048 case Vehicle.LINEAR_MOTOR_OFFSET:
3049 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
3050 break;
3051 }
3052
3053 }//end ProcessVectorVehicleParam
3054
3055 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
3056 {
3057 switch (pParam)
3058 {
3059 case Vehicle.REFERENCE_FRAME:
3060 // m_referenceFrame = pValue;
3061 break;
3062 }
3063
3064 }//end ProcessRotationVehicleParam
3065
3066 internal void ProcessVehicleFlags(int pParam, bool remove)
3067 {
3068 if (remove)
3069 {
3070 m_flags &= ~((VehicleFlag)pParam);
3071 }
3072 else
3073 {
3074 m_flags |= (VehicleFlag)pParam;
3075 }
3076 }
3077
3078 internal void ProcessTypeChange(Vehicle pType)
3079 {
3080 // Set Defaults For Type
3081 m_type = pType;
3082 switch (pType)
3083 {
3084 case Vehicle.TYPE_SLED:
3085 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
3086 m_angularFrictionTimescale = new Vector3(30, 30, 30);
3087 // m_lLinMotorVel = Vector3.Zero;
3088 m_linearMotorTimescale = 1000;
3089 m_linearMotorDecayTimescale = 120;
3090 m_angularMotorDirection = Vector3.Zero;
3091 m_angularMotorDVel = Vector3.Zero;
3092 m_angularMotorTimescale = 1000;
3093 m_angularMotorDecayTimescale = 120;
3094 m_VhoverHeight = 0;
3095 // m_VhoverEfficiency = 1;
3096 m_VhoverTimescale = 10;
3097 m_VehicleBuoyancy = 0;
3098 // m_linearDeflectionEfficiency = 1;
3099 // m_linearDeflectionTimescale = 1;
3100 // m_angularDeflectionEfficiency = 1;
3101 // m_angularDeflectionTimescale = 1000;
3102 // m_bankingEfficiency = 0;
3103 // m_bankingMix = 1;
3104 // m_bankingTimescale = 10;
3105 // m_referenceFrame = Quaternion.Identity;
3106 m_flags &=
3107 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3108 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3109 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3110 break;
3111 case Vehicle.TYPE_CAR:
3112 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
3113 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
3114 // m_lLinMotorVel = Vector3.Zero;
3115 m_linearMotorTimescale = 1;
3116 m_linearMotorDecayTimescale = 60;
3117 m_angularMotorDirection = Vector3.Zero;
3118 m_angularMotorDVel = Vector3.Zero;
3119 m_angularMotorTimescale = 1;
3120 m_angularMotorDecayTimescale = 0.8f;
3121 m_VhoverHeight = 0;
3122 // m_VhoverEfficiency = 0;
3123 m_VhoverTimescale = 1000;
3124 m_VehicleBuoyancy = 0;
3125 // // m_linearDeflectionEfficiency = 1;
3126 // // m_linearDeflectionTimescale = 2;
3127 // // m_angularDeflectionEfficiency = 0;
3128 // m_angularDeflectionTimescale = 10;
3129 m_verticalAttractionEfficiency = 1f;
3130 m_verticalAttractionTimescale = 10f;
3131 // m_bankingEfficiency = -0.2f;
3132 // m_bankingMix = 1;
3133 // m_bankingTimescale = 1;
3134 // m_referenceFrame = Quaternion.Identity;
3135 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3136 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
3137 VehicleFlag.LIMIT_MOTOR_UP);
3138 break;
3139 case Vehicle.TYPE_BOAT:
3140 m_linearFrictionTimescale = new Vector3(10, 3, 2);
3141 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3142 // m_lLinMotorVel = Vector3.Zero;
3143 m_linearMotorTimescale = 5;
3144 m_linearMotorDecayTimescale = 60;
3145 m_angularMotorDirection = Vector3.Zero;
3146 m_angularMotorDVel = Vector3.Zero;
3147 m_angularMotorTimescale = 4;
3148 m_angularMotorDecayTimescale = 4;
3149 m_VhoverHeight = 0;
3150 // m_VhoverEfficiency = 0.5f;
3151 m_VhoverTimescale = 2;
3152 m_VehicleBuoyancy = 1;
3153 // m_linearDeflectionEfficiency = 0.5f;
3154 // m_linearDeflectionTimescale = 3;
3155 // m_angularDeflectionEfficiency = 0.5f;
3156 // m_angularDeflectionTimescale = 5;
3157 m_verticalAttractionEfficiency = 0.5f;
3158 m_verticalAttractionTimescale = 5f;
3159 // m_bankingEfficiency = -0.3f;
3160 // m_bankingMix = 0.8f;
3161 // m_bankingTimescale = 1;
3162 // m_referenceFrame = Quaternion.Identity;
3163 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
3164 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3165 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
3166 VehicleFlag.LIMIT_MOTOR_UP);
3167 break;
3168 case Vehicle.TYPE_AIRPLANE:
3169 m_linearFrictionTimescale = new Vector3(200, 10, 5);
3170 m_angularFrictionTimescale = new Vector3(20, 20, 20);
3171 // m_lLinMotorVel = Vector3.Zero;
3172 m_linearMotorTimescale = 2;
3173 m_linearMotorDecayTimescale = 60;
3174 m_angularMotorDirection = Vector3.Zero;
3175 m_angularMotorDVel = Vector3.Zero;
3176 m_angularMotorTimescale = 4;
3177 m_angularMotorDecayTimescale = 4;
3178 m_VhoverHeight = 0;
3179 // m_VhoverEfficiency = 0.5f;
3180 m_VhoverTimescale = 1000;
3181 m_VehicleBuoyancy = 0;
3182 // m_linearDeflectionEfficiency = 0.5f;
3183 // m_linearDeflectionTimescale = 3;
3184 // m_angularDeflectionEfficiency = 1;
3185 // m_angularDeflectionTimescale = 2;
3186 m_verticalAttractionEfficiency = 0.9f;
3187 m_verticalAttractionTimescale = 2f;
3188 // m_bankingEfficiency = 1;
3189 // m_bankingMix = 0.7f;
3190 // m_bankingTimescale = 2;
3191 // m_referenceFrame = Quaternion.Identity;
3192 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3193 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3194 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
3195 break;
3196 case Vehicle.TYPE_BALLOON:
3197 m_linearFrictionTimescale = new Vector3(5, 5, 5);
3198 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3199 m_linearMotorTimescale = 5;
3200 m_linearMotorDecayTimescale = 60;
3201 m_angularMotorDirection = Vector3.Zero;
3202 m_angularMotorDVel = Vector3.Zero;
3203 m_angularMotorTimescale = 6;
3204 m_angularMotorDecayTimescale = 10;
3205 m_VhoverHeight = 5;
3206 // m_VhoverEfficiency = 0.8f;
3207 m_VhoverTimescale = 10;
3208 m_VehicleBuoyancy = 1;
3209 // m_linearDeflectionEfficiency = 0;
3210 // m_linearDeflectionTimescale = 5;
3211 // m_angularDeflectionEfficiency = 0;
3212 // m_angularDeflectionTimescale = 5;
3213 m_verticalAttractionEfficiency = 1f;
3214 m_verticalAttractionTimescale = 100f;
3215 // m_bankingEfficiency = 0;
3216 // m_bankingMix = 0.7f;
3217 // m_bankingTimescale = 5;
3218 // m_referenceFrame = Quaternion.Identity;
3219 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3220 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3221 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3222 break;
3223
3224 }
3225 }//end SetDefaultsForType
3226
3227 internal void Enable(IntPtr pBody, OdeScene pParentScene)
3228 {
3229 if (m_type == Vehicle.TYPE_NONE)
3230 return;
3231
3232 m_body = pBody;
3233 }
3234
3235
3236 internal void Halt()
3237 { // Kill all motions, when non-physical
3238 // m_linearMotorDirection = Vector3.Zero;
3239 m_lLinMotorDVel = Vector3.Zero;
3240 m_lLinObjectVel = Vector3.Zero;
3241 m_wLinObjectVel = Vector3.Zero;
3242 m_angularMotorDirection = Vector3.Zero;
3243 m_lastAngularVelocity = Vector3.Zero;
3244 m_angularMotorDVel = Vector3.Zero;
3245 _acceleration = Vector3.Zero;
3246 }
3247
3248 private void UpdateLinDecay()
3249 {
3250 m_lLinMotorDVel.X = m_linearMotorDirection.X;
3251 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3252 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3253 } // else let the motor decay on its own
3254
3255 private void UpdateAngDecay()
3256 {
3257 m_angularMotorDVel.X = m_angularMotorDirection.X;
3258 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3259 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3260 } // else let the motor decay on its own
3261
3262 public void Move(float timestep)
3263 {
3264 float fx = 0;
3265 float fy = 0;
3266 float fz = 0;
3267 Vector3 linvel; // velocity applied, including any reversal
3268
3269 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3270 // This is a temp patch until proper region crossing is developed.
3271
3272
3273 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3274 {
3275 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3276 bool lastZeroFlag = _zeroFlag; // was it stopped
3277
3278 d.Vector3 vec = d.BodyGetPosition(Body);
3279 Vector3 l_position = Vector3.Zero;
3280 l_position.X = vec.X;
3281 l_position.Y = vec.Y;
3282 l_position.Z = vec.Z;
3283 m_lastposition = _position;
3284 _position = l_position;
3285
3286 d.Quaternion ori = d.BodyGetQuaternion(Body);
3287 // Quaternion l_orientation = Quaternion.Identity;
3288 _orientation.X = ori.X;
3289 _orientation.Y = ori.Y;
3290 _orientation.Z = ori.Z;
3291 _orientation.W = ori.W;
3292 m_lastorientation = _orientation;
3293
3294 d.Vector3 vel = d.BodyGetLinearVel(Body);
3295 m_lastVelocity = _velocity;
3296 _velocity.X = vel.X;
3297 _velocity.Y = vel.Y;
3298 _velocity.Z = vel.Z;
3299 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3300
3301 d.Vector3 torque = d.BodyGetTorque(Body);
3302 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3303
3304
3305 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3306 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3307 )
3308 {
3309 // we are outside current region
3310 // clip position to a stop just outside region and stop it only internally
3311 // do it only once using m_crossingfailures as control
3312 _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f);
3313 _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f);
3314 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3315 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3316 d.BodySetLinearVel(Body, 0, 0, 0);
3317 m_outofBounds = true;
3318 base.RequestPhysicsterseUpdate();
3319 return;
3320 }
3321
3322 base.RequestPhysicsterseUpdate();
3323
3324 if (l_position.Z < 0)
3325 {
3326 // This is so prim that get lost underground don't fall forever and suck up
3327 //
3328 // Sim resources and memory.
3329 // Disables the prim's movement physics....
3330 // It's a hack and will generate a console message if it fails.
3331
3332 //IsPhysical = false;
3333 if (_parent == null) base.RaiseOutOfBounds(_position);
3334
3335
3336 _acceleration.X = 0; // This stuff may stop client display but it has no
3337 _acceleration.Y = 0; // effect on the object in phys engine!
3338 _acceleration.Z = 0;
3339
3340 _velocity.X = 0;
3341 _velocity.Y = 0;
3342 _velocity.Z = 0;
3343 m_lastVelocity = Vector3.Zero;
3344 m_rotationalVelocity.X = 0;
3345 m_rotationalVelocity.Y = 0;
3346 m_rotationalVelocity.Z = 0;
3347
3348 if (_parent == null) base.RequestPhysicsterseUpdate();
3349
3350 m_throttleUpdates = false;
3351 throttleCounter = 0;
3352 _zeroFlag = true;
3353 //outofBounds = true;
3354 } // end neg Z check
3355
3356 // Is it moving?
3357 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3358 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3359 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3360 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3361 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3362 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3363 {
3364 _zeroFlag = true;
3365 m_throttleUpdates = false;
3366 }
3367 else
3368 {
3369 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3370 _zeroFlag = false;
3371 m_lastUpdateSent = false;
3372 //m_throttleUpdates = false;
3373 }
3374
3375 if (_zeroFlag)
3376 { // Its stopped
3377 _velocity.X = 0.0f;
3378 _velocity.Y = 0.0f;
3379 // _velocity.Z = 0.0f;
3380
3381 _acceleration.X = 0;
3382 _acceleration.Y = 0;
3383 // _acceleration.Z = 0;
3384
3385 m_rotationalVelocity.X = 0;
3386 m_rotationalVelocity.Y = 0;
3387 m_rotationalVelocity.Z = 0;
3388 // Stop it in the phys engine
3389 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3390 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3391 d.BodySetForce(Body, 0f, 0f, 0f);
3392
3393 if (!m_lastUpdateSent)
3394 {
3395 m_throttleUpdates = false;
3396 throttleCounter = 0;
3397 if (_parent == null)
3398 {
3399 base.RequestPhysicsterseUpdate();
3400 }
3401
3402 m_lastUpdateSent = true;
3403 }
3404 }
3405 else
3406 { // Its moving
3407 if (lastZeroFlag != _zeroFlag)
3408 {
3409 if (_parent == null)
3410 {
3411 base.RequestPhysicsterseUpdate();
3412 }
3413 }
3414 m_lastUpdateSent = false;
3415 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3416 {
3417 if (_parent == null)
3418 {
3419 base.RequestPhysicsterseUpdate();
3420 }
3421 }
3422 else
3423 {
3424 throttleCounter++;
3425 }
3426 }
3427 m_lastposition = l_position;
3428
3429 /// End UpdatePositionAndVelocity insert
3430
3431
3432 // Rotation lock =====================================
3433 if (m_rotateEnableUpdate)
3434 {
3435 // Snapshot current angles, set up Amotor(s)
3436 m_rotateEnableUpdate = false;
3437 m_rotateEnable = m_rotateEnableRequest;
3438 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3439
3440 if (Amotor != IntPtr.Zero)
3441 {
3442 d.JointDestroy(Amotor);
3443 Amotor = IntPtr.Zero;
3444 //Console.WriteLine("Old Amotor Destroyed");
3445 }
3446
3447 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3448 { // not all are enabled
3449 d.Quaternion r = d.BodyGetQuaternion(Body);
3450 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3451 // extract the axes vectors
3452 Vector3 vX = new Vector3(1f, 0f, 0f);
3453 Vector3 vY = new Vector3(0f, 1f, 0f);
3454 Vector3 vZ = new Vector3(0f, 0f, 1f);
3455 vX = vX * locrot;
3456 vY = vY * locrot;
3457 vZ = vZ * locrot;
3458 // snapshot the current angle vectors
3459 m_lockX = vX;
3460 m_lockY = vY;
3461 m_lockZ = vZ;
3462 // m_lockRot = locrot;
3463 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3464 d.JointAttach(Amotor, Body, IntPtr.Zero);
3465 d.JointSetAMotorMode(Amotor, 0); // User mode??
3466 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3467
3468 float axisnum = 3; // how many to lock
3469 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3470 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3471 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3472
3473 int i = 0;
3474
3475 if (m_rotateEnable.X == 0)
3476 {
3477 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3478 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3479 i++;
3480 }
3481
3482 if (m_rotateEnable.Y == 0)
3483 {
3484 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3485 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3486 i++;
3487 }
3488
3489 if (m_rotateEnable.Z == 0)
3490 {
3491 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3492 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3493 i++;
3494 }
3495
3496 // These lowstops and high stops are effectively (no wiggle room)
3497 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3498 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3499 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3500 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3501 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3502 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3503 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3504 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3505 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3506 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3507 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3508 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3509 } // else none are locked
3510 } // end Rotation Update
3511
3512
3513 // VEHICLE processing ==========================================
3514 if (m_type != Vehicle.TYPE_NONE)
3515 {
3516 // get body attitude
3517 d.Quaternion rot = d.BodyGetQuaternion(Body);
3518 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3519 Quaternion irotq = Quaternion.Inverse(rotq);
3520
3521 // VEHICLE Linear Motion
3522 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3523 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3524 m_lLinObjectVel = vel_now * irotq;
3525 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3526 {
3527 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3528 {
3529 float decayfactor = m_linearMotorDecayTimescale / timestep;
3530 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3531 m_lLinMotorDVel -= decayAmount;
3532 }
3533 else
3534 {
3535 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3536 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3537 m_lLinMotorDVel -= decel;
3538 }
3539 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3540 {
3541 m_lLinMotorDVel = Vector3.Zero;
3542 }
3543
3544 /* else
3545 {
3546 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3547 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3548 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3549 } */
3550 } // end linear motor decay
3551
3552 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3553 {
3554 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3555 if (m_linearMotorTimescale < 300.0f)
3556 {
3557 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3558 float linfactor = m_linearMotorTimescale / timestep;
3559 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3560 m_lLinObjectVel += attackAmount;
3561 }
3562 if (m_linearFrictionTimescale.X < 300.0f)
3563 {
3564 float fricfactor = m_linearFrictionTimescale.X / timestep;
3565 float fricX = m_lLinObjectVel.X / fricfactor;
3566 m_lLinObjectVel.X -= fricX;
3567 }
3568 if (m_linearFrictionTimescale.Y < 300.0f)
3569 {
3570 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3571 float fricY = m_lLinObjectVel.Y / fricfactor;
3572 m_lLinObjectVel.Y -= fricY;
3573 }
3574 if (m_linearFrictionTimescale.Z < 300.0f)
3575 {
3576 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3577 float fricZ = m_lLinObjectVel.Z / fricfactor;
3578 m_lLinObjectVel.Z -= fricZ;
3579 }
3580 }
3581 m_wLinObjectVel = m_lLinObjectVel * rotq;
3582
3583 // Gravity and Buoyancy
3584 Vector3 grav = Vector3.Zero;
3585 if (m_VehicleBuoyancy < 1.0f)
3586 {
3587 // There is some gravity, make a gravity force vector
3588 // that is applied after object velocity.
3589 d.Mass objMass;
3590 d.BodyGetMass(Body, out objMass);
3591 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3592 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3593 } // else its 1.0, no gravity.
3594
3595 // Hovering
3596 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3597 {
3598 // We should hover, get the target height
3599 d.Vector3 pos = d.BodyGetPosition(Body);
3600 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3601 {
3602 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3603 }
3604 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3605 {
3606 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3607 }
3608 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3609 {
3610 m_VhoverTargetHeight = m_VhoverHeight;
3611 }
3612
3613 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3614 {
3615 // If body is aready heigher, use its height as target height
3616 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3617 }
3618
3619 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3620 // m_VhoverTimescale = 0f; // time to acheive height
3621 // timestep is time since last frame,in secs
3622 float herr0 = pos.Z - m_VhoverTargetHeight;
3623 // Replace Vertical speed with correction figure if significant
3624 if (Math.Abs(herr0) > 0.01f)
3625 {
3626 //? d.Mass objMass;
3627 //? d.BodyGetMass(Body, out objMass);
3628 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3629 //KF: m_VhoverEfficiency is not yet implemented
3630 }
3631 else
3632 {
3633 m_wLinObjectVel.Z = 0f;
3634 }
3635 }
3636 else
3637 { // not hovering
3638 if (m_wLinObjectVel.Z == 0f)
3639 { // Gravity rules
3640 m_wLinObjectVel.Z = vel_now.Z;
3641 } // else the motor has it
3642 }
3643 linvel = m_wLinObjectVel;
3644
3645 // Vehicle Linear Motion done =======================================
3646 // Apply velocity
3647 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3648 // apply gravity force
3649 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3650 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3651 // end MoveLinear()
3652
3653
3654 // MoveAngular
3655 /*
3656 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3657
3658 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3659 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3660 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3661
3662 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3663 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3664 */
3665 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3666
3667 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3668 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3669 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3670
3671 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3672
3673 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3674 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3675 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3676 // Decay Angular Motor 2.
3677 if (m_angularMotorDecayTimescale < 300.0f)
3678 {
3679 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3680 {
3681 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3682 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3683 m_angularMotorDVel -= decayAmount;
3684 }
3685 else
3686 {
3687 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3688 m_angularMotorDVel -= decel;
3689 }
3690
3691 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3692 {
3693 m_angularMotorDVel = Vector3.Zero;
3694 }
3695 else
3696 {
3697 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3698 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3699 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3700 }
3701 } // end decay angular motor
3702 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3703
3704 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3705
3706 if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3707 { // if motor or object have motion
3708 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3709
3710 if (m_angularMotorTimescale < 300.0f)
3711 {
3712 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3713 float angfactor = m_angularMotorTimescale / timestep;
3714 Vector3 attackAmount = (attack_error / angfactor);
3715 angObjectVel += attackAmount;
3716 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3717 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3718 }
3719
3720 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3721 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3722 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3723 } // else no signif. motion
3724
3725 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3726 // Bank section tba
3727 // Deflection section tba
3728 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3729
3730
3731 /* // Rotation Axis Disables:
3732 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3733 {
3734 if (m_angularEnable.X == 0)
3735 angObjectVel.X = 0f;
3736 if (m_angularEnable.Y == 0)
3737 angObjectVel.Y = 0f;
3738 if (m_angularEnable.Z == 0)
3739 angObjectVel.Z = 0f;
3740 }
3741 */
3742 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3743
3744 // Vertical attractor section
3745 Vector3 vertattr = Vector3.Zero;
3746
3747 if (m_verticalAttractionTimescale < 300)
3748 {
3749 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3750 // make a vector pointing up
3751 Vector3 verterr = Vector3.Zero;
3752 verterr.Z = 1.0f;
3753 // rotate it to Body Angle
3754 verterr = verterr * rotq;
3755 // 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.
3756 // 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
3757 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3758
3759 if (verterr.Z < 0.0f)
3760 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3761 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3762 //Console.WriteLine("InvertFlip");
3763 verterr.X = 2.0f - verterr.X;
3764 verterr.Y = 2.0f - verterr.Y;
3765 }
3766 verterr *= 0.5f;
3767 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3768 Vector3 xyav = angObjectVel;
3769 xyav.Z = 0.0f;
3770 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3771 {
3772 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3773 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3774 vertattr.X = verterr.Y;
3775 vertattr.Y = -verterr.X;
3776 vertattr.Z = 0f;
3777 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3778
3779 // scaling appears better usingsquare-law
3780 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3781 float bounce = 1.0f - damped;
3782 // 0 = crit damp, 1 = bouncy
3783 float oavz = angObjectVel.Z; // retain z velocity
3784 // time-scaled correction, which sums, therefore is bouncy:
3785 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3786 // damped, good @ < 90:
3787 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3788 angObjectVel.Z = oavz;
3789 //if(frcount == 0) Console.WriteLine("VA+");
3790 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3791 }
3792 else
3793 {
3794 // else error is very small
3795 angObjectVel.X = 0f;
3796 angObjectVel.Y = 0f;
3797 //if(frcount == 0) Console.WriteLine("VA0");
3798 }
3799 } // else vertical attractor is off
3800 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3801
3802
3803 m_lastAngularVelocity = angObjectVel;
3804 // apply Angular Velocity to body
3805 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3806 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3807
3808 } // end VEHICLES
3809 else
3810 {
3811 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3812
3813 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3814
3815 /// Dynamics Buoyancy
3816 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3817 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3818 // NB Prims in ODE are no subject to global gravity
3819 // This should only affect gravity operations
3820
3821 float m_mass = CalculateMass();
3822 // calculate z-force due togravity on object.
3823 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3824 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3825 {
3826 fz = 0; // llMoveToTarget ignores gravity.
3827 // it also ignores mass of object, and any physical resting on it.
3828 // Vector3 m_PIDTarget is where we are going
3829 // float m_PIDTau is time to get there
3830 fx = 0;
3831 fy = 0;
3832 d.Vector3 pos = d.BodyGetPosition(Body);
3833 Vector3 error = new Vector3(
3834 (m_PIDTarget.X - pos.X),
3835 (m_PIDTarget.Y - pos.Y),
3836 (m_PIDTarget.Z - pos.Z));
3837 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3838 { // Very close, Jump there and quit move
3839
3840 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3841 _target_velocity = Vector3.Zero;
3842 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3843 d.BodySetForce(Body, 0f, 0f, 0f);
3844 }
3845 else
3846 {
3847 float scale = 50.0f * timestep / m_PIDTau;
3848 if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
3849 {
3850 // Nearby, quit update of velocity
3851 }
3852 else
3853 { // Far, calc damped velocity
3854 _target_velocity = error * scale;
3855 }
3856 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3857 }
3858 } // end PID MoveToTarget
3859
3860
3861 /// Dynamics Hover ===================================================================================
3862 // Hover PID Controller can only run if the PIDcontroller is not in use.
3863 if (m_useHoverPID && !m_usePID)
3864 {
3865 //Console.WriteLine("Hover " + m_primName);
3866
3867 // If we're using the PID controller, then we have no gravity
3868 fz = (-1 * _parent_scene.gravityz) * m_mass;
3869
3870 // no lock; for now it's only called from within Simulate()
3871
3872 // If the PID Controller isn't active then we set our force
3873 // calculating base velocity to the current position
3874
3875 if ((m_PIDTau < 1))
3876 {
3877 PID_G = PID_G / m_PIDTau;
3878 }
3879
3880 if ((PID_G - m_PIDTau) <= 0)
3881 {
3882 PID_G = m_PIDTau + 1;
3883 }
3884
3885
3886 // Where are we, and where are we headed?
3887 d.Vector3 pos = d.BodyGetPosition(Body);
3888 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3889
3890
3891 // Non-Vehicles have a limited set of Hover options.
3892 // determine what our target height really is based on HoverType
3893 switch (m_PIDHoverType)
3894 {
3895 case PIDHoverType.Ground:
3896 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3897 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3898 break;
3899 case PIDHoverType.GroundAndWater:
3900 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3901 m_waterHeight = _parent_scene.GetWaterLevel();
3902 if (m_groundHeight > m_waterHeight)
3903 {
3904 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3905 }
3906 else
3907 {
3908 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3909 }
3910 break;
3911
3912 } // end switch (m_PIDHoverType)
3913
3914
3915 _target_velocity =
3916 new Vector3(0.0f, 0.0f,
3917 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3918 );
3919
3920 // if velocity is zero, use position control; otherwise, velocity control
3921
3922 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3923 {
3924 // keep track of where we stopped. No more slippin' & slidin'
3925
3926 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3927 // react to the physics scene by moving it's position.
3928 // Avatar to Avatar collisions
3929 // Prim to avatar collisions
3930 d.Vector3 dlinvel = vel;
3931 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3932 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3933 d.BodyAddForce(Body, 0, 0, fz);
3934 //KF this prevents furthur motions return;
3935 }
3936 else
3937 {
3938 _zeroFlag = false;
3939
3940 // We're flying and colliding with something
3941 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3942 }
3943 } // end m_useHoverPID && !m_usePID
3944
3945
3946 /// Dynamics Apply Forces ===================================================================================
3947 fx *= m_mass;
3948 fy *= m_mass;
3949 //fz *= m_mass;
3950 fx += m_force.X;
3951 fy += m_force.Y;
3952 fz += m_force.Z;
3953
3954 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3955 if (fx != 0 || fy != 0 || fz != 0)
3956 {
3957 //m_taintdisable = true;
3958 //base.RaiseOutOfBounds(Position);
3959 //d.BodySetLinearVel(Body, fx, fy, 0f);
3960 if (!d.BodyIsEnabled(Body))
3961 {
3962 // A physical body at rest on a surface will auto-disable after a while,
3963 // this appears to re-enable it incase the surface it is upon vanishes,
3964 // and the body should fall again.
3965 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3966 d.BodySetForce(Body, 0f, 0f, 0f);
3967 enableBodySoft();
3968 }
3969
3970 // 35x10 = 350n times the mass per second applied maximum.
3971 float nmax = 35f * m_mass;
3972 float nmin = -35f * m_mass;
3973
3974
3975 if (fx > nmax)
3976 fx = nmax;
3977 if (fx < nmin)
3978 fx = nmin;
3979 if (fy > nmax)
3980 fy = nmax;
3981 if (fy < nmin)
3982 fy = nmin;
3983 d.BodyAddForce(Body, fx, fy, fz);
3984 } // end apply forces
3985 } // end Vehicle/Dynamics
3986
3987 /// RotLookAt / LookAt =================================================================================
3988 if (m_useAPID)
3989 {
3990 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3991 // Quaternion m_APIDTarget
3992 // float m_APIDStrength // From SL experiments, this is the time to get there
3993 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3994 // Also in SL the mass of the object has no effect on time to get there.
3995 // Factors:
3996 // get present body rotation
3997 float limit = 1.0f;
3998 float rscaler = 50f; // adjusts rotation damping time
3999 float lscaler = 10f; // adjusts linear damping time in llLookAt
4000 float RLAservo = 0f;
4001 Vector3 diff_axis;
4002 float diff_angle;
4003 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
4004 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
4005 Quaternion rtarget = new Quaternion();
4006
4007 if (m_APIDTarget.W == -99.9f)
4008 {
4009 // this is really a llLookAt(), x,y,z is the target vector
4010 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
4011 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
4012 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
4013 float twopi = 2.0f * (float)Math.PI;
4014 Vector3 dir = target - _position;
4015 dir.Normalize();
4016 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
4017 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
4018 float terot = (float)Math.Atan2(dir.Z, txy);
4019 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
4020 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
4021 float oerot = (float)Math.Atan2(ospin.Z, oxy);
4022 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
4023 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
4024 float roll = (float)Math.Atan2(ra, rb);
4025 float errorz = tzrot - ozrot;
4026 if (errorz > (float)Math.PI) errorz -= twopi;
4027 else if (errorz < -(float)Math.PI) errorz += twopi;
4028 float errory = oerot - terot;
4029 if (errory > (float)Math.PI) errory -= twopi;
4030 else if (errory < -(float)Math.PI) errory += twopi;
4031 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
4032 if (diff_angle > 0.01f * m_APIDdamper)
4033 {
4034 m_APIDdamper = 1.0f;
4035 RLAservo = timestep / m_APIDStrength * rscaler;
4036 errorz *= RLAservo;
4037 errory *= RLAservo;
4038 error.X = -roll * 8.0f;
4039 error.Y = errory;
4040 error.Z = errorz;
4041 error *= rotq;
4042 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
4043 }
4044 else
4045 {
4046 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4047 m_APIDdamper = 2.0f;
4048 }
4049 }
4050 else
4051 {
4052 // this is a llRotLookAt()
4053 rtarget = m_APIDTarget;
4054
4055 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
4056 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
4057 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
4058
4059 // diff_axis.Normalize(); it already is!
4060 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
4061 {
4062 m_APIDdamper = 1.0f;
4063 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
4064 rotforce = rotforce * rotq;
4065 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
4066 RLAservo = timestep / m_APIDStrength * lscaler;
4067 rotforce = rotforce * RLAservo * diff_angle;
4068 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
4069 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
4070 }
4071 else
4072 { // close enough
4073 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
4074 m_APIDdamper = 2.0f;
4075 }
4076 } // end llLookAt/llRotLookAt
4077 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
4078 } // end m_useAPID
4079 } // end root prims
4080 } // end Move()
4081 } // end class
4082}
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..be67204 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -68,6 +68,19 @@ namespace OpenSim.Region.Physics.Manager
68 } 68 }
69 } 69 }
70 70
71 public struct ContactData
72 {
73 public float mu;
74 public float bounce;
75 public bool softcolide;
76
77 public ContactData(float _mu, float _bounce, bool _softcolide)
78 {
79 mu = _mu;
80 bounce = _bounce;
81 softcolide = _softcolide;
82 }
83 }
71 /// <summary> 84 /// <summary>
72 /// Used to pass collision information to OnCollisionUpdate listeners. 85 /// Used to pass collision information to OnCollisionUpdate listeners.
73 /// </summary> 86 /// </summary>
@@ -135,6 +148,8 @@ namespace OpenSim.Region.Physics.Manager
135 /// </summary> 148 /// </summary>
136 public event CollisionUpdate OnCollisionUpdate; 149 public event CollisionUpdate OnCollisionUpdate;
137 150
151 public virtual void SetVehicle(object vdata) { }
152
138 public event OutOfBounds OnOutOfBounds; 153 public event OutOfBounds OnOutOfBounds;
139#pragma warning restore 67 154#pragma warning restore 67
140 155
@@ -142,11 +157,23 @@ namespace OpenSim.Region.Physics.Manager
142 { 157 {
143 get { return new NullPhysicsActor(); } 158 get { return new NullPhysicsActor(); }
144 } 159 }
160
161 public virtual bool Building { get; set; }
162
163 public virtual void getContactData(ref ContactData cdata)
164 {
165 cdata.mu = 0;
166 cdata.bounce = 0;
167 }
145 168
146 public abstract bool Stopped { get; } 169 public abstract bool Stopped { get; }
147 170
148 public abstract Vector3 Size { get; set; } 171 public abstract Vector3 Size { get; set; }
149 172
173 public virtual bool Phantom { get; set; }
174
175 public virtual byte PhysicsShapeType { get; set; }
176
150 public abstract PrimitiveBaseShape Shape { set; } 177 public abstract PrimitiveBaseShape Shape { set; }
151 178
152 uint m_baseLocalID; 179 uint m_baseLocalID;
@@ -195,6 +222,11 @@ namespace OpenSim.Region.Physics.Manager
195 } 222 }
196 } 223 }
197 224
225 public virtual byte[] Serialize(bool PhysIsRunning)
226 {
227 return new byte[0];
228 }
229
198 public virtual void RaiseOutOfBounds(Vector3 pos) 230 public virtual void RaiseOutOfBounds(Vector3 pos)
199 { 231 {
200 // Make a temporary copy of the event to avoid possibility of 232 // Make a temporary copy of the event to avoid possibility of
@@ -222,6 +254,11 @@ namespace OpenSim.Region.Physics.Manager
222 { 254 {
223 } 255 }
224 256
257 public virtual float Density { get; set; }
258 public virtual float GravModifier { get; set; }
259 public virtual float Friction { get; set; }
260 public virtual float Bounce { get; set; }
261
225 /// <summary> 262 /// <summary>
226 /// Position of this actor. 263 /// Position of this actor.
227 /// </summary> 264 /// </summary>
@@ -554,5 +591,6 @@ namespace OpenSim.Region.Physics.Manager
554 { 591 {
555 return false; 592 return false;
556 } 593 }
594
557 } 595 }
558} 596}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 2a6163c..f2c0c28 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -125,6 +125,25 @@ namespace OpenSim.Region.Physics.Manager
125 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 125 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
126 Vector3 size, Quaternion rotation, bool isPhysical, uint localid); 126 Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
127 127
128 public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
129 uint localid, byte[] sdata)
130 {
131 return null;
132 }
133
134 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
135 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
136 {
137 return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
138 }
139
140
141 public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
142 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid)
143 {
144 return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid);
145 }
146
128 public virtual float TimeDilation 147 public virtual float TimeDilation
129 { 148 {
130 get { return 1.0f; } 149 get { return 1.0f; }
@@ -222,7 +241,7 @@ namespace OpenSim.Region.Physics.Manager
222 } 241 }
223 242
224 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} 243 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
225 244 public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {}
226 public virtual void UnCombine(PhysicsScene pScene) {} 245 public virtual void UnCombine(PhysicsScene pScene) {}
227 246
228 /// <summary> 247 /// <summary>
@@ -260,5 +279,13 @@ namespace OpenSim.Region.Physics.Manager
260 { 279 {
261 return new List<ContactResult>(); 280 return new List<ContactResult>();
262 } 281 }
282
283 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){}
284 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { }
285 public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
286 {
287 return new List<ContactResult>();
288 }
289
263 } 290 }
264} 291}
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..75fa1ef 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;
@@ -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 1f79cd8..6f37347 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -595,8 +595,8 @@ namespace OpenSim.Region.Physics.OdePlugin
595 break; 595 break;
596 596
597 case HollowShape.Circle: 597 case HollowShape.Circle:
598 // Hollow shape is a perfect cyllinder in respect to the cube's scale 598 // Hollow shape is a perfect cylinder in respect to the cube's scale
599 // Cyllinder hollow volume calculation 599 // Cylinder hollow volume calculation
600 600
601 hollowVolume *= 0.1963495f * 3.07920140172638f; 601 hollowVolume *= 0.1963495f * 3.07920140172638f;
602 break; 602 break;
@@ -2845,7 +2845,7 @@ Console.WriteLine(" JointCreateFixed");
2845 } 2845 }
2846 public override bool PIDActive { set { m_usePID = value; } } 2846 public override bool PIDActive { set { m_usePID = value; } }
2847 public override float PIDTau { set { m_PIDTau = value; } } 2847 public override float PIDTau { set { m_PIDTau = value; } }
2848 2848
2849 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 2849 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2850 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 2850 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2851 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 2851 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..8e903e8
--- /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.ErrorFormat("[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..9a22331
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1454 @@
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 CollisionLocker ode;
107
108 private string m_name = String.Empty;
109 // other filter control
110 int m_colliderfilter = 0;
111 // int m_colliderGroundfilter = 0;
112 int m_colliderObjectfilter = 0;
113
114 // Default we're a Character
115 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
116
117 // Default, Collide with Other Geometries, spaces, bodies and characters.
118 private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
119 | CollisionCategories.Space
120 | CollisionCategories.Body
121 | CollisionCategories.Character
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 private bool m_haseventsubscription = false;
132 public int m_eventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
134
135 // unique UUID of this character object
136 public UUID m_uuid;
137 public bool bad = false;
138
139 float mu;
140 float bounce;
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 mu = parent_scene.AvatarFriction;
173 bounce = parent_scene.AvatarBounce;
174
175 walkDivisor = walk_divisor;
176 runDivisor = rundivisor;
177
178 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
179 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
180
181 m_isPhysical = false; // current status: no ODE information exists
182
183 m_name = avName;
184
185 AddChange(changes.Add, null);
186 }
187
188 public override int PhysicsActorType
189 {
190 get { return (int)ActorTypes.Agent; }
191 set { return; }
192 }
193
194 public override void getContactData(ref ContactData cdata)
195 {
196 cdata.mu = mu;
197 cdata.bounce = bounce;
198 cdata.softcolide = false;
199 }
200
201 public override bool Building { get; set; }
202
203 /// <summary>
204 /// If this is set, the avatar will move faster
205 /// </summary>
206 public override bool SetAlwaysRun
207 {
208 get { return m_alwaysRun; }
209 set { m_alwaysRun = value; }
210 }
211
212 public override uint LocalID
213 {
214 set { m_localID = value; }
215 }
216
217 public override bool Grabbed
218 {
219 set { return; }
220 }
221
222 public override bool Selected
223 {
224 set { return; }
225 }
226
227 public override float Buoyancy
228 {
229 get { return m_buoyancy; }
230 set { m_buoyancy = value; }
231 }
232
233 public override bool FloatOnWater
234 {
235 set { return; }
236 }
237
238 public override bool IsPhysical
239 {
240 get { return false; }
241 set { return; }
242 }
243
244 public override bool ThrottleUpdates
245 {
246 get { return false; }
247 set { return; }
248 }
249
250 public override bool Flying
251 {
252 get { return flying; }
253 set
254 {
255 flying = value;
256 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
257 }
258 }
259
260 /// <summary>
261 /// Returns if the avatar is colliding in general.
262 /// This includes the ground and objects and avatar.
263 /// </summary>
264 public override bool IsColliding
265 {
266 get { return (m_iscolliding || m_iscollidingGround); }
267 set
268 {
269 if (value)
270 {
271 m_colliderfilter += 2;
272 if (m_colliderfilter > 2)
273 m_colliderfilter = 2;
274 }
275 else
276 {
277 m_colliderfilter--;
278 if (m_colliderfilter < 0)
279 m_colliderfilter = 0;
280 }
281
282 if (m_colliderfilter == 0)
283 m_iscolliding = false;
284 else
285 {
286// SetPidStatus(false);
287 m_pidControllerActive = true;
288 m_iscolliding = true;
289 }
290 }
291 }
292
293 /// <summary>
294 /// Returns if an avatar is colliding with the ground
295 /// </summary>
296 public override bool CollidingGround
297 {
298 get { return m_iscollidingGround; }
299 set
300 {
301 /* we now control this
302 if (value)
303 {
304 m_colliderGroundfilter += 2;
305 if (m_colliderGroundfilter > 2)
306 m_colliderGroundfilter = 2;
307 }
308 else
309 {
310 m_colliderGroundfilter--;
311 if (m_colliderGroundfilter < 0)
312 m_colliderGroundfilter = 0;
313 }
314
315 if (m_colliderGroundfilter == 0)
316 m_iscollidingGround = false;
317 else
318 m_iscollidingGround = true;
319 */
320 }
321
322 }
323
324 /// <summary>
325 /// Returns if the avatar is colliding with an object
326 /// </summary>
327 public override bool CollidingObj
328 {
329 get { return m_iscollidingObj; }
330 set
331 {
332 // Ubit filter this also
333 if (value)
334 {
335 m_colliderObjectfilter += 2;
336 if (m_colliderObjectfilter > 2)
337 m_colliderObjectfilter = 2;
338 }
339 else
340 {
341 m_colliderObjectfilter--;
342 if (m_colliderObjectfilter < 0)
343 m_colliderObjectfilter = 0;
344 }
345
346 if (m_colliderObjectfilter == 0)
347 m_iscollidingObj = false;
348 else
349 m_iscollidingObj = true;
350
351 // m_iscollidingObj = value;
352/*
353 if (m_iscollidingObj)
354 m_pidControllerActive = false;
355 else
356 m_pidControllerActive = true;
357 */
358 }
359 }
360
361 /// <summary>
362 /// turn the PID controller on or off.
363 /// The PID Controller will turn on all by itself in many situations
364 /// </summary>
365 /// <param name="status"></param>
366 public void SetPidStatus(bool status)
367 {
368 m_pidControllerActive = status;
369 }
370
371 public override bool Stopped
372 {
373 get { return _zeroFlag; }
374 }
375
376 /// <summary>
377 /// This 'puts' an avatar somewhere in the physics space.
378 /// Not really a good choice unless you 'know' it's a good
379 /// spot otherwise you're likely to orbit the avatar.
380 /// </summary>
381 public override Vector3 Position
382 {
383 get { return _position; }
384 set
385 {
386 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
387 {
388 if (value.IsFinite())
389 {
390 if (value.Z > 9999999f)
391 {
392 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
393 }
394 if (value.Z < -100f)
395 {
396 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
397 }
398 AddChange(changes.Position, value);
399 }
400 else
401 {
402 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
403 }
404 }
405 }
406 }
407
408 public override Vector3 RotationalVelocity
409 {
410 get { return m_rotationalVelocity; }
411 set { m_rotationalVelocity = value; }
412 }
413
414 /// <summary>
415 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
416 /// and use it to offset landings properly
417 /// </summary>
418 public override Vector3 Size
419 {
420 get {
421 float d = CAPSULE_RADIUS * 2;
422 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
423 set
424 {
425 if (value.IsFinite())
426 {
427 AddChange(changes.Size, value);
428 }
429 else
430 {
431 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
432 }
433 }
434 }
435
436 /// <summary>
437 /// This creates the Avatar's physical Surrogate at the position supplied
438 /// </summary>
439 /// <param name="npositionX"></param>
440 /// <param name="npositionY"></param>
441 /// <param name="npositionZ"></param>
442
443 //
444 /// <summary>
445 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
446 /// This may be used in calculations in the scene/scenepresence
447 /// </summary>
448 public override float Mass
449 {
450 get
451 {
452 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
453 return m_density * AVvolume;
454 }
455 }
456 public override void link(PhysicsActor obj)
457 {
458
459 }
460
461 public override void delink()
462 {
463
464 }
465
466 public override void LockAngularMotion(Vector3 axis)
467 {
468
469 }
470
471
472 public override Vector3 Force
473 {
474 get { return _target_velocity; }
475 set { return; }
476 }
477
478 public override int VehicleType
479 {
480 get { return 0; }
481 set { return; }
482 }
483
484 public override void VehicleFloatParam(int param, float value)
485 {
486
487 }
488
489 public override void VehicleVectorParam(int param, Vector3 value)
490 {
491
492 }
493
494 public override void VehicleRotationParam(int param, Quaternion rotation)
495 {
496
497 }
498
499 public override void VehicleFlags(int param, bool remove)
500 {
501
502 }
503
504 public override void SetVolumeDetect(int param)
505 {
506
507 }
508
509 public override Vector3 CenterOfMass
510 {
511 get
512 {
513 Vector3 pos = _position;
514 return pos;
515 }
516 }
517
518 public override Vector3 GeometricCenter
519 {
520 get
521 {
522 Vector3 pos = _position;
523 return pos;
524 }
525 }
526
527 //UBit mess
528 /* for later use
529 public override Vector3 PrimOOBsize
530 {
531 get
532 {
533 Vector3 s=Size;
534 s.X *=0.5f;
535 s.Y *=0.5f;
536 s.Z *=0.5f;
537 return s;
538 }
539 }
540
541 public override Vector3 PrimOOBoffset
542 {
543 get
544 {
545 return Vector3.Zero;
546 }
547 }
548 */
549
550 public override PrimitiveBaseShape Shape
551 {
552 set { return; }
553 }
554
555 public override Vector3 Velocity
556 {
557 get
558 {
559 return _velocity;
560 }
561 set
562 {
563 if (value.IsFinite())
564 {
565 AddChange(changes.Velocity, value);
566 }
567 else
568 {
569 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
570 }
571 }
572 }
573
574 public override Vector3 Torque
575 {
576 get { return Vector3.Zero; }
577 set { return; }
578 }
579
580 public override float CollisionScore
581 {
582 get { return 0f; }
583 set { }
584 }
585
586 public override bool Kinematic
587 {
588 get { return false; }
589 set { }
590 }
591
592 public override Quaternion Orientation
593 {
594 get { return Quaternion.Identity; }
595 set
596 {
597 }
598 }
599
600 public override Vector3 Acceleration
601 {
602 get { return _acceleration; }
603 set { }
604 }
605
606 public void SetAcceleration(Vector3 accel)
607 {
608 m_pidControllerActive = true;
609 _acceleration = accel;
610 }
611
612 /// <summary>
613 /// Adds the force supplied to the Target Velocity
614 /// The PID controller takes this target velocity and tries to make it a reality
615 /// </summary>
616 /// <param name="force"></param>
617 public override void AddForce(Vector3 force, bool pushforce)
618 {
619 if (force.IsFinite())
620 {
621 if (pushforce)
622 {
623 AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f);
624 }
625 else
626 {
627 AddChange(changes.Velocity, force);
628 }
629 }
630 else
631 {
632 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
633 }
634 //m_lastUpdateSent = false;
635 }
636
637 public override void AddAngularForce(Vector3 force, bool pushforce)
638 {
639
640 }
641
642 public override void SetMomentum(Vector3 momentum)
643 {
644 }
645
646
647 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
648 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
649 // place that is safe to call this routine AvatarGeomAndBodyCreation.
650 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
651 {
652 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
653 if (CAPSULE_LENGTH <= 0)
654 {
655 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
656 CAPSULE_LENGTH = 0.01f;
657
658 }
659
660 if (CAPSULE_RADIUS <= 0)
661 {
662 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
663 CAPSULE_RADIUS = 0.01f;
664
665 }
666 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
667
668 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
669 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
670
671 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
672
673 m_mass = ShellMass.mass; // update mass
674
675 // rescale PID parameters
676 PID_D = _parent_scene.avPIDD;
677 PID_P = _parent_scene.avPIDP;
678
679 // rescale PID parameters so that this aren't affected by mass
680 // and so don't get unstable for some masses
681 // also scale by ode time step so you don't need to refix them
682
683 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
684 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
685 PID_P /= 50 * 80;
686 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
687
688 Body = d.BodyCreate(_parent_scene.world);
689
690 d.BodySetAutoDisableFlag(Body, false);
691 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
692
693 _position.X = npositionX;
694 _position.Y = npositionY;
695 _position.Z = npositionZ;
696
697 d.BodySetMass(Body, ref ShellMass);
698 d.GeomSetBody(Shell, Body);
699
700 // The purpose of the AMotor here is to keep the avatar's physical
701 // surrogate from rotating while moving
702 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
703 d.JointAttach(Amotor, Body, IntPtr.Zero);
704
705 d.JointSetAMotorMode(Amotor, 0);
706 d.JointSetAMotorNumAxes(Amotor, 3);
707 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
708 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
709 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
710
711 d.JointSetAMotorAngle(Amotor, 0, 0);
712 d.JointSetAMotorAngle(Amotor, 1, 0);
713 d.JointSetAMotorAngle(Amotor, 2, 0);
714
715 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
716 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
717 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
718 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
719 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
720 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
721
722 // These lowstops and high stops are effectively (no wiggle room)
723 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
724 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
725 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
726 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
727 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
728 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
729
730 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
731 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
732 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
733
734 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f);
735 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f);
736 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f);
737 }
738
739 /// <summary>
740 /// Destroys the avatar body and geom
741
742 private void AvatarGeomAndBodyDestroy()
743 {
744 // Kill the Amotor
745 if (Amotor != IntPtr.Zero)
746 {
747 d.JointDestroy(Amotor);
748 Amotor = IntPtr.Zero;
749 }
750
751 if (Body != IntPtr.Zero)
752 {
753 //kill the body
754 d.BodyDestroy(Body);
755 Body = IntPtr.Zero;
756 }
757
758 //kill the Geometry
759 if (Shell != IntPtr.Zero)
760 {
761 _parent_scene.geom_name_map.Remove(Shell);
762 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
763 d.GeomDestroy(Shell);
764 _parent_scene.geom_name_map.Remove(Shell);
765 Shell = IntPtr.Zero;
766 }
767 }
768
769 /// <summary>
770 /// Called from Simulate
771 /// This is the avatar's movement control + PID Controller
772 /// </summary>
773 /// <param name="timeStep"></param>
774 public void Move(float timeStep, List<OdeCharacter> defects)
775 {
776 // no lock; for now it's only called from within Simulate()
777
778 // If the PID Controller isn't active then we set our force
779 // calculating base velocity to the current position
780
781 if (Body == IntPtr.Zero)
782 return;
783
784 d.Vector3 dtmp;
785 d.BodyCopyPosition(Body, out dtmp);
786 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
787
788 // the Amotor still lets avatar rotation to drift during colisions
789 // so force it back to identity
790
791 d.Quaternion qtmp;
792 qtmp.W = 1;
793 qtmp.X = 0;
794 qtmp.Y = 0;
795 qtmp.Z = 0;
796 d.BodySetQuaternion(Body, ref qtmp);
797
798 if (m_pidControllerActive == false)
799 {
800 _zeroPosition = localpos;
801 }
802 //PidStatus = true;
803
804
805 if (!localpos.IsFinite())
806 {
807
808 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
809 defects.Add(this);
810 // _parent_scene.RemoveCharacter(this);
811
812 // destroy avatar capsule and related ODE data
813 AvatarGeomAndBodyDestroy();
814
815 return;
816 }
817
818 Vector3 vec = Vector3.Zero;
819 dtmp = d.BodyGetLinearVel(Body);
820 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
821
822 float movementdivisor = 1f;
823 //Ubit change divisions into multiplications below
824 if (!m_alwaysRun)
825 {
826 movementdivisor = 1 / walkDivisor;
827 }
828 else
829 {
830 movementdivisor = 1 / runDivisor;
831 }
832
833 // colide with land
834
835 d.AABB aabb;
836 d.GeomGetAABB(Shell, out aabb);
837 float chrminZ = aabb.MinZ;
838
839 Vector3 posch = localpos;
840
841 float ftmp;
842
843 if (flying)
844 {
845 ftmp = timeStep;
846 posch.X += vel.X * ftmp;
847 posch.Y += vel.Y * ftmp;
848 }
849
850 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
851 if (chrminZ < terrainheight)
852 {
853 float depth = terrainheight - chrminZ;
854 if (!flying)
855 {
856 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
857 }
858 else
859 vec.Z = depth * PID_P * 50;
860
861 /*
862 Vector3 vtmp;
863 vtmp.X = _target_velocity.X * timeStep;
864 vtmp.Y = _target_velocity.Y * timeStep;
865 // fake and avoid squares
866 float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
867 if (k > 0)
868 {
869 posch.X += vtmp.X;
870 posch.Y += vtmp.Y;
871 terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
872 k = 1 + Math.Abs(terrainheight) / k;
873 movementdivisor /= k;
874
875 if (k < 1)
876 k = 1;
877 }
878 */
879
880
881 if (depth < 0.1f)
882 {
883 m_iscolliding = true;
884 m_colliderfilter = 2;
885 m_iscollidingGround = true;
886
887 ContactPoint contact = new ContactPoint();
888 contact.PenetrationDepth = depth;
889 contact.Position.X = localpos.X;
890 contact.Position.Y = localpos.Y;
891 contact.Position.Z = chrminZ;
892 contact.SurfaceNormal.X = 0f;
893 contact.SurfaceNormal.Y = 0f;
894 contact.SurfaceNormal.Z = -1f;
895 AddCollisionEvent(0, contact);
896
897 vec.Z *= 0.5f;
898 }
899
900 else
901 m_iscollidingGround = false;
902 }
903 else
904 m_iscollidingGround = false;
905
906
907 // if velocity is zero, use position control; otherwise, velocity control
908 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
909 && m_iscolliding)
910 {
911 // keep track of where we stopped. No more slippin' & slidin'
912 if (!_zeroFlag)
913 {
914 _zeroFlag = true;
915 _zeroPosition = localpos;
916 }
917 if (m_pidControllerActive)
918 {
919 // We only want to deactivate the PID Controller if we think we want to have our surrogate
920 // react to the physics scene by moving it's position.
921 // Avatar to Avatar collisions
922 // Prim to avatar collisions
923
924 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
925 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
926 if (flying)
927 {
928 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
929 }
930 }
931 //PidStatus = true;
932 }
933 else
934 {
935 m_pidControllerActive = true;
936 _zeroFlag = false;
937
938 if (m_iscolliding)
939 {
940 if (!flying)
941 {
942 if (_target_velocity.Z > 0.0f)
943 {
944 // We're colliding with something and we're not flying but we're moving
945 // This means we're walking or running. JUMPING
946 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
947 }
948 // We're standing on something
949 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
950 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
951 }
952 else
953 {
954 // We're flying and colliding with something
955 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
956 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
957 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
958 }
959 }
960 else // ie not colliding
961 {
962 if (flying) //(!m_iscolliding && flying)
963 {
964 // we're in mid air suspended
965 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
966 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
967 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
968 }
969
970 else
971 {
972 // we're not colliding and we're not flying so that means we're falling!
973 // m_iscolliding includes collisions with the ground.
974
975 // d.Vector3 pos = d.BodyGetPosition(Body);
976 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
977 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
978 }
979 }
980 }
981
982 if (flying)
983 {
984 vec.Z -= _parent_scene.gravityz * m_mass;
985
986 //Added for auto fly height. Kitto Flora
987 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
988
989 if (localpos.Z < target_altitude)
990 {
991 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
992 }
993 // end add Kitto Flora
994 }
995
996 if (vec.IsFinite())
997 {
998 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
999 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1000 }
1001 else
1002 {
1003 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1004 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1005 defects.Add(this);
1006 // _parent_scene.RemoveCharacter(this);
1007 // destroy avatar capsule and related ODE data
1008 AvatarGeomAndBodyDestroy();
1009 }
1010 }
1011
1012 /// <summary>
1013 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1014 /// </summary>
1015 public void UpdatePositionAndVelocity()
1016 {
1017 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1018 if (Body == IntPtr.Zero)
1019 return;
1020
1021 d.Vector3 vec;
1022 try
1023 {
1024 d.BodyCopyPosition(Body, out vec);
1025 }
1026 catch (NullReferenceException)
1027 {
1028 bad = true;
1029 _parent_scene.BadCharacter(this);
1030 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1031 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1032 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1033 }
1034
1035 _position.X = vec.X;
1036 _position.Y = vec.Y;
1037 _position.Z = vec.Z;
1038
1039 bool fixbody = false;
1040
1041 if (_position.X < 0.0f)
1042 {
1043 fixbody = true;
1044 _position.X = 0.1f;
1045 }
1046 else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
1047 {
1048 fixbody = true;
1049 _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
1050 }
1051
1052 if (_position.Y < 0.0f)
1053 {
1054 fixbody = true;
1055 _position.Y = 0.1f;
1056 }
1057 else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
1058 {
1059 fixbody = true;
1060 _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
1061 }
1062
1063 if (fixbody)
1064 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1065
1066 // Did we move last? = zeroflag
1067 // This helps keep us from sliding all over
1068/*
1069 if (_zeroFlag)
1070 {
1071 _velocity.X = 0.0f;
1072 _velocity.Y = 0.0f;
1073 _velocity.Z = 0.0f;
1074
1075 // Did we send out the 'stopped' message?
1076 if (!m_lastUpdateSent)
1077 {
1078 m_lastUpdateSent = true;
1079 base.RequestPhysicsterseUpdate();
1080 }
1081 }
1082 else
1083 {
1084 m_lastUpdateSent = false;
1085 */
1086 try
1087 {
1088 vec = d.BodyGetLinearVel(Body);
1089 }
1090 catch (NullReferenceException)
1091 {
1092 vec.X = _velocity.X;
1093 vec.Y = _velocity.Y;
1094 vec.Z = _velocity.Z;
1095 }
1096 _velocity.X = (vec.X);
1097 _velocity.Y = (vec.Y);
1098 _velocity.Z = (vec.Z);
1099 // }
1100 }
1101
1102 /// <summary>
1103 /// Cleanup the things we use in the scene.
1104 /// </summary>
1105 public void Destroy()
1106 {
1107 AddChange(changes.Remove, null);
1108 }
1109
1110 public override void CrossingFailure()
1111 {
1112 }
1113
1114 public override Vector3 PIDTarget { set { return; } }
1115 public override bool PIDActive { set { return; } }
1116 public override float PIDTau { set { return; } }
1117
1118 public override float PIDHoverHeight { set { return; } }
1119 public override bool PIDHoverActive { set { return; } }
1120 public override PIDHoverType PIDHoverType { set { return; } }
1121 public override float PIDHoverTau { set { return; } }
1122
1123 public override Quaternion APIDTarget { set { return; } }
1124
1125 public override bool APIDActive { set { return; } }
1126
1127 public override float APIDStrength { set { return; } }
1128
1129 public override float APIDDamping { set { return; } }
1130
1131
1132 public override void SubscribeEvents(int ms)
1133 {
1134 m_requestedUpdateFrequency = ms;
1135 m_eventsubscription = ms;
1136 _parent_scene.AddCollisionEventReporting(this);
1137 m_haseventsubscription = true;
1138 }
1139
1140 public override void UnSubscribeEvents()
1141 {
1142 m_haseventsubscription = false;
1143 _parent_scene.RemoveCollisionEventReporting(this);
1144 m_requestedUpdateFrequency = 0;
1145 m_eventsubscription = 0;
1146 }
1147
1148 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1149 {
1150 if (m_haseventsubscription)
1151 {
1152 // m_log.DebugFormat(
1153 // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
1154
1155 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1156 }
1157 }
1158
1159 public void SendCollisions()
1160 {
1161 if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency)
1162 {
1163 if (CollisionEventsThisFrame != null)
1164 {
1165 base.SendCollisionUpdate(CollisionEventsThisFrame);
1166 }
1167 CollisionEventsThisFrame = new CollisionEventUpdate();
1168 m_eventsubscription = 0;
1169 }
1170 }
1171
1172 public override bool SubscribedEvents()
1173 {
1174 return m_haseventsubscription;
1175 }
1176
1177 private void changePhysicsStatus(bool NewStatus)
1178 {
1179 if (NewStatus != m_isPhysical)
1180 {
1181 if (NewStatus)
1182 {
1183 // Create avatar capsule and related ODE data
1184 if ((Shell != IntPtr.Zero))
1185 {
1186 // a lost shell ?
1187 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1188 + (Shell != IntPtr.Zero ? "Shell " : "")
1189 + (Body != IntPtr.Zero ? "Body " : "")
1190 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1191 AvatarGeomAndBodyDestroy();
1192 }
1193
1194 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1195 _parent_scene.geom_name_map[Shell] = m_name;
1196 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1197 _parent_scene.AddCharacter(this);
1198 }
1199 else
1200 {
1201 _parent_scene.RemoveCharacter(this);
1202 // destroy avatar capsule and related ODE data
1203 AvatarGeomAndBodyDestroy();
1204 }
1205
1206 m_isPhysical = NewStatus;
1207 }
1208 }
1209
1210 private void changeAdd()
1211 {
1212 changePhysicsStatus(true);
1213 }
1214
1215 private void changeRemove()
1216 {
1217 changePhysicsStatus(false);
1218 }
1219
1220 private void changeShape(PrimitiveBaseShape arg)
1221 {
1222 }
1223
1224 private void changeSize(Vector3 Size)
1225 {
1226 if (Size.IsFinite())
1227 {
1228 float caplen = Size.Z;
1229
1230 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1231
1232 if (caplen != CAPSULE_LENGTH)
1233 {
1234 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1235 {
1236 AvatarGeomAndBodyDestroy();
1237
1238 float prevCapsule = CAPSULE_LENGTH;
1239 CAPSULE_LENGTH = caplen;
1240
1241 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1242 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
1243
1244 Velocity = Vector3.Zero;
1245
1246 _parent_scene.geom_name_map[Shell] = m_name;
1247 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1248 }
1249 else
1250 {
1251 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1252 + (Shell == IntPtr.Zero ? "Shell " : "")
1253 + (Body == IntPtr.Zero ? "Body " : "")
1254 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1255 }
1256 }
1257
1258 m_pidControllerActive = true;
1259 }
1260 else
1261 {
1262 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1263 }
1264 }
1265
1266 private void changePosition( Vector3 newPos)
1267 {
1268 if (Body != IntPtr.Zero)
1269 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1270 _position = newPos;
1271 }
1272
1273 private void changeOrientation(Quaternion newOri)
1274 {
1275 }
1276
1277 private void changeVelocity(Vector3 newVel)
1278 {
1279 m_pidControllerActive = true;
1280 _target_velocity = newVel;
1281 }
1282
1283 private void changeSetTorque(Vector3 newTorque)
1284 {
1285 }
1286
1287 private void changeAddForce(Vector3 newForce)
1288 {
1289 }
1290
1291 private void changeAddAngularForce(Vector3 arg)
1292 {
1293 }
1294
1295 private void changeAngularLock(Vector3 arg)
1296 {
1297 }
1298
1299 private void changeFloatOnWater(bool arg)
1300 {
1301 }
1302
1303 private void changeVolumedetetion(bool arg)
1304 {
1305 }
1306
1307 private void changeSelectedStatus(bool arg)
1308 {
1309 }
1310
1311 private void changeDisable(bool arg)
1312 {
1313 }
1314
1315 private void changeBuilding(bool arg)
1316 {
1317 }
1318
1319 private void changeForce(Vector3 newForce)
1320 {
1321 m_pidControllerActive = false;
1322 if (Body != IntPtr.Zero)
1323 {
1324 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1325 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1326 }
1327 }
1328
1329 private void donullchange()
1330 {
1331 }
1332
1333 public bool DoAChange(changes what, object arg)
1334 {
1335 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1336 {
1337 return false;
1338 }
1339
1340 // nasty switch
1341 switch (what)
1342 {
1343 case changes.Add:
1344 changeAdd();
1345 break;
1346 case changes.Remove:
1347 changeRemove();
1348 break;
1349
1350 case changes.Position:
1351 changePosition((Vector3)arg);
1352 break;
1353
1354 case changes.Orientation:
1355 changeOrientation((Quaternion)arg);
1356 break;
1357
1358 case changes.PosOffset:
1359 donullchange();
1360 break;
1361
1362 case changes.OriOffset:
1363 donullchange();
1364 break;
1365
1366 case changes.Velocity:
1367 changeVelocity((Vector3)arg);
1368 break;
1369
1370 // case changes.Acceleration:
1371 // changeacceleration((Vector3)arg);
1372 // break;
1373 // case changes.AngVelocity:
1374 // changeangvelocity((Vector3)arg);
1375 // break;
1376
1377 case changes.Force:
1378 changeForce((Vector3)arg);
1379 break;
1380
1381 case changes.Torque:
1382 changeSetTorque((Vector3)arg);
1383 break;
1384
1385 case changes.AddForce:
1386 changeAddForce((Vector3)arg);
1387 break;
1388
1389 case changes.AddAngForce:
1390 changeAddAngularForce((Vector3)arg);
1391 break;
1392
1393 case changes.AngLock:
1394 changeAngularLock((Vector3)arg);
1395 break;
1396
1397 case changes.Size:
1398 changeSize((Vector3)arg);
1399 break;
1400/* not in use for now
1401 case changes.Shape:
1402 changeShape((PrimitiveBaseShape)arg);
1403 break;
1404
1405 case changes.CollidesWater:
1406 changeFloatOnWater((bool)arg);
1407 break;
1408
1409 case changes.VolumeDtc:
1410 changeVolumedetetion((bool)arg);
1411 break;
1412
1413 case changes.Physical:
1414 changePhysicsStatus((bool)arg);
1415 break;
1416
1417 case changes.Selected:
1418 changeSelectedStatus((bool)arg);
1419 break;
1420
1421 case changes.disabled:
1422 changeDisable((bool)arg);
1423 break;
1424
1425 case changes.building:
1426 changeBuilding((bool)arg);
1427 break;
1428*/
1429 case changes.Null:
1430 donullchange();
1431 break;
1432
1433 default:
1434 donullchange();
1435 break;
1436 }
1437 return false;
1438 }
1439
1440 public void AddChange(changes what, object arg)
1441 {
1442 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1443 }
1444
1445
1446 internal void AddCollisionFrameTime(int p)
1447 {
1448 // protect it from overflow crashing
1449 if (m_eventsubscription + p >= int.MaxValue)
1450 m_eventsubscription = 0;
1451 m_eventsubscription += p;
1452 }
1453 }
1454}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..d0b4546
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,993 @@
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 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
67 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
68
69 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
70
71 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
72 // HOVER_TERRAIN_ONLY
73 // HOVER_GLOBAL_HEIGHT
74 // NO_DEFLECTION_UP
75 // HOVER_WATER_ONLY
76 // HOVER_UP_ONLY
77 // LIMIT_MOTOR_UP
78 // LIMIT_ROLL_ONLY
79 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
80
81 // Linear properties
82 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
83 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
84 private float m_linearMotorDecayTimescale = 120;
85 private float m_linearMotorTimescale = 1000;
86 private Vector3 m_linearMotorOffset = Vector3.Zero;
87
88 //Angular properties
89 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
90 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
91 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
92 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
93
94 //Deflection properties
95 private float m_angularDeflectionEfficiency = 0;
96 private float m_angularDeflectionTimescale = 1000;
97 private float m_linearDeflectionEfficiency = 0;
98 private float m_linearDeflectionTimescale = 1000;
99
100 //Banking properties
101 private float m_bankingEfficiency = 0;
102 private float m_bankingMix = 0;
103 private float m_bankingTimescale = 1000;
104
105 //Hover and Buoyancy properties
106 private float m_VhoverHeight = 0f;
107 private float m_VhoverEfficiency = 0f;
108 private float m_VhoverTimescale = 1000f;
109 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
110 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
111 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
112 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
113
114 //Attractor properties
115 private float m_verticalAttractionEfficiency = 1.0f; // damped
116 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
117
118
119 // auxiliar
120 private float m_lmEfect = 0; // current linear motor eficiency
121 private float m_amEfect = 0; // current angular motor eficiency
122 private float m_ffactor = 1.0f;
123
124 public float FrictionFactor
125 {
126 get
127 {
128 return m_ffactor;
129 }
130 }
131
132 public ODEDynamics(OdePrim rootp)
133 {
134 rootPrim = rootp;
135 _pParentScene = rootPrim._parent_scene;
136 }
137
138 public void DoSetVehicle(VehicleData vd)
139 {
140
141 float timestep = _pParentScene.ODE_STEPSIZE;
142 float invtimestep = 1.0f / timestep;
143
144 m_type = vd.m_type;
145 m_flags = vd.m_flags;
146
147 // Linear properties
148 m_linearMotorDirection = vd.m_linearMotorDirection;
149
150 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
151 if (m_linearFrictionTimescale.X < timestep) m_linearFrictionTimescale.X = timestep;
152 if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep;
153 if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep;
154
155 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
156 if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f;
157 m_linearMotorDecayTimescale *= invtimestep;
158
159 m_linearMotorTimescale = vd.m_linearMotorTimescale;
160 if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep;
161
162
163 m_linearMotorOffset = vd.m_linearMotorOffset;
164
165 //Angular properties
166 m_angularMotorDirection = vd.m_angularMotorDirection;
167 m_angularMotorTimescale = vd.m_angularMotorTimescale;
168 if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep;
169
170 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
171 if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f;
172 m_angularMotorDecayTimescale *= invtimestep;
173
174 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
175 if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep;
176 if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep;
177 if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep;
178
179 //Deflection properties
180 m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
181 m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
182 if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep;
183
184 m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
185 m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
186 if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep;
187
188 //Banking properties
189 m_bankingEfficiency = vd.m_bankingEfficiency;
190 m_bankingMix = vd.m_bankingMix;
191 m_bankingTimescale = vd.m_bankingTimescale;
192 if (m_bankingTimescale < timestep) m_bankingTimescale = timestep;
193
194 //Hover and Buoyancy properties
195 m_VhoverHeight = vd.m_VhoverHeight;
196 m_VhoverEfficiency = vd.m_VhoverEfficiency;
197 m_VhoverTimescale = vd.m_VhoverTimescale;
198 if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep;
199
200 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
201
202 //Attractor properties
203 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
204 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
205 if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep;
206
207 // Axis
208 m_referenceFrame = vd.m_referenceFrame;
209
210 m_lmEfect = 0;
211 m_amEfect = 0;
212 m_ffactor = 1.0f;
213 }
214
215 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
216 {
217 float len;
218 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
219 float timestep = _pParentScene.ODE_STEPSIZE;
220
221 switch (pParam)
222 {
223 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
224 if (pValue < 0f) pValue = 0f;
225 if (pValue > 1f) pValue = 1f;
226 m_angularDeflectionEfficiency = pValue;
227 break;
228 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
229 if (pValue < timestep) pValue = timestep;
230 m_angularDeflectionTimescale = pValue;
231 break;
232 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
233 // if (pValue < timestep) pValue = timestep;
234 // try to make impulses to work a bit better
235 if (pValue < 0.5f) pValue = 0.5f;
236 else if (pValue > 120) pValue = 120;
237 m_angularMotorDecayTimescale = pValue * invtimestep;
238 break;
239 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
240 if (pValue < timestep) pValue = timestep;
241 m_angularMotorTimescale = pValue;
242 break;
243 case Vehicle.BANKING_EFFICIENCY:
244 if (pValue < -1f) pValue = -1f;
245 if (pValue > 1f) pValue = 1f;
246 m_bankingEfficiency = pValue;
247 break;
248 case Vehicle.BANKING_MIX:
249 if (pValue < 0f) pValue = 0f;
250 if (pValue > 1f) pValue = 1f;
251 m_bankingMix = pValue;
252 break;
253 case Vehicle.BANKING_TIMESCALE:
254 if (pValue < timestep) pValue = timestep;
255 m_bankingTimescale = pValue;
256 break;
257 case Vehicle.BUOYANCY:
258 if (pValue < -1f) pValue = -1f;
259 if (pValue > 1f) pValue = 1f;
260 m_VehicleBuoyancy = pValue;
261 break;
262 case Vehicle.HOVER_EFFICIENCY:
263 if (pValue < 0f) pValue = 0f;
264 if (pValue > 1f) pValue = 1f;
265 m_VhoverEfficiency = pValue;
266 break;
267 case Vehicle.HOVER_HEIGHT:
268 m_VhoverHeight = pValue;
269 break;
270 case Vehicle.HOVER_TIMESCALE:
271 if (pValue < timestep) pValue = timestep;
272 m_VhoverTimescale = pValue;
273 break;
274 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
275 if (pValue < 0f) pValue = 0f;
276 if (pValue > 1f) pValue = 1f;
277 m_linearDeflectionEfficiency = pValue;
278 break;
279 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
280 if (pValue < timestep) pValue = timestep;
281 m_linearDeflectionTimescale = pValue;
282 break;
283 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
284 // if (pValue < timestep) pValue = timestep;
285 // try to make impulses to work a bit better
286 if (pValue < 0.5f) pValue = 0.5f;
287 else if (pValue > 120) pValue = 120;
288 m_linearMotorDecayTimescale = pValue * invtimestep;
289 break;
290 case Vehicle.LINEAR_MOTOR_TIMESCALE:
291 if (pValue < timestep) pValue = timestep;
292 m_linearMotorTimescale = pValue;
293 break;
294 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
295 if (pValue < 0f) pValue = 0f;
296 if (pValue > 1f) pValue = 1f;
297 m_verticalAttractionEfficiency = pValue;
298 break;
299 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
300 if (pValue < timestep) pValue = timestep;
301 m_verticalAttractionTimescale = pValue;
302 break;
303
304 // These are vector properties but the engine lets you use a single float value to
305 // set all of the components to the same value
306 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
307 if (pValue < timestep) pValue = timestep;
308 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
309 break;
310 case Vehicle.ANGULAR_MOTOR_DIRECTION:
311 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
312 len = m_angularMotorDirection.Length();
313 if (len > 12.566f)
314 m_angularMotorDirection *= (12.566f / len);
315 m_amEfect = 1.0f; // turn it on
316 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
317 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
318 d.BodyEnable(rootPrim.Body);
319 break;
320 case Vehicle.LINEAR_FRICTION_TIMESCALE:
321 if (pValue < timestep) pValue = timestep;
322 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
323 break;
324 case Vehicle.LINEAR_MOTOR_DIRECTION:
325 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
326 len = m_linearMotorDirection.Length();
327 if (len > 30.0f)
328 m_linearMotorDirection *= (30.0f / len);
329 m_lmEfect = 1.0f; // turn it on
330 m_ffactor = 0.01f;
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_MOTOR_OFFSET:
336 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
337 len = m_linearMotorOffset.Length();
338 if (len > 100.0f)
339 m_linearMotorOffset *= (100.0f / len);
340 break;
341 }
342 }//end ProcessFloatVehicleParam
343
344 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
345 {
346 float len;
347 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
348 float timestep = _pParentScene.ODE_STEPSIZE;
349 switch (pParam)
350 {
351 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
352 if (pValue.X < timestep) pValue.X = timestep;
353 if (pValue.Y < timestep) pValue.Y = timestep;
354 if (pValue.Z < timestep) pValue.Z = timestep;
355
356 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
357 break;
358 case Vehicle.ANGULAR_MOTOR_DIRECTION:
359 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
360 // Limit requested angular speed to 2 rps= 4 pi rads/sec
361 len = m_angularMotorDirection.Length();
362 if (len > 12.566f)
363 m_angularMotorDirection *= (12.566f / len);
364 m_amEfect = 1.0f; // turn it on
365 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
366 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
367 d.BodyEnable(rootPrim.Body);
368 break;
369 case Vehicle.LINEAR_FRICTION_TIMESCALE:
370 if (pValue.X < timestep) pValue.X = timestep;
371 if (pValue.Y < timestep) pValue.Y = timestep;
372 if (pValue.Z < timestep) pValue.Z = timestep;
373 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
374 break;
375 case Vehicle.LINEAR_MOTOR_DIRECTION:
376 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
377 len = m_linearMotorDirection.Length();
378 if (len > 30.0f)
379 m_linearMotorDirection *= (30.0f / len);
380 m_lmEfect = 1.0f; // turn it on
381 m_ffactor = 0.01f;
382 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
383 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
384 d.BodyEnable(rootPrim.Body);
385 break;
386 case Vehicle.LINEAR_MOTOR_OFFSET:
387 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
388 len = m_linearMotorOffset.Length();
389 if (len > 100.0f)
390 m_linearMotorOffset *= (100.0f / len);
391 break;
392 case Vehicle.BLOCK_EXIT:
393 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
394 break;
395 }
396 }//end ProcessVectorVehicleParam
397
398 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
399 {
400 switch (pParam)
401 {
402 case Vehicle.REFERENCE_FRAME:
403 m_referenceFrame = Quaternion.Inverse(pValue);
404 break;
405 case Vehicle.ROLL_FRAME:
406 m_RollreferenceFrame = pValue;
407 break;
408 }
409 }//end ProcessRotationVehicleParam
410
411 internal void ProcessVehicleFlags(int pParam, bool remove)
412 {
413 if (remove)
414 {
415 m_flags &= ~((VehicleFlag)pParam);
416 }
417 else
418 {
419 m_flags |= (VehicleFlag)pParam;
420 }
421 }//end ProcessVehicleFlags
422
423 internal void ProcessTypeChange(Vehicle pType)
424 {
425 float invtimestep = _pParentScene.ODE_STEPSIZE;
426 m_lmEfect = 0;
427 m_amEfect = 0;
428 m_ffactor = 1f;
429
430 m_linearMotorDirection = Vector3.Zero;
431 m_angularMotorDirection = Vector3.Zero;
432
433 m_BlockingEndPoint = Vector3.Zero;
434 m_RollreferenceFrame = Quaternion.Identity;
435 m_linearMotorOffset = Vector3.Zero;
436
437 m_referenceFrame = Quaternion.Identity;
438
439 // Set Defaults For Type
440 m_type = pType;
441 switch (pType)
442 {
443 case Vehicle.TYPE_NONE:
444 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
445 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
446 m_linearMotorTimescale = 1000;
447 m_linearMotorDecayTimescale = 120;
448 m_angularMotorTimescale = 1000;
449 m_angularMotorDecayTimescale = 1000;
450 m_VhoverHeight = 0;
451 m_VhoverEfficiency = 1;
452 m_VhoverTimescale = 1000;
453 m_VehicleBuoyancy = 0;
454 m_linearDeflectionEfficiency = 0;
455 m_linearDeflectionTimescale = 1000;
456 m_angularDeflectionEfficiency = 0;
457 m_angularDeflectionTimescale = 1000;
458 m_bankingEfficiency = 0;
459 m_bankingMix = 1;
460 m_bankingTimescale = 1000;
461 m_verticalAttractionEfficiency = 0;
462 m_verticalAttractionTimescale = 1000;
463
464 m_flags = (VehicleFlag)0;
465 break;
466
467 case Vehicle.TYPE_SLED:
468 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
469 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
470 m_linearMotorTimescale = 1000;
471 m_linearMotorDecayTimescale = 120 * invtimestep;
472 m_angularMotorTimescale = 1000;
473 m_angularMotorDecayTimescale = 120 * invtimestep;
474 m_VhoverHeight = 0;
475 m_VhoverEfficiency = 1;
476 m_VhoverTimescale = 10;
477 m_VehicleBuoyancy = 0;
478 m_linearDeflectionEfficiency = 1;
479 m_linearDeflectionTimescale = 1;
480 m_angularDeflectionEfficiency = 0;
481 m_angularDeflectionTimescale = 1000;
482 m_bankingEfficiency = 0;
483 m_bankingMix = 1;
484 m_bankingTimescale = 10;
485 m_flags &=
486 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
487 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
488 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
489 break;
490 case Vehicle.TYPE_CAR:
491 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
492 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
493 m_linearMotorTimescale = 1;
494 m_linearMotorDecayTimescale = 60 * invtimestep;
495 m_angularMotorTimescale = 1;
496 m_angularMotorDecayTimescale = 0.8f * invtimestep;
497 m_VhoverHeight = 0;
498 m_VhoverEfficiency = 0;
499 m_VhoverTimescale = 1000;
500 m_VehicleBuoyancy = 0;
501 m_linearDeflectionEfficiency = 1;
502 m_linearDeflectionTimescale = 2;
503 m_angularDeflectionEfficiency = 0;
504 m_angularDeflectionTimescale = 10;
505 m_verticalAttractionEfficiency = 1f;
506 m_verticalAttractionTimescale = 10f;
507 m_bankingEfficiency = -0.2f;
508 m_bankingMix = 1;
509 m_bankingTimescale = 1;
510 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
511 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
512 VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
513 break;
514 case Vehicle.TYPE_BOAT:
515 m_linearFrictionTimescale = new Vector3(10, 3, 2);
516 m_angularFrictionTimescale = new Vector3(10, 10, 10);
517 m_linearMotorTimescale = 5;
518 m_linearMotorDecayTimescale = 60 * invtimestep;
519 m_angularMotorTimescale = 4;
520 m_angularMotorDecayTimescale = 4 * invtimestep;
521 m_VhoverHeight = 0;
522 m_VhoverEfficiency = 0.5f;
523 m_VhoverTimescale = 2;
524 m_VehicleBuoyancy = 1;
525 m_linearDeflectionEfficiency = 0.5f;
526 m_linearDeflectionTimescale = 3;
527 m_angularDeflectionEfficiency = 0.5f;
528 m_angularDeflectionTimescale = 5;
529 m_verticalAttractionEfficiency = 0.5f;
530 m_verticalAttractionTimescale = 5f;
531 m_bankingEfficiency = -0.3f;
532 m_bankingMix = 0.8f;
533 m_bankingTimescale = 1;
534 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
535 VehicleFlag.HOVER_GLOBAL_HEIGHT |
536 VehicleFlag.HOVER_UP_ONLY |
537 VehicleFlag.LIMIT_ROLL_ONLY);
538 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
539 VehicleFlag.LIMIT_MOTOR_UP |
540 VehicleFlag.HOVER_WATER_ONLY);
541 break;
542 case Vehicle.TYPE_AIRPLANE:
543 m_linearFrictionTimescale = new Vector3(200, 10, 5);
544 m_angularFrictionTimescale = new Vector3(20, 20, 20);
545 m_linearMotorTimescale = 2;
546 m_linearMotorDecayTimescale = 60 * invtimestep;
547 m_angularMotorTimescale = 4;
548 m_angularMotorDecayTimescale = 8 * invtimestep;
549 m_VhoverHeight = 0;
550 m_VhoverEfficiency = 0.5f;
551 m_VhoverTimescale = 1000;
552 m_VehicleBuoyancy = 0;
553 m_linearDeflectionEfficiency = 0.5f;
554 m_linearDeflectionTimescale = 0.5f;
555 m_angularDeflectionEfficiency = 1;
556 m_angularDeflectionTimescale = 2;
557 m_verticalAttractionEfficiency = 0.9f;
558 m_verticalAttractionTimescale = 2f;
559 m_bankingEfficiency = 1;
560 m_bankingMix = 0.7f;
561 m_bankingTimescale = 2;
562 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
563 VehicleFlag.HOVER_TERRAIN_ONLY |
564 VehicleFlag.HOVER_GLOBAL_HEIGHT |
565 VehicleFlag.HOVER_UP_ONLY |
566 VehicleFlag.NO_DEFLECTION_UP |
567 VehicleFlag.LIMIT_MOTOR_UP);
568 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
569 break;
570 case Vehicle.TYPE_BALLOON:
571 m_linearFrictionTimescale = new Vector3(5, 5, 5);
572 m_angularFrictionTimescale = new Vector3(10, 10, 10);
573 m_linearMotorTimescale = 5;
574 m_linearMotorDecayTimescale = 60 * invtimestep;
575 m_angularMotorTimescale = 6;
576 m_angularMotorDecayTimescale = 10 * invtimestep;
577 m_VhoverHeight = 5;
578 m_VhoverEfficiency = 0.8f;
579 m_VhoverTimescale = 10;
580 m_VehicleBuoyancy = 1;
581 m_linearDeflectionEfficiency = 0;
582 m_linearDeflectionTimescale = 5 * invtimestep;
583 m_angularDeflectionEfficiency = 0;
584 m_angularDeflectionTimescale = 5;
585 m_verticalAttractionEfficiency = 0f;
586 m_verticalAttractionTimescale = 1000f;
587 m_bankingEfficiency = 0;
588 m_bankingMix = 0.7f;
589 m_bankingTimescale = 5;
590 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
591 VehicleFlag.HOVER_TERRAIN_ONLY |
592 VehicleFlag.HOVER_UP_ONLY |
593 VehicleFlag.NO_DEFLECTION_UP |
594 VehicleFlag.LIMIT_MOTOR_UP);
595 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
596 VehicleFlag.HOVER_GLOBAL_HEIGHT);
597 break;
598 }
599
600 }//end SetDefaultsForType
601
602 internal void Stop()
603 {
604 m_lmEfect = 0;
605 m_amEfect = 0;
606 m_ffactor = 1f;
607 }
608
609 public static Vector3 Xrot(Quaternion rot)
610 {
611 Vector3 vec;
612 rot.Normalize(); // just in case
613 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
614 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
615 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
616 return vec;
617 }
618
619 public static Vector3 Zrot(Quaternion rot)
620 {
621 Vector3 vec;
622 rot.Normalize(); // just in case
623 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
624 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
625 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
626
627 return vec;
628 }
629
630 private const float pi = (float)Math.PI;
631 private const float halfpi = 0.5f * (float)Math.PI;
632
633 public static Vector3 ubitRot2Euler(Quaternion rot)
634 {
635 // returns roll in X
636 // pitch in Y
637 // yaw in Z
638 Vector3 vec;
639
640 // assuming rot is normalised
641 // rot.Normalize();
642
643 float zX = rot.X * rot.Z + rot.Y * rot.W;
644
645 if (zX < -0.49999f)
646 {
647 vec.X = 0;
648 vec.Y = -halfpi;
649 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
650 }
651 else if (zX > 0.49999f)
652 {
653 vec.X = 0;
654 vec.Y = halfpi;
655 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
656 }
657 else
658 {
659 vec.Y = (float)Math.Asin(2 * zX);
660
661 float sqw = rot.W * rot.W;
662
663 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
664 float zZ = rot.Z * rot.Z + sqw - 0.5f;
665
666 vec.X = (float)Math.Atan2(minuszY, zZ);
667
668 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
669 float yY = rot.X * rot.X + sqw - 0.5f;
670 vec.Z = (float)Math.Atan2(yX, yY);
671 }
672 return vec;
673 }
674
675 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
676 {
677 // assuming rot is normalised
678 // rot.Normalize();
679
680 float zX = rot.X * rot.Z + rot.Y * rot.W;
681
682 if (zX < -0.49999f)
683 {
684 roll = 0;
685 pitch = -halfpi;
686 }
687 else if (zX > 0.49999f)
688 {
689 roll = 0;
690 pitch = halfpi;
691 }
692 else
693 {
694 pitch = (float)Math.Asin(2 * zX);
695
696 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
697 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
698
699 roll = (float)Math.Atan2(minuszY, zZ);
700 }
701 return ;
702 }
703
704 internal void Step()//float pTimestep)
705 {
706 IntPtr Body = rootPrim.Body;
707
708 d.Quaternion rot = d.BodyGetQuaternion(Body);
709 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
710 Quaternion rotq = objrotq; // rotq = rotation of object
711 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
712 Quaternion irotq = Quaternion.Inverse(rotq);
713
714 d.Vector3 dvtmp;
715 Vector3 tmpV;
716 Vector3 curVel; // velocity in world
717 Vector3 curAngVel; // angular velocity in world
718 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
719 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
720 d.Vector3 dtorque = new d.Vector3();
721
722 dvtmp = d.BodyGetLinearVel(Body);
723 curVel.X = dvtmp.X;
724 curVel.Y = dvtmp.Y;
725 curVel.Z = dvtmp.Z;
726 Vector3 curLocalVel = curVel * irotq; // current velocity in local
727
728 dvtmp = d.BodyGetAngularVel(Body);
729 curAngVel.X = dvtmp.X;
730 curAngVel.Y = dvtmp.Y;
731 curAngVel.Z = dvtmp.Z;
732 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
733
734 // linear motor
735 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
736 {
737 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
738 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
739 tmpV *= rotq; // to world
740
741 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
742 tmpV.Z = 0;
743
744 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
745 {
746 // have offset, do it now
747 tmpV *= rootPrim.Mass;
748 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
749 }
750 else
751 {
752 force.X += tmpV.X;
753 force.Y += tmpV.Y;
754 force.Z += tmpV.Z;
755 }
756 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
757
758 m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
759 }
760 else
761 {
762 m_lmEfect = 0;
763 m_ffactor = 1f;
764 }
765
766 // friction
767 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
768 {
769 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
770 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
771 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
772 tmpV *= rotq; // to world
773 force.X += tmpV.X;
774 force.Y += tmpV.Y;
775 force.Z += tmpV.Z;
776 }
777
778 // hover
779 if (m_VhoverTimescale < 300)
780 {
781 d.Vector3 pos = d.BodyGetPosition(Body);
782
783 // default to global
784 float perr = m_VhoverHeight - pos.Z;;
785
786 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
787 {
788 perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
789 }
790 else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
791 {
792 perr += _pParentScene.GetWaterLevel();
793 }
794 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
795 {
796 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
797 float w = _pParentScene.GetWaterLevel();
798 if (t > w)
799 perr += t;
800 else
801 perr += w;
802 }
803
804 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
805 {
806 force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
807 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
808 }
809 else // no buoyancy
810 force.Z += _pParentScene.gravityz;
811 }
812 else
813 {
814 // default gravity and buoancy
815 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
816 }
817
818 // linear deflection
819 if (m_linearDeflectionEfficiency > 0)
820 {
821 float len = curVel.Length();
822 Vector3 atAxis;
823 atAxis = Xrot(rotq); // where are we pointing to
824 atAxis *= len; // make it same size as world velocity vector
825 tmpV = -atAxis; // oposite direction
826 atAxis -= curVel; // error to one direction
827 len = atAxis.LengthSquared();
828 tmpV -= curVel; // error to oposite
829 float lens = tmpV.LengthSquared();
830 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
831 {
832 if (len < lens)
833 tmpV = atAxis;
834
835 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
836 force.X += tmpV.X;
837 force.Y += tmpV.Y;
838 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
839 force.Z += tmpV.Z;
840 }
841 }
842
843 // angular motor
844 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
845 {
846 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
847 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
848 torque.X += tmpV.X;
849 torque.Y += tmpV.Y;
850 torque.Z += tmpV.Z;
851 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
852 }
853 else
854 m_amEfect = 0;
855
856 // angular friction
857 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
858 {
859 torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
860 torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
861 torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
862 }
863
864 // angular deflection
865 if (m_angularDeflectionEfficiency > 0)
866 {
867 Vector3 dirv;
868
869 if (curLocalVel.X > 0.01f)
870 dirv = curLocalVel;
871 else if (curLocalVel.X < -0.01f)
872 // use oposite
873 dirv = -curLocalVel;
874 else
875 {
876 // make it fall into small positive x case
877 dirv.X = 0.01f;
878 dirv.Y = curLocalVel.Y;
879 dirv.Z = curLocalVel.Z;
880 }
881
882 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
883
884 if (Math.Abs(dirv.Z) > 0.01)
885 {
886 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
887 }
888
889 if (Math.Abs(dirv.Y) > 0.01)
890 {
891 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
892 }
893 }
894
895 // vertical atractor
896 if (m_verticalAttractionTimescale < 300)
897 {
898 float roll;
899 float pitch;
900
901 GetRollPitch(irotq, out roll, out pitch);
902
903 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
904 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
905
906 if (roll > halfpi)
907 roll = pi - roll;
908 else if (roll < -halfpi)
909 roll = -pi - roll;
910
911 float effroll = pitch / halfpi;
912 effroll *= effroll;
913 effroll = 1 - effroll;
914 effroll *= roll;
915
916 if (Math.Abs(effroll) > 0.01) // roll
917 {
918 torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2;
919 }
920
921 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
922 {
923 float effpitch = roll / halfpi;
924 effpitch *= effpitch;
925 effpitch = 1 - effpitch;
926 effpitch *= pitch;
927
928 if (Math.Abs(effpitch) > 0.01) // pitch
929 {
930 torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2;
931 }
932 }
933
934 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
935 {
936
937 float broll = effroll;
938/*
939 if (broll > halfpi)
940 broll = pi - broll;
941 else if (broll < -halfpi)
942 broll = -pi - broll;
943*/
944 broll *= m_bankingEfficiency;
945 if (m_bankingMix != 0)
946 {
947 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
948 if (vfact > 1.0f) vfact = 1.0f;
949
950 if (curLocalVel.X >= 0)
951 broll *= (1 + (vfact - 1) * m_bankingMix);
952 else
953 broll *= -(1 + (vfact - 1) * m_bankingMix);
954 }
955 // make z rot be in world Z not local as seems to be in sl
956
957 broll = broll / m_bankingTimescale;
958
959 ftmp = -Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
960
961 tmpV.X = ftmp * curAngVel.X;
962 tmpV.Y = ftmp * curAngVel.Y;
963 tmpV.Z = broll + ftmp * curAngVel.Z;
964 tmpV *= irotq;
965
966 torque.X += tmpV.X;
967 torque.Y += tmpV.Y;
968 torque.Z += tmpV.Z;
969 }
970 }
971
972 d.Mass dmass;
973 d.BodyGetMass(Body,out dmass);
974
975 if (force.X != 0 || force.Y != 0 || force.Z != 0)
976 {
977 force *= dmass.mass;
978 d.BodySetForce(Body, force.X, force.Y, force.Z);
979 }
980
981 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
982 {
983 torque *= m_referenceFrame; // to object frame
984 dtorque.X = torque.X;
985 dtorque.Y = torque.Y;
986 dtorque.Z = torque.Z;
987
988 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
989 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
990 }
991 }
992 }
993}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..71aec4c
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,3725 @@
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
72 protected bool m_building;
73 private Quaternion m_lastorientation = new Quaternion();
74 private Quaternion _orientation;
75
76 private Vector3 _position;
77 private Vector3 _velocity;
78 private Vector3 _torque;
79 private Vector3 m_lastVelocity;
80 private Vector3 m_lastposition;
81 private Vector3 m_rotationalVelocity;
82 private Vector3 _size;
83 private Vector3 _acceleration;
84 private Vector3 m_angularlock = Vector3.One;
85 private IntPtr Amotor = IntPtr.Zero;
86
87 private Vector3 m_force;
88 private Vector3 m_forceacc;
89 private Vector3 m_angularForceacc;
90
91 private Vector3 m_PIDTarget;
92 private float m_PIDTau;
93 private float PID_D = 35f;
94 private float PID_G = 25f;
95 private bool m_usePID;
96
97 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
98 // and are for non-VEHICLES only.
99
100 private float m_PIDHoverHeight;
101 private float m_PIDHoverTau;
102 private bool m_useHoverPID;
103 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
104 private float m_targetHoverHeight;
105 private float m_groundHeight;
106 private float m_waterHeight;
107 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
108
109 private int body_autodisable_frames = 20;
110
111 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
112 | CollisionCategories.Space
113 | CollisionCategories.Body
114 | CollisionCategories.Character
115 );
116// private bool m_collidesLand = true;
117 private bool m_collidesWater;
118 public bool m_returnCollisions;
119 private bool m_softcolide;
120
121 private bool m_NoColide; // for now only for internal use for bad meshs
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_disabled;
130
131
132 public uint m_localID;
133
134 private PrimitiveBaseShape _pbs;
135 public OdeScene _parent_scene;
136
137 /// <summary>
138 /// The physics space which contains prim geometry
139 /// </summary>
140 public IntPtr m_targetSpace = IntPtr.Zero;
141
142 public IntPtr prim_geom;
143 public IntPtr _triMeshData;
144
145 private PhysicsActor _parent;
146
147 private List<OdePrim> childrenPrim = new List<OdePrim>();
148
149 private bool m_iscolliding;
150
151 public bool m_isSelected;
152 private bool m_delaySelect;
153 private bool m_lastdoneSelected;
154 public bool m_outbounds;
155
156 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
157
158 private bool m_throttleUpdates;
159 private int throttleCounter;
160 public float m_collisionscore;
161 int m_colliderfilter = 0;
162
163 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
164
165 private float m_density = 10.000006836f; // Aluminum g/cm3;
166 private byte m_shapetype;
167 public bool _zeroFlag;
168 private bool m_lastUpdateSent;
169
170 public IntPtr Body = IntPtr.Zero;
171 public String Name { get; private set; }
172 private Vector3 _target_velocity;
173
174 public Vector3 primOOBsize; // prim real dimensions from mesh
175 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
176 public float primOOBradiusSQ;
177 public d.Mass primdMass; // prim inertia information on it's own referencial
178 float primMass; // prim own mass
179 float _mass; // object mass acording to case
180 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
181
182 public int givefakepos = 0;
183 private Vector3 fakepos;
184 public int givefakeori = 0;
185 private Quaternion fakeori;
186
187 public int m_eventsubscription;
188 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
189
190 public volatile bool childPrim;
191
192 public ODEDynamics m_vehicle;
193
194 internal int m_material = (int)Material.Wood;
195 private float mu;
196 private float bounce;
197
198 /// <summary>
199 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
200 /// </summary>
201 public override bool IsPhysical // this is not reliable for internal use
202 {
203 get { return m_fakeisphysical; }
204 set
205 {
206 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
207 // and also to stop imediatly some updates
208 // but real change will only happen in taintprocessing
209
210 if (!value) // Zero the remembered last velocity
211 m_lastVelocity = Vector3.Zero;
212 AddChange(changes.Physical, value);
213 }
214 }
215
216 public override bool Phantom // this is not reliable for internal use
217 {
218 get { return m_fakeisphantom; }
219 set
220 {
221 m_fakeisphantom = value; // we show imediatly to outside that we changed physical
222 // and also to stop imediatly some updates
223 // but real change will only happen in taintprocessing
224
225 AddChange(changes.Phantom, value);
226 }
227 }
228
229 public override bool Building // this is not reliable for internal use
230 {
231 get { return m_building; }
232 set
233 {
234 if (value)
235 m_building = true;
236 AddChange(changes.building, value);
237 }
238 }
239
240 public override void getContactData(ref ContactData cdata)
241 {
242 cdata.mu = mu;
243 cdata.bounce = bounce;
244
245 // cdata.softcolide = m_softcolide;
246 cdata.softcolide = false;
247
248 if (m_isphysical)
249 {
250 ODEDynamics veh;
251 if (_parent != null)
252 veh = ((OdePrim)_parent).m_vehicle;
253 else
254 veh = m_vehicle;
255
256 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
257 cdata.mu *= veh.FrictionFactor;
258 }
259 }
260
261 public override int PhysicsActorType
262 {
263 get { return (int)ActorTypes.Prim; }
264 set { return; }
265 }
266
267 public override bool SetAlwaysRun
268 {
269 get { return false; }
270 set { return; }
271 }
272
273 public override uint LocalID
274 {
275 get
276 {
277 return m_localID;
278 }
279 set
280 {
281 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
282 m_localID = value;
283 }
284 }
285
286 public override bool Grabbed
287 {
288 set { return; }
289 }
290
291 public override bool Selected
292 {
293 set
294 {
295 if (value)
296 m_isSelected = value; // if true set imediatly to stop moves etc
297 AddChange(changes.Selected, value);
298 }
299 }
300
301 public override bool Flying
302 {
303 // no flying prims for you
304 get { return false; }
305 set { }
306 }
307
308 public override bool IsColliding
309 {
310 get { return m_iscolliding; }
311 set
312 {
313 if (value)
314 {
315 m_colliderfilter += 2;
316 if (m_colliderfilter > 2)
317 m_colliderfilter = 2;
318 }
319 else
320 {
321 m_colliderfilter--;
322 if (m_colliderfilter < 0)
323 m_colliderfilter = 0;
324 }
325
326 if (m_colliderfilter == 0)
327 {
328 m_softcolide = false;
329 m_iscolliding = false;
330 }
331 else
332 m_iscolliding = true;
333 }
334 }
335
336 public override bool CollidingGround
337 {
338 get { return false; }
339 set { return; }
340 }
341
342 public override bool CollidingObj
343 {
344 get { return false; }
345 set { return; }
346 }
347
348 public override bool ThrottleUpdates
349 {
350 get { return m_throttleUpdates; }
351 set { m_throttleUpdates = value; }
352 }
353
354 public override bool Stopped
355 {
356 get { return _zeroFlag; }
357 }
358
359 public override Vector3 Position
360 {
361 get
362 {
363 if (givefakepos > 0)
364 return fakepos;
365 else
366 return _position;
367 }
368
369 set
370 {
371 fakepos = value;
372 givefakepos++;
373 AddChange(changes.Position, value);
374 }
375 }
376
377 public override Vector3 Size
378 {
379 get { return _size; }
380 set
381 {
382 if (value.IsFinite())
383 {
384 AddChange(changes.Size, value);
385 }
386 else
387 {
388 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
389 }
390 }
391 }
392
393 public override float Mass
394 {
395 get { return _mass; }
396 }
397
398 public override Vector3 Force
399 {
400 //get { return Vector3.Zero; }
401 get { return m_force; }
402 set
403 {
404 if (value.IsFinite())
405 {
406 AddChange(changes.Force, value);
407 }
408 else
409 {
410 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
411 }
412 }
413 }
414
415 public override void SetVolumeDetect(int param)
416 {
417 AddChange(changes.VolumeDtc, (param != 0));
418 }
419
420 public override Vector3 GeometricCenter
421 {
422 get
423 {
424 return Vector3.Zero;
425 }
426 }
427
428 public override Vector3 CenterOfMass
429 {
430 get
431 {
432 d.Vector3 dtmp;
433 if (IsPhysical && !childPrim && Body != IntPtr.Zero)
434 {
435 dtmp = d.BodyGetPosition(Body);
436 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
437 }
438 else if (prim_geom != IntPtr.Zero)
439 {
440 d.Quaternion dq;
441 d.GeomCopyQuaternion(prim_geom, out dq);
442 Quaternion q;
443 q.X = dq.X;
444 q.Y = dq.Y;
445 q.Z = dq.Z;
446 q.W = dq.W;
447
448 Vector3 vtmp = primOOBoffset * q;
449 dtmp = d.GeomGetPosition(prim_geom);
450 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
451 }
452 else
453 return Vector3.Zero;
454 }
455 }
456 /*
457 public override Vector3 PrimOOBsize
458 {
459 get
460 {
461 return primOOBsize;
462 }
463 }
464
465 public override Vector3 PrimOOBoffset
466 {
467 get
468 {
469 return primOOBoffset;
470 }
471 }
472
473 public override float PrimOOBRadiusSQ
474 {
475 get
476 {
477 return primOOBradiusSQ;
478 }
479 }
480 */
481 public override PrimitiveBaseShape Shape
482 {
483 set
484 {
485 AddChange(changes.Shape, value);
486 }
487 }
488
489 public override byte PhysicsShapeType
490 {
491 get
492 {
493 return m_shapetype;
494 }
495 set
496 {
497 m_shapetype = value;
498 AddChange(changes.Shape, null);
499 }
500 }
501
502 public override Vector3 Velocity
503 {
504 get
505 {
506 if (_zeroFlag)
507 return Vector3.Zero;
508 return _velocity;
509 }
510 set
511 {
512 if (value.IsFinite())
513 {
514 AddChange(changes.Velocity, value);
515 // _velocity = value;
516
517 }
518 else
519 {
520 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
521 }
522
523 }
524 }
525
526 public override Vector3 Torque
527 {
528 get
529 {
530 if (!IsPhysical || Body == IntPtr.Zero)
531 return Vector3.Zero;
532
533 return _torque;
534 }
535
536 set
537 {
538 if (value.IsFinite())
539 {
540 AddChange(changes.Torque, value);
541 }
542 else
543 {
544 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
545 }
546 }
547 }
548
549 public override float CollisionScore
550 {
551 get { return m_collisionscore; }
552 set { m_collisionscore = value; }
553 }
554
555 public override bool Kinematic
556 {
557 get { return false; }
558 set { }
559 }
560
561 public override Quaternion Orientation
562 {
563 get
564 {
565 if (givefakeori > 0)
566 return fakeori;
567 else
568
569 return _orientation;
570 }
571 set
572 {
573 if (QuaternionIsFinite(value))
574 {
575 fakeori = value;
576 givefakeori++;
577 AddChange(changes.Orientation, value);
578 }
579 else
580 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
581
582 }
583 }
584
585 public override Vector3 Acceleration
586 {
587 get { return _acceleration; }
588 set { }
589 }
590
591 public override Vector3 RotationalVelocity
592 {
593 get
594 {
595 Vector3 pv = Vector3.Zero;
596 if (_zeroFlag)
597 return pv;
598 m_lastUpdateSent = false;
599
600 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
601 return pv;
602
603 return m_rotationalVelocity;
604 }
605 set
606 {
607 if (value.IsFinite())
608 {
609 m_rotationalVelocity = value;
610 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
611 d.BodyEnable(Body);
612 }
613 else
614 {
615 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
616 }
617 }
618 }
619
620
621 public override float Buoyancy
622 {
623 get { return m_buoyancy; }
624 set
625 {
626 m_buoyancy = value;
627 }
628 }
629
630 public override bool FloatOnWater
631 {
632 set
633 {
634 AddChange(changes.CollidesWater, value);
635 }
636 }
637
638 public override Vector3 PIDTarget
639 {
640 set
641 {
642 if (value.IsFinite())
643 {
644 m_PIDTarget = value;
645 }
646 else
647 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
648 }
649 }
650
651 public override bool PIDActive { set { m_usePID = value; } }
652 public override float PIDTau { set { m_PIDTau = value; } }
653
654 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
655 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
656 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
657 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
658
659 public override Quaternion APIDTarget { set { return; } }
660
661 public override bool APIDActive { set { return; } }
662
663 public override float APIDStrength { set { return; } }
664
665 public override float APIDDamping { set { return; } }
666
667 public override int VehicleType
668 {
669 // we may need to put a fake on this
670 get
671 {
672 if (m_vehicle == null)
673 return (int)Vehicle.TYPE_NONE;
674 else
675 return (int)m_vehicle.Type;
676 }
677 set
678 {
679 AddChange(changes.VehicleType, value);
680 }
681 }
682
683 public override void VehicleFloatParam(int param, float value)
684 {
685 strVehicleFloatParam fp = new strVehicleFloatParam();
686 fp.param = param;
687 fp.value = value;
688 AddChange(changes.VehicleFloatParam, fp);
689 }
690
691 public override void VehicleVectorParam(int param, Vector3 value)
692 {
693 strVehicleVectorParam fp = new strVehicleVectorParam();
694 fp.param = param;
695 fp.value = value;
696 AddChange(changes.VehicleVectorParam, fp);
697 }
698
699 public override void VehicleRotationParam(int param, Quaternion value)
700 {
701 strVehicleQuatParam fp = new strVehicleQuatParam();
702 fp.param = param;
703 fp.value = value;
704 AddChange(changes.VehicleRotationParam, fp);
705 }
706
707 public override void VehicleFlags(int param, bool value)
708 {
709 strVehicleBoolParam bp = new strVehicleBoolParam();
710 bp.param = param;
711 bp.value = value;
712 AddChange(changes.VehicleFlags, bp);
713 }
714
715 public override void SetVehicle(object vdata)
716 {
717 AddChange(changes.SetVehicle, vdata);
718 }
719 public void SetAcceleration(Vector3 accel)
720 {
721 _acceleration = accel;
722 }
723
724 public override void AddForce(Vector3 force, bool pushforce)
725 {
726 if (force.IsFinite())
727 {
728 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE);
729 }
730 else
731 {
732 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
733 }
734 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
735 }
736
737 public override void AddAngularForce(Vector3 force, bool pushforce)
738 {
739 if (force.IsFinite())
740 {
741 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE);
742 }
743 else
744 {
745 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
746 }
747 }
748
749 public override void CrossingFailure()
750 {
751 if (m_outbounds)
752 {
753 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
754 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
755 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
756
757 m_lastposition = _position;
758 _velocity.X = 0;
759 _velocity.Y = 0;
760 _velocity.Z = 0;
761
762 m_lastVelocity = _velocity;
763 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
764 m_vehicle.Stop();
765
766 if(Body != IntPtr.Zero)
767 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
768 if (prim_geom != IntPtr.Zero)
769 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
770
771 m_outbounds = false;
772 changeDisable(false);
773 base.RequestPhysicsterseUpdate();
774 }
775 }
776
777 public override void SetMomentum(Vector3 momentum)
778 {
779 }
780
781 public override void SetMaterial(int pMaterial)
782 {
783 m_material = pMaterial;
784 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
785 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
786 }
787
788 public void setPrimForRemoval()
789 {
790 AddChange(changes.Remove, null);
791 }
792
793 public override void link(PhysicsActor obj)
794 {
795 AddChange(changes.Link, obj);
796 }
797
798 public override void delink()
799 {
800 AddChange(changes.DeLink, null);
801 }
802
803 public override void LockAngularMotion(Vector3 axis)
804 {
805 // reverse the zero/non zero values for ODE.
806 if (axis.IsFinite())
807 {
808 axis.X = (axis.X > 0) ? 1f : 0f;
809 axis.Y = (axis.Y > 0) ? 1f : 0f;
810 axis.Z = (axis.Z > 0) ? 1f : 0f;
811 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
812 AddChange(changes.AngLock, axis);
813 }
814 else
815 {
816 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
817 }
818 }
819
820 public override void SubscribeEvents(int ms)
821 {
822 m_eventsubscription = ms;
823 _parent_scene.AddCollisionEventReporting(this);
824 }
825
826 public override void UnSubscribeEvents()
827 {
828 _parent_scene.RemoveCollisionEventReporting(this);
829 m_eventsubscription = 0;
830 }
831
832 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
833 {
834 if (CollisionEventsThisFrame == null)
835 CollisionEventsThisFrame = new CollisionEventUpdate();
836
837 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
838 }
839
840 public void SendCollisions()
841 {
842 if (CollisionEventsThisFrame == null)
843 return;
844
845 base.SendCollisionUpdate(CollisionEventsThisFrame);
846
847 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
848 CollisionEventsThisFrame = null;
849 else
850 CollisionEventsThisFrame = new CollisionEventUpdate();
851 }
852
853 public override bool SubscribedEvents()
854 {
855 if (m_eventsubscription > 0)
856 return true;
857 return false;
858 }
859
860
861 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
862 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
863 {
864 Name = primName;
865 LocalID = plocalID;
866
867 m_vehicle = null;
868
869 if (!pos.IsFinite())
870 {
871 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
872 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
873 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
874 }
875 _position = pos;
876 givefakepos = 0;
877
878 PID_D = parent_scene.bodyPIDD;
879 PID_G = parent_scene.bodyPIDG;
880 m_density = parent_scene.geomDefaultDensity;
881 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
882 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
883
884 prim_geom = IntPtr.Zero;
885 collide_geom = IntPtr.Zero;
886 Body = IntPtr.Zero;
887
888 if (!size.IsFinite())
889 {
890 size = new Vector3(0.5f, 0.5f, 0.5f);
891 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
892 }
893
894 if (size.X <= 0) size.X = 0.01f;
895 if (size.Y <= 0) size.Y = 0.01f;
896 if (size.Z <= 0) size.Z = 0.01f;
897
898 _size = size;
899
900 if (!QuaternionIsFinite(rotation))
901 {
902 rotation = Quaternion.Identity;
903 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
904 }
905
906 _orientation = rotation;
907 givefakeori = 0;
908
909 _pbs = pbs;
910
911 _parent_scene = parent_scene;
912 m_targetSpace = IntPtr.Zero;
913
914 if (pos.Z < 0)
915 {
916 m_isphysical = false;
917 }
918 else
919 {
920 m_isphysical = pisPhysical;
921 }
922 m_fakeisphysical = m_isphysical;
923
924 m_isVolumeDetect = false;
925
926 m_force = Vector3.Zero;
927
928 m_iscolliding = false;
929 m_colliderfilter = 0;
930 m_softcolide = true;
931 m_NoColide = false;
932
933 hasOOBoffsetFromMesh = false;
934 _triMeshData = IntPtr.Zero;
935
936 m_shapetype = _shapeType;
937
938 m_lastdoneSelected = false;
939 m_isSelected = false;
940 m_delaySelect = false;
941
942 m_isphantom = pisPhantom;
943 m_fakeisphantom = pisPhantom;
944
945 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
946 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
947
948 CalcPrimBodyData();
949
950 m_building = true; // control must set this to false when done
951
952 AddChange(changes.Add, null);
953 }
954
955 private void resetCollisionAccounting()
956 {
957 m_collisionscore = 0;
958 }
959
960 private void createAMotor(Vector3 axis)
961 {
962 if (Body == IntPtr.Zero)
963 return;
964
965 if (Amotor != IntPtr.Zero)
966 {
967 d.JointDestroy(Amotor);
968 Amotor = IntPtr.Zero;
969 }
970
971 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
972
973 if (axisnum <= 0)
974 return;
975
976 // stop it
977 d.BodySetTorque(Body, 0, 0, 0);
978 d.BodySetAngularVel(Body, 0, 0, 0);
979
980 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
981 d.JointAttach(Amotor, Body, IntPtr.Zero);
982
983 d.JointSetAMotorMode(Amotor, 0);
984
985 d.JointSetAMotorNumAxes(Amotor, axisnum);
986
987 // get current orientation to lock
988
989 d.Quaternion dcur = d.BodyGetQuaternion(Body);
990 Quaternion curr; // crap convertion between identical things
991 curr.X = dcur.X;
992 curr.Y = dcur.Y;
993 curr.Z = dcur.Z;
994 curr.W = dcur.W;
995 Vector3 ax;
996
997 int i = 0;
998 int j = 0;
999 if (axis.X == 0)
1000 {
1001 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1002 // ODE should do this with axis relative to body 1 but seems to fail
1003 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1004 d.JointSetAMotorAngle(Amotor, 0, 0);
1005 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1006 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1007 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1008 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1009 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1010 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1011 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1012 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1013 i++;
1014 j = 256; // move to next axis set
1015 }
1016
1017 if (axis.Y == 0)
1018 {
1019 ax = (new Vector3(0, 1, 0)) * curr;
1020 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1021 d.JointSetAMotorAngle(Amotor, i, 0);
1022 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1023 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1024 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1025 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1026 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1027 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1028 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1029 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1030 i++;
1031 j += 256;
1032 }
1033
1034 if (axis.Z == 0)
1035 {
1036 ax = (new Vector3(0, 0, 1)) * curr;
1037 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1038 d.JointSetAMotorAngle(Amotor, i, 0);
1039 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1040 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1041 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1042 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1043 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1044 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1045 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1046 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1047 }
1048 }
1049
1050 private bool setMesh(OdeScene parent_scene)
1051 {
1052 if (Body != IntPtr.Zero)
1053 {
1054 if (childPrim)
1055 {
1056 if (_parent != null)
1057 {
1058 OdePrim parent = (OdePrim)_parent;
1059 parent.ChildDelink(this, false);
1060 }
1061 }
1062 else
1063 {
1064 DestroyBody();
1065 }
1066 }
1067
1068 bool convex;
1069 if (m_shapetype == 0)
1070 convex = false;
1071 else
1072 convex = true;
1073
1074 IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex);
1075 if (mesh == null)
1076 {
1077 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1078 return false;
1079 }
1080
1081 IntPtr vertices, indices;
1082 int vertexCount, indexCount;
1083 int vertexStride, triStride;
1084
1085 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1086 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1087
1088 if (vertexCount == 0 || indexCount == 0)
1089 {
1090 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1091 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1092 mesh.releaseSourceMeshData();
1093 return false;
1094 }
1095
1096 primOOBoffset = mesh.GetCentroid();
1097 hasOOBoffsetFromMesh = true;
1098
1099 mesh.releaseSourceMeshData();
1100
1101 IntPtr geo = IntPtr.Zero;
1102
1103 try
1104 {
1105 _triMeshData = d.GeomTriMeshDataCreate();
1106
1107 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1108 d.GeomTriMeshDataPreprocess(_triMeshData);
1109
1110 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1111 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1112 }
1113
1114 catch (Exception e)
1115 {
1116 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1117 if (_triMeshData != IntPtr.Zero)
1118 {
1119 d.GeomTriMeshDataDestroy(_triMeshData);
1120 _triMeshData = IntPtr.Zero;
1121 }
1122 return false;
1123 }
1124
1125 SetGeom(geo);
1126 return true;
1127 }
1128
1129 private void SetGeom(IntPtr geom)
1130 {
1131 prim_geom = geom;
1132 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1133 if (prim_geom != IntPtr.Zero)
1134 {
1135 if (m_NoColide)
1136 {
1137 d.GeomSetCategoryBits(prim_geom, 0);
1138 if (m_isphysical)
1139 {
1140 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1141 }
1142 else
1143 {
1144 d.GeomSetCollideBits(prim_geom, 0);
1145 d.GeomDisable(prim_geom);
1146 }
1147 }
1148 else
1149 {
1150 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1151 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1152 }
1153
1154 CalcPrimBodyData();
1155
1156 _parent_scene.geom_name_map[prim_geom] = Name;
1157 _parent_scene.actor_name_map[prim_geom] = this;
1158
1159 }
1160 else
1161 m_log.Warn("Setting bad Geom");
1162 }
1163
1164
1165 /// <summary>
1166 /// Create a geometry for the given mesh in the given target space.
1167 /// </summary>
1168 /// <param name="m_targetSpace"></param>
1169 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1170 private void CreateGeom()
1171 {
1172 if (_triMeshData != IntPtr.Zero)
1173 {
1174 d.GeomTriMeshDataDestroy(_triMeshData);
1175 _triMeshData = IntPtr.Zero;
1176 }
1177
1178 bool haveMesh = false;
1179 hasOOBoffsetFromMesh = false;
1180 m_NoColide = false;
1181
1182 if (_parent_scene.needsMeshing(_pbs))
1183 {
1184 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1185 if (!haveMesh)
1186 m_NoColide = true;
1187 }
1188
1189 if (!haveMesh)
1190 {
1191 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1192 && _size.X == _size.Y && _size.Y == _size.Z)
1193 { // it's a sphere
1194 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1195 try
1196 {
1197 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1198 }
1199 catch (Exception e)
1200 {
1201 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1202 return;
1203 }
1204 }
1205 else
1206 {// do it as a box
1207 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1208 try
1209 {
1210 //Console.WriteLine(" CreateGeom 4");
1211 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1212 }
1213 catch (Exception e)
1214 {
1215 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1216 return;
1217 }
1218 }
1219 }
1220 }
1221
1222 /// <summary>
1223 /// Set a new geometry for this prim.
1224 /// </summary>
1225 /// <param name="geom"></param>
1226 private void RemoveGeom()
1227 {
1228 if (prim_geom != IntPtr.Zero)
1229 {
1230 _parent_scene.geom_name_map.Remove(prim_geom);
1231 _parent_scene.actor_name_map.Remove(prim_geom);
1232 try
1233 {
1234 d.GeomDestroy(prim_geom);
1235 if (_triMeshData != IntPtr.Zero)
1236 {
1237 d.GeomTriMeshDataDestroy(_triMeshData);
1238 _triMeshData = IntPtr.Zero;
1239 }
1240 }
1241 // catch (System.AccessViolationException)
1242 catch (Exception e)
1243 {
1244 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1245 }
1246
1247 prim_geom = IntPtr.Zero;
1248 }
1249 else
1250 {
1251 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1252 }
1253 Body = IntPtr.Zero;
1254 hasOOBoffsetFromMesh = false;
1255 CalcPrimBodyData();
1256 }
1257
1258 private void ChildSetGeom(OdePrim odePrim)
1259 {
1260 // well..
1261 DestroyBody();
1262 MakeBody();
1263 }
1264
1265 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1266 // should only be called for non physical prims unless they are becoming non physical
1267 private void SetInStaticSpace(OdePrim prim)
1268 {
1269 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1270 prim.m_targetSpace = targetSpace;
1271 d.GeomEnable(prim_geom);
1272 }
1273
1274 public void enableBodySoft()
1275 {
1276 if (!childPrim && !m_isSelected)
1277 {
1278 if (m_isphysical && Body != IntPtr.Zero)
1279 {
1280 if (m_isphantom && !m_isVolumeDetect)
1281 {
1282 m_collisionCategories = 0;
1283 m_collisionFlags = CollisionCategories.Land;
1284 }
1285 else
1286 {
1287 m_collisionCategories |= CollisionCategories.Body;
1288 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1289 }
1290
1291 foreach (OdePrim prm in childrenPrim)
1292 {
1293 prm.m_collisionCategories = m_collisionCategories;
1294 prm.m_collisionFlags = m_collisionFlags;
1295
1296 if (prm.prim_geom != IntPtr.Zero)
1297 {
1298 if (prm.m_NoColide)
1299 {
1300 d.GeomSetCategoryBits(prm.prim_geom, 0);
1301 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1302 }
1303 else
1304 {
1305 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1306 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1307 }
1308 d.GeomEnable(prm.prim_geom);
1309 }
1310 }
1311
1312 if (prim_geom != IntPtr.Zero)
1313 {
1314 if (m_NoColide)
1315 {
1316 d.GeomSetCategoryBits(prim_geom, 0);
1317 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1318 }
1319 else
1320 {
1321 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1322 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1323 }
1324 d.GeomEnable(prim_geom);
1325 }
1326 d.BodyEnable(Body);
1327 }
1328 }
1329 m_disabled = false;
1330 resetCollisionAccounting(); // this sets m_disable to false
1331 }
1332
1333 private void disableBodySoft()
1334 {
1335 m_disabled = true;
1336 if (!childPrim)
1337 {
1338 if (m_isphysical && Body != IntPtr.Zero)
1339 {
1340 m_collisionCategories &= ~CollisionCategories.Body;
1341 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1342
1343 foreach (OdePrim prm in childrenPrim)
1344 {
1345 prm.m_collisionCategories = m_collisionCategories;
1346 prm.m_collisionFlags = m_collisionFlags;
1347
1348 if (prm.prim_geom != IntPtr.Zero)
1349 {
1350 if (prm.m_NoColide)
1351 {
1352 d.GeomSetCategoryBits(prm.prim_geom, 0);
1353 d.GeomSetCollideBits(prm.prim_geom, 0);
1354 }
1355 else
1356 {
1357 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1358 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1359 }
1360 d.GeomDisable(prm.prim_geom);
1361 }
1362 }
1363
1364 if (prim_geom != IntPtr.Zero)
1365 {
1366 if (m_NoColide)
1367 {
1368 d.GeomSetCategoryBits(prim_geom, 0);
1369 d.GeomSetCollideBits(prim_geom, 0);
1370 }
1371 else
1372 {
1373 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1374 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1375 }
1376 d.GeomDisable(prim_geom);
1377 }
1378
1379 d.BodyDisable(Body);
1380 }
1381 }
1382 }
1383
1384 private void MakeBody()
1385 {
1386 if (!m_isphysical) // only physical get bodies
1387 return;
1388
1389 if (childPrim) // child prims don't get bodies;
1390 return;
1391
1392 if (m_building)
1393 return;
1394
1395 if (prim_geom == IntPtr.Zero)
1396 {
1397 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1398 return;
1399 }
1400
1401 if (Body != IntPtr.Zero)
1402 {
1403 d.BodyDestroy(Body);
1404 Body = IntPtr.Zero;
1405 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1406 }
1407
1408
1409 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1410 {
1411 d.GeomSetBody(prim_geom, IntPtr.Zero);
1412 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1413 }
1414
1415 d.Matrix3 mymat = new d.Matrix3();
1416 d.Quaternion myrot = new d.Quaternion();
1417 d.Mass objdmass = new d.Mass { };
1418
1419 Body = d.BodyCreate(_parent_scene.world);
1420
1421 DMassDup(ref primdMass, out objdmass);
1422
1423 // rotate inertia
1424 myrot.X = _orientation.X;
1425 myrot.Y = _orientation.Y;
1426 myrot.Z = _orientation.Z;
1427 myrot.W = _orientation.W;
1428
1429 d.RfromQ(out mymat, ref myrot);
1430 d.MassRotate(ref objdmass, ref mymat);
1431
1432 // set the body rotation and position
1433 d.BodySetRotation(Body, ref mymat);
1434
1435 // recompute full object inertia if needed
1436 if (childrenPrim.Count > 0)
1437 {
1438 d.Matrix3 mat = new d.Matrix3();
1439 d.Quaternion quat = new d.Quaternion();
1440 d.Mass tmpdmass = new d.Mass { };
1441 Vector3 rcm;
1442
1443 rcm.X = _position.X + objdmass.c.X;
1444 rcm.Y = _position.Y + objdmass.c.Y;
1445 rcm.Z = _position.Z + objdmass.c.Z;
1446
1447 lock (childrenPrim)
1448 {
1449 foreach (OdePrim prm in childrenPrim)
1450 {
1451 if (prm.prim_geom == IntPtr.Zero)
1452 {
1453 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1454 continue;
1455 }
1456
1457 DMassCopy(ref prm.primdMass, ref tmpdmass);
1458
1459 // apply prim current rotation to inertia
1460 quat.X = prm._orientation.X;
1461 quat.Y = prm._orientation.Y;
1462 quat.Z = prm._orientation.Z;
1463 quat.W = prm._orientation.W;
1464 d.RfromQ(out mat, ref quat);
1465 d.MassRotate(ref tmpdmass, ref mat);
1466
1467 Vector3 ppos = prm._position;
1468 ppos.X += tmpdmass.c.X - rcm.X;
1469 ppos.Y += tmpdmass.c.Y - rcm.Y;
1470 ppos.Z += tmpdmass.c.Z - rcm.Z;
1471
1472 // refer inertia to root prim center of mass position
1473 d.MassTranslate(ref tmpdmass,
1474 ppos.X,
1475 ppos.Y,
1476 ppos.Z);
1477
1478 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1479 // fix prim colision cats
1480
1481 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1482 {
1483 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1484 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1485 }
1486
1487 d.GeomClearOffset(prm.prim_geom);
1488 d.GeomSetBody(prm.prim_geom, Body);
1489 prm.Body = Body;
1490 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1491 }
1492 }
1493 }
1494
1495 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1496 // associate root geom with body
1497 d.GeomSetBody(prim_geom, Body);
1498
1499 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1500 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1501
1502 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1503 myrot.W = -myrot.W;
1504 d.RfromQ(out mymat, ref myrot);
1505 d.MassRotate(ref objdmass, ref mymat);
1506 d.BodySetMass(Body, ref objdmass);
1507 _mass = objdmass.mass;
1508
1509 // disconnect from world gravity so we can apply buoyancy
1510 d.BodySetGravityMode(Body, false);
1511
1512 d.BodySetAutoDisableFlag(Body, true);
1513 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1514 // d.BodySetLinearDampingThreshold(Body, 0.01f);
1515 // d.BodySetAngularDampingThreshold(Body, 0.001f);
1516 d.BodySetDamping(Body, .002f, .002f);
1517
1518
1519 if (m_targetSpace != IntPtr.Zero)
1520 {
1521 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1522 if (d.SpaceQuery(m_targetSpace, prim_geom))
1523 d.SpaceRemove(m_targetSpace, prim_geom);
1524 }
1525
1526
1527 if (childrenPrim.Count == 0)
1528 {
1529 collide_geom = prim_geom;
1530 m_targetSpace = _parent_scene.ActiveSpace;
1531 d.SpaceAdd(m_targetSpace, prim_geom);
1532 }
1533 else
1534 {
1535 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1536 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1537 d.SpaceSetSublevel(m_targetSpace, 3);
1538 d.SpaceSetCleanup(m_targetSpace, false);
1539 d.SpaceAdd(m_targetSpace, prim_geom);
1540 collide_geom = m_targetSpace;
1541 }
1542
1543 if (m_delaySelect)
1544 {
1545 m_isSelected = true;
1546 m_delaySelect = false;
1547 }
1548
1549 lock (childrenPrim)
1550 {
1551 foreach (OdePrim prm in childrenPrim)
1552 {
1553 if (prm.prim_geom == IntPtr.Zero)
1554 continue;
1555
1556 Vector3 ppos = prm._position;
1557 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1558
1559 if (prm.m_targetSpace != m_targetSpace)
1560 {
1561 if (prm.m_targetSpace != IntPtr.Zero)
1562 {
1563 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1564 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1565 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1566 }
1567 prm.m_targetSpace = m_targetSpace;
1568 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1569 }
1570
1571 if (m_isSelected || m_disabled)
1572 {
1573 prm.m_collisionCategories &= ~CollisionCategories.Body;
1574 prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1575 d.GeomDisable(prm.prim_geom);
1576 }
1577 else
1578 {
1579 if (m_isphantom && !m_isVolumeDetect)
1580 {
1581 prm.m_collisionCategories = 0;
1582 prm.m_collisionFlags = CollisionCategories.Land;
1583 }
1584 else
1585 {
1586 prm.m_collisionCategories |= CollisionCategories.Body;
1587 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1588 }
1589 d.GeomEnable(prm.prim_geom);
1590 }
1591
1592 if (prm.m_NoColide)
1593 {
1594 d.GeomSetCategoryBits(prm.prim_geom, 0);
1595 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1596 d.GeomEnable(prm.prim_geom);
1597 }
1598 else
1599 {
1600 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1601 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1602 }
1603 prm.m_collisionscore = 0;
1604
1605 if(!m_disabled)
1606 prm.m_disabled = false;
1607
1608 _parent_scene.addActivePrim(prm);
1609 }
1610 }
1611
1612 // The body doesn't already have a finite rotation mode set here
1613 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1614 {
1615 createAMotor(m_angularlock);
1616 }
1617
1618 if (m_isSelected || m_disabled)
1619 {
1620 m_collisionCategories &= ~CollisionCategories.Body;
1621 m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1622
1623 d.GeomDisable(prim_geom);
1624 d.BodyDisable(Body);
1625 }
1626 else
1627 {
1628 if (m_isphantom && !m_isVolumeDetect)
1629 {
1630 m_collisionCategories = 0;
1631 m_collisionFlags = CollisionCategories.Land;
1632 }
1633 else
1634 {
1635 m_collisionCategories |= CollisionCategories.Body;
1636 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1637 }
1638
1639 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1640 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1641 }
1642
1643 if (m_NoColide)
1644 {
1645 d.GeomSetCategoryBits(prim_geom, 0);
1646 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1647 }
1648 else
1649 {
1650 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1651 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1652 }
1653
1654 m_collisionscore = 0;
1655
1656 m_softcolide = true;
1657 _parent_scene.addActivePrim(this);
1658 _parent_scene.addActiveGroups(this);
1659 }
1660
1661 private void DestroyBody()
1662 {
1663 if (Body != IntPtr.Zero)
1664 {
1665 _parent_scene.remActivePrim(this);
1666 m_collisionCategories &= ~CollisionCategories.Body;
1667 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1668 if (prim_geom != IntPtr.Zero)
1669 {
1670 if (m_NoColide)
1671 {
1672 d.GeomSetCategoryBits(prim_geom, 0);
1673 d.GeomSetCollideBits(prim_geom, 0);
1674 }
1675 else
1676 {
1677 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1678 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1679 }
1680 UpdateDataFromGeom();
1681 d.GeomSetBody(prim_geom, IntPtr.Zero);
1682 SetInStaticSpace(this);
1683 }
1684
1685 if (!childPrim)
1686 {
1687 lock (childrenPrim)
1688 {
1689 foreach (OdePrim prm in childrenPrim)
1690 {
1691 _parent_scene.remActivePrim(prm);
1692 prm.m_collisionCategories = m_collisionCategories;
1693 prm.m_collisionFlags = m_collisionFlags;
1694 if (prm.prim_geom != IntPtr.Zero)
1695 {
1696 if (prm.m_NoColide)
1697 {
1698 d.GeomSetCategoryBits(prm.prim_geom, 0);
1699 d.GeomSetCollideBits(prm.prim_geom, 0);
1700 }
1701 else
1702 {
1703 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1704 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1705 }
1706 prm.UpdateDataFromGeom();
1707 SetInStaticSpace(prm);
1708 }
1709 prm.Body = IntPtr.Zero;
1710 prm._mass = prm.primMass;
1711 prm.m_collisionscore = 0;
1712 }
1713 }
1714 if (Amotor != IntPtr.Zero)
1715 {
1716 d.JointDestroy(Amotor);
1717 Amotor = IntPtr.Zero;
1718 }
1719 _parent_scene.remActiveGroup(this);
1720 d.BodyDestroy(Body);
1721 }
1722 Body = IntPtr.Zero;
1723 }
1724 _mass = primMass;
1725 m_collisionscore = 0;
1726 }
1727
1728 #region Mass Calculation
1729
1730 private float CalculatePrimVolume()
1731 {
1732 float volume = _size.X * _size.Y * _size.Z; // default
1733 float tmp;
1734
1735 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1736 float hollowVolume = hollowAmount * hollowAmount;
1737
1738 switch (_pbs.ProfileShape)
1739 {
1740 case ProfileShape.Square:
1741 // default box
1742
1743 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1744 {
1745 if (hollowAmount > 0.0)
1746 {
1747 switch (_pbs.HollowShape)
1748 {
1749 case HollowShape.Square:
1750 case HollowShape.Same:
1751 break;
1752
1753 case HollowShape.Circle:
1754
1755 hollowVolume *= 0.78539816339f;
1756 break;
1757
1758 case HollowShape.Triangle:
1759
1760 hollowVolume *= (0.5f * .5f);
1761 break;
1762
1763 default:
1764 hollowVolume = 0;
1765 break;
1766 }
1767 volume *= (1.0f - hollowVolume);
1768 }
1769 }
1770
1771 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1772 {
1773 //a tube
1774
1775 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1776 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1777 volume -= volume * tmp * tmp;
1778
1779 if (hollowAmount > 0.0)
1780 {
1781 hollowVolume *= hollowAmount;
1782
1783 switch (_pbs.HollowShape)
1784 {
1785 case HollowShape.Square:
1786 case HollowShape.Same:
1787 break;
1788
1789 case HollowShape.Circle:
1790 hollowVolume *= 0.78539816339f;
1791 break;
1792
1793 case HollowShape.Triangle:
1794 hollowVolume *= 0.5f * 0.5f;
1795 break;
1796 default:
1797 hollowVolume = 0;
1798 break;
1799 }
1800 volume *= (1.0f - hollowVolume);
1801 }
1802 }
1803
1804 break;
1805
1806 case ProfileShape.Circle:
1807
1808 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1809 {
1810 volume *= 0.78539816339f; // elipse base
1811
1812 if (hollowAmount > 0.0)
1813 {
1814 switch (_pbs.HollowShape)
1815 {
1816 case HollowShape.Same:
1817 case HollowShape.Circle:
1818 break;
1819
1820 case HollowShape.Square:
1821 hollowVolume *= 0.5f * 2.5984480504799f;
1822 break;
1823
1824 case HollowShape.Triangle:
1825 hollowVolume *= .5f * 1.27323954473516f;
1826 break;
1827
1828 default:
1829 hollowVolume = 0;
1830 break;
1831 }
1832 volume *= (1.0f - hollowVolume);
1833 }
1834 }
1835
1836 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1837 {
1838 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1839 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1840 volume *= (1.0f - tmp * tmp);
1841
1842 if (hollowAmount > 0.0)
1843 {
1844
1845 // calculate the hollow volume by it's shape compared to the prim shape
1846 hollowVolume *= hollowAmount;
1847
1848 switch (_pbs.HollowShape)
1849 {
1850 case HollowShape.Same:
1851 case HollowShape.Circle:
1852 break;
1853
1854 case HollowShape.Square:
1855 hollowVolume *= 0.5f * 2.5984480504799f;
1856 break;
1857
1858 case HollowShape.Triangle:
1859 hollowVolume *= .5f * 1.27323954473516f;
1860 break;
1861
1862 default:
1863 hollowVolume = 0;
1864 break;
1865 }
1866 volume *= (1.0f - hollowVolume);
1867 }
1868 }
1869 break;
1870
1871 case ProfileShape.HalfCircle:
1872 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1873 {
1874 volume *= 0.52359877559829887307710723054658f;
1875 }
1876 break;
1877
1878 case ProfileShape.EquilateralTriangle:
1879
1880 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1881 {
1882 volume *= 0.32475953f;
1883
1884 if (hollowAmount > 0.0)
1885 {
1886
1887 // calculate the hollow volume by it's shape compared to the prim shape
1888 switch (_pbs.HollowShape)
1889 {
1890 case HollowShape.Same:
1891 case HollowShape.Triangle:
1892 hollowVolume *= .25f;
1893 break;
1894
1895 case HollowShape.Square:
1896 hollowVolume *= 0.499849f * 3.07920140172638f;
1897 break;
1898
1899 case HollowShape.Circle:
1900 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1901 // Cyllinder hollow volume calculation
1902
1903 hollowVolume *= 0.1963495f * 3.07920140172638f;
1904 break;
1905
1906 default:
1907 hollowVolume = 0;
1908 break;
1909 }
1910 volume *= (1.0f - hollowVolume);
1911 }
1912 }
1913 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1914 {
1915 volume *= 0.32475953f;
1916 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1917 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1918 volume *= (1.0f - tmp * tmp);
1919
1920 if (hollowAmount > 0.0)
1921 {
1922
1923 hollowVolume *= hollowAmount;
1924
1925 switch (_pbs.HollowShape)
1926 {
1927 case HollowShape.Same:
1928 case HollowShape.Triangle:
1929 hollowVolume *= .25f;
1930 break;
1931
1932 case HollowShape.Square:
1933 hollowVolume *= 0.499849f * 3.07920140172638f;
1934 break;
1935
1936 case HollowShape.Circle:
1937
1938 hollowVolume *= 0.1963495f * 3.07920140172638f;
1939 break;
1940
1941 default:
1942 hollowVolume = 0;
1943 break;
1944 }
1945 volume *= (1.0f - hollowVolume);
1946 }
1947 }
1948 break;
1949
1950 default:
1951 break;
1952 }
1953
1954 float taperX1;
1955 float taperY1;
1956 float taperX;
1957 float taperY;
1958 float pathBegin;
1959 float pathEnd;
1960 float profileBegin;
1961 float profileEnd;
1962
1963 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1964 {
1965 taperX1 = _pbs.PathScaleX * 0.01f;
1966 if (taperX1 > 1.0f)
1967 taperX1 = 2.0f - taperX1;
1968 taperX = 1.0f - taperX1;
1969
1970 taperY1 = _pbs.PathScaleY * 0.01f;
1971 if (taperY1 > 1.0f)
1972 taperY1 = 2.0f - taperY1;
1973 taperY = 1.0f - taperY1;
1974 }
1975 else
1976 {
1977 taperX = _pbs.PathTaperX * 0.01f;
1978 if (taperX < 0.0f)
1979 taperX = -taperX;
1980 taperX1 = 1.0f - taperX;
1981
1982 taperY = _pbs.PathTaperY * 0.01f;
1983 if (taperY < 0.0f)
1984 taperY = -taperY;
1985 taperY1 = 1.0f - taperY;
1986 }
1987
1988 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1989
1990 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1991 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1992 volume *= (pathEnd - pathBegin);
1993
1994 // this is crude aproximation
1995 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1996 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1997 volume *= (profileEnd - profileBegin);
1998
1999 return volume;
2000 }
2001
2002
2003 private void CalcPrimBodyData()
2004 {
2005 float volume;
2006
2007 if (prim_geom == IntPtr.Zero)
2008 {
2009 // Ubit let's have a initial basic OOB
2010 primOOBsize.X = _size.X;
2011 primOOBsize.Y = _size.Y;
2012 primOOBsize.Z = _size.Z;
2013 primOOBoffset = Vector3.Zero;
2014 }
2015 else
2016 {
2017 d.AABB AABB;
2018 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
2019
2020 primOOBsize.X = (AABB.MaxX - AABB.MinX);
2021 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
2022 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
2023 if (!hasOOBoffsetFromMesh)
2024 {
2025 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
2026 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
2027 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
2028 }
2029 }
2030
2031 // also its own inertia and mass
2032 // keep using basic shape mass for now
2033 volume = CalculatePrimVolume();
2034
2035 primMass = m_density * volume;
2036
2037 if (primMass <= 0)
2038 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2039 if (primMass > _parent_scene.maximumMassObject)
2040 primMass = _parent_scene.maximumMassObject;
2041
2042 _mass = primMass; // just in case
2043
2044 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
2045
2046 d.MassTranslate(ref primdMass,
2047 primOOBoffset.X,
2048 primOOBoffset.Y,
2049 primOOBoffset.Z);
2050
2051 primOOBsize *= 0.5f; // let obb size be a corner coords
2052 primOOBradiusSQ = primOOBsize.LengthSquared();
2053 }
2054
2055
2056 #endregion
2057
2058
2059 /// <summary>
2060 /// Add a child prim to this parent prim.
2061 /// </summary>
2062 /// <param name="prim">Child prim</param>
2063 // I'm the parent
2064 // prim is the child
2065 public void ParentPrim(OdePrim prim)
2066 {
2067 //Console.WriteLine("ParentPrim " + m_primName);
2068 if (this.m_localID != prim.m_localID)
2069 {
2070 DestroyBody(); // for now we need to rebuil entire object on link change
2071
2072 lock (childrenPrim)
2073 {
2074 // adopt the prim
2075 if (!childrenPrim.Contains(prim))
2076 childrenPrim.Add(prim);
2077
2078 // see if this prim has kids and adopt them also
2079 // should not happen for now
2080 foreach (OdePrim prm in prim.childrenPrim)
2081 {
2082 if (!childrenPrim.Contains(prm))
2083 {
2084 if (prm.Body != IntPtr.Zero)
2085 {
2086 if (prm.prim_geom != IntPtr.Zero)
2087 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2088 if (prm.Body != prim.Body)
2089 prm.DestroyBody(); // don't loose bodies around
2090 prm.Body = IntPtr.Zero;
2091 }
2092
2093 childrenPrim.Add(prm);
2094 prm._parent = this;
2095 }
2096 }
2097 }
2098 //Remove old children from the prim
2099 prim.childrenPrim.Clear();
2100
2101 if (prim.Body != IntPtr.Zero)
2102 {
2103 if (prim.prim_geom != IntPtr.Zero)
2104 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2105 prim.DestroyBody(); // don't loose bodies around
2106 prim.Body = IntPtr.Zero;
2107 }
2108
2109 prim.childPrim = true;
2110 prim._parent = this;
2111
2112 MakeBody(); // full nasty reconstruction
2113 }
2114 }
2115
2116 private void UpdateChildsfromgeom()
2117 {
2118 if (childrenPrim.Count > 0)
2119 {
2120 foreach (OdePrim prm in childrenPrim)
2121 prm.UpdateDataFromGeom();
2122 }
2123 }
2124
2125 private void UpdateDataFromGeom()
2126 {
2127 if (prim_geom != IntPtr.Zero)
2128 {
2129 d.Vector3 lpos;
2130 d.GeomCopyPosition(prim_geom, out lpos);
2131 _position.X = lpos.X;
2132 _position.Y = lpos.Y;
2133 _position.Z = lpos.Z;
2134 d.Quaternion qtmp = new d.Quaternion { };
2135 d.GeomCopyQuaternion(prim_geom, out qtmp);
2136 _orientation.W = qtmp.W;
2137 _orientation.X = qtmp.X;
2138 _orientation.Y = qtmp.Y;
2139 _orientation.Z = qtmp.Z;
2140 }
2141 }
2142
2143 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2144 {
2145 // Okay, we have a delinked child.. destroy all body and remake
2146 if (odePrim != this && !childrenPrim.Contains(odePrim))
2147 return;
2148
2149 DestroyBody();
2150
2151 if (odePrim == this) // delinking the root prim
2152 {
2153 OdePrim newroot = null;
2154 lock (childrenPrim)
2155 {
2156 if (childrenPrim.Count > 0)
2157 {
2158 newroot = childrenPrim[0];
2159 childrenPrim.RemoveAt(0);
2160 foreach (OdePrim prm in childrenPrim)
2161 {
2162 newroot.childrenPrim.Add(prm);
2163 }
2164 childrenPrim.Clear();
2165 }
2166 if (newroot != null)
2167 {
2168 newroot.childPrim = false;
2169 newroot._parent = null;
2170 if (remakebodies)
2171 newroot.MakeBody();
2172 }
2173 }
2174 }
2175
2176 else
2177 {
2178 lock (childrenPrim)
2179 {
2180 childrenPrim.Remove(odePrim);
2181 odePrim.childPrim = false;
2182 odePrim._parent = null;
2183 // odePrim.UpdateDataFromGeom();
2184 if (remakebodies)
2185 odePrim.MakeBody();
2186 }
2187 }
2188 if (remakebodies)
2189 MakeBody();
2190 }
2191
2192 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2193 {
2194 // Okay, we have a delinked child.. destroy all body and remake
2195 if (odePrim != this && !childrenPrim.Contains(odePrim))
2196 return;
2197
2198 DestroyBody();
2199
2200 if (odePrim == this)
2201 {
2202 OdePrim newroot = null;
2203 lock (childrenPrim)
2204 {
2205 if (childrenPrim.Count > 0)
2206 {
2207 newroot = childrenPrim[0];
2208 childrenPrim.RemoveAt(0);
2209 foreach (OdePrim prm in childrenPrim)
2210 {
2211 newroot.childrenPrim.Add(prm);
2212 }
2213 childrenPrim.Clear();
2214 }
2215 if (newroot != null)
2216 {
2217 newroot.childPrim = false;
2218 newroot._parent = null;
2219 newroot.MakeBody();
2220 }
2221 }
2222 if (reMakeBody)
2223 MakeBody();
2224 return;
2225 }
2226 else
2227 {
2228 lock (childrenPrim)
2229 {
2230 childrenPrim.Remove(odePrim);
2231 odePrim.childPrim = false;
2232 odePrim._parent = null;
2233 if (reMakeBody)
2234 odePrim.MakeBody();
2235 }
2236 }
2237 MakeBody();
2238 }
2239
2240 #region changes
2241
2242 private void changeadd()
2243 {
2244 CreateGeom();
2245
2246 if (prim_geom != IntPtr.Zero)
2247 {
2248 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2249 d.Quaternion myrot = new d.Quaternion();
2250 myrot.X = _orientation.X;
2251 myrot.Y = _orientation.Y;
2252 myrot.Z = _orientation.Z;
2253 myrot.W = _orientation.W;
2254 d.GeomSetQuaternion(prim_geom, ref myrot);
2255
2256 if (!m_isphysical)
2257 SetInStaticSpace(this);
2258 }
2259
2260 if (m_isphysical && Body == IntPtr.Zero)
2261 {
2262 MakeBody();
2263 }
2264 }
2265
2266 private void changeAngularLock(Vector3 newLock)
2267 {
2268 // do we have a Physical object?
2269 if (Body != IntPtr.Zero)
2270 {
2271 //Check that we have a Parent
2272 //If we have a parent then we're not authorative here
2273 if (_parent == null)
2274 {
2275 if (!newLock.ApproxEquals(Vector3.One, 0f))
2276 {
2277 createAMotor(newLock);
2278 }
2279 else
2280 {
2281 if (Amotor != IntPtr.Zero)
2282 {
2283 d.JointDestroy(Amotor);
2284 Amotor = IntPtr.Zero;
2285 }
2286 }
2287 }
2288 }
2289 // Store this for later in case we get turned into a separate body
2290 m_angularlock = newLock;
2291 }
2292
2293 private void changeLink(OdePrim NewParent)
2294 {
2295 if (_parent == null && NewParent != null)
2296 {
2297 NewParent.ParentPrim(this);
2298 }
2299 else if (_parent != null)
2300 {
2301 if (_parent is OdePrim)
2302 {
2303 if (NewParent != _parent)
2304 {
2305 (_parent as OdePrim).ChildDelink(this, false); // for now...
2306 childPrim = false;
2307
2308 if (NewParent != null)
2309 {
2310 NewParent.ParentPrim(this);
2311 }
2312 }
2313 }
2314 }
2315 _parent = NewParent;
2316 }
2317
2318
2319 private void Stop()
2320 {
2321 if (!childPrim)
2322 {
2323 m_force = Vector3.Zero;
2324 m_forceacc = Vector3.Zero;
2325 m_angularForceacc = Vector3.Zero;
2326 _torque = Vector3.Zero;
2327 _velocity = Vector3.Zero;
2328 _acceleration = Vector3.Zero;
2329 m_rotationalVelocity = Vector3.Zero;
2330 _target_velocity = Vector3.Zero;
2331 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2332 m_vehicle.Stop();
2333 }
2334
2335 if (Body != IntPtr.Zero)
2336 {
2337 d.BodySetForce(Body, 0f, 0f, 0f);
2338 d.BodySetTorque(Body, 0f, 0f, 0f);
2339 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2340 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2341 }
2342 }
2343
2344
2345 private void changePhantomStatus(bool newval)
2346 {
2347 m_isphantom = newval;
2348
2349 if (m_isSelected)
2350 {
2351 m_collisionCategories = CollisionCategories.Selected;
2352 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2353 }
2354 else
2355 {
2356 if (m_isphantom && !m_isVolumeDetect)
2357 {
2358 m_collisionCategories = 0;
2359 if (m_isphysical)
2360 m_collisionFlags = CollisionCategories.Land;
2361 else
2362 m_collisionFlags = 0; // should never happen
2363 }
2364
2365 else
2366 {
2367 m_collisionCategories = CollisionCategories.Geom;
2368 if (m_isphysical)
2369 m_collisionCategories |= CollisionCategories.Body;
2370
2371 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2372
2373 if (m_collidesWater)
2374 m_collisionFlags |= CollisionCategories.Water;
2375 }
2376 }
2377
2378 if (!childPrim)
2379 {
2380 foreach (OdePrim prm in childrenPrim)
2381 {
2382 prm.m_collisionCategories = m_collisionCategories;
2383 prm.m_collisionFlags = m_collisionFlags;
2384
2385 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2386 {
2387 if (prm.m_NoColide)
2388 {
2389 d.GeomSetCategoryBits(prm.prim_geom, 0);
2390 if (m_isphysical)
2391 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2392 else
2393 d.GeomSetCollideBits(prm.prim_geom, 0);
2394 }
2395 else
2396 {
2397 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2398 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2399 }
2400 if(!m_isSelected)
2401 d.GeomEnable(prm.prim_geom);
2402 }
2403 }
2404 }
2405
2406 if (!m_disabled && prim_geom != IntPtr.Zero)
2407 {
2408 if (m_NoColide)
2409 {
2410 d.GeomSetCategoryBits(prim_geom, 0);
2411 if (m_isphysical)
2412 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2413 else
2414 d.GeomSetCollideBits(prim_geom, 0);
2415 }
2416 else
2417 {
2418 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2419 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2420 }
2421 if(!m_isSelected)
2422 d.GeomEnable(prim_geom);
2423 }
2424 }
2425
2426 private void changeSelectedStatus(bool newval)
2427 {
2428 if (m_lastdoneSelected == newval)
2429 return;
2430
2431 m_lastdoneSelected = newval;
2432 DoSelectedStatus(newval);
2433 }
2434
2435 private void CheckDelaySelect()
2436 {
2437 if (m_delaySelect)
2438 {
2439 DoSelectedStatus(m_isSelected);
2440 }
2441 }
2442
2443 private void DoSelectedStatus(bool newval)
2444 {
2445 m_isSelected = newval;
2446 Stop();
2447
2448 if (newval)
2449 {
2450 if (!childPrim && Body != IntPtr.Zero)
2451 d.BodyDisable(Body);
2452
2453 if (m_delaySelect || m_isphysical)
2454 {
2455 m_collisionCategories = CollisionCategories.Selected;
2456 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2457
2458 if (!childPrim)
2459 {
2460 foreach (OdePrim prm in childrenPrim)
2461 {
2462 prm.m_collisionCategories = m_collisionCategories;
2463 prm.m_collisionFlags = m_collisionFlags;
2464
2465 if (prm.prim_geom != null)
2466 {
2467
2468 if (prm.m_NoColide)
2469 {
2470 d.GeomSetCategoryBits(prm.prim_geom, 0);
2471 d.GeomSetCollideBits(prm.prim_geom, 0);
2472 }
2473 else
2474 {
2475 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2476 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2477 }
2478 d.GeomDisable(prm.prim_geom);
2479 }
2480 prm.m_delaySelect = false;
2481 }
2482 }
2483
2484 if (prim_geom != null)
2485 {
2486 if (m_NoColide)
2487 {
2488 d.GeomSetCategoryBits(prim_geom, 0);
2489 d.GeomSetCollideBits(prim_geom, 0);
2490 }
2491 else
2492 {
2493 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2494 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2495 }
2496 d.GeomDisable(prim_geom);
2497 }
2498
2499 m_delaySelect = false;
2500 }
2501 else if(!m_isphysical)
2502 {
2503 m_delaySelect = true;
2504 }
2505 }
2506 else
2507 {
2508 if (!childPrim && Body != IntPtr.Zero && !m_disabled)
2509 d.BodyEnable(Body);
2510
2511 if (m_isphantom && !m_isVolumeDetect)
2512 {
2513 m_collisionCategories = 0;
2514 if(m_isphysical)
2515 m_collisionFlags = CollisionCategories.Land;
2516 else
2517 m_collisionFlags = 0;
2518 }
2519 else
2520 {
2521 m_collisionCategories = CollisionCategories.Geom;
2522 if (m_isphysical)
2523 m_collisionCategories |= CollisionCategories.Body;
2524
2525 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2526
2527 if (m_collidesWater)
2528 m_collisionFlags |= CollisionCategories.Water;
2529 }
2530
2531 if (!childPrim)
2532 {
2533 foreach (OdePrim prm in childrenPrim)
2534 {
2535 prm.m_collisionCategories = m_collisionCategories;
2536 prm.m_collisionFlags = m_collisionFlags;
2537
2538 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2539 {
2540 if (prm.m_NoColide)
2541 {
2542 d.GeomSetCategoryBits(prm.prim_geom, 0);
2543 if (m_isphysical)
2544 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2545 else
2546 d.GeomSetCollideBits(prm.prim_geom, 0);
2547 }
2548 else
2549 {
2550 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2551 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2552 }
2553 d.GeomEnable(prm.prim_geom);
2554 }
2555 prm.m_delaySelect = false;
2556 prm.m_softcolide = true;
2557 }
2558 }
2559
2560 if (!m_disabled && prim_geom != IntPtr.Zero)
2561 {
2562 if (m_NoColide)
2563 {
2564 d.GeomSetCategoryBits(prim_geom, 0);
2565 if (m_isphysical)
2566 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2567 else
2568 d.GeomSetCollideBits(prim_geom, 0);
2569 }
2570 else
2571 {
2572 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2573 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2574 }
2575 d.GeomEnable(prim_geom);
2576 }
2577
2578 m_delaySelect = false;
2579 m_softcolide = true;
2580 }
2581
2582 resetCollisionAccounting();
2583 }
2584
2585 private void changePosition(Vector3 newPos)
2586 {
2587 CheckDelaySelect();
2588 if (m_isphysical)
2589 {
2590 if (childPrim) // inertia is messed, must rebuild
2591 {
2592 if (m_building)
2593 {
2594 _position = newPos;
2595 }
2596 }
2597 else
2598 {
2599 if (_position != newPos)
2600 {
2601 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2602 _position = newPos;
2603 }
2604 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2605 d.BodyEnable(Body);
2606 }
2607 }
2608 else
2609 {
2610 if (prim_geom != IntPtr.Zero)
2611 {
2612 if (newPos != _position)
2613 {
2614 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2615 _position = newPos;
2616
2617 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2618 }
2619 }
2620 }
2621 givefakepos--;
2622 if (givefakepos < 0)
2623 givefakepos = 0;
2624 // changeSelectedStatus();
2625 m_softcolide = true;
2626 resetCollisionAccounting();
2627 }
2628
2629 private void changeOrientation(Quaternion newOri)
2630 {
2631 CheckDelaySelect();
2632 if (m_isphysical)
2633 {
2634 if (childPrim) // inertia is messed, must rebuild
2635 {
2636 if (m_building)
2637 {
2638 _orientation = newOri;
2639 }
2640 }
2641 else
2642 {
2643 if (newOri != _orientation)
2644 {
2645 d.Quaternion myrot = new d.Quaternion();
2646 myrot.X = newOri.X;
2647 myrot.Y = newOri.Y;
2648 myrot.Z = newOri.Z;
2649 myrot.W = newOri.W;
2650 d.GeomSetQuaternion(prim_geom, ref myrot);
2651 _orientation = newOri;
2652 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2653 createAMotor(m_angularlock);
2654 }
2655 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2656 d.BodyEnable(Body);
2657 }
2658 }
2659 else
2660 {
2661 if (prim_geom != IntPtr.Zero)
2662 {
2663 if (newOri != _orientation)
2664 {
2665 d.Quaternion myrot = new d.Quaternion();
2666 myrot.X = newOri.X;
2667 myrot.Y = newOri.Y;
2668 myrot.Z = newOri.Z;
2669 myrot.W = newOri.W;
2670 d.GeomSetQuaternion(prim_geom, ref myrot);
2671 _orientation = newOri;
2672 }
2673 }
2674 }
2675 givefakeori--;
2676 if (givefakeori < 0)
2677 givefakeori = 0;
2678 m_softcolide = true;
2679 resetCollisionAccounting();
2680 }
2681
2682 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2683 {
2684 CheckDelaySelect();
2685 if (m_isphysical)
2686 {
2687 if (childPrim && m_building) // inertia is messed, must rebuild
2688 {
2689 _position = newPos;
2690 _orientation = newOri;
2691 }
2692 else
2693 {
2694 if (newOri != _orientation)
2695 {
2696 d.Quaternion myrot = new d.Quaternion();
2697 myrot.X = newOri.X;
2698 myrot.Y = newOri.Y;
2699 myrot.Z = newOri.Z;
2700 myrot.W = newOri.W;
2701 d.GeomSetQuaternion(prim_geom, ref myrot);
2702 _orientation = newOri;
2703 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2704 createAMotor(m_angularlock);
2705 }
2706 if (_position != newPos)
2707 {
2708 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2709 _position = newPos;
2710 }
2711 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2712 d.BodyEnable(Body);
2713 }
2714 }
2715 else
2716 {
2717 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2718 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2719
2720 if (prim_geom != IntPtr.Zero)
2721 {
2722 if (newOri != _orientation)
2723 {
2724 d.Quaternion myrot = new d.Quaternion();
2725 myrot.X = newOri.X;
2726 myrot.Y = newOri.Y;
2727 myrot.Z = newOri.Z;
2728 myrot.W = newOri.W;
2729 d.GeomSetQuaternion(prim_geom, ref myrot);
2730 _orientation = newOri;
2731 }
2732
2733 if (newPos != _position)
2734 {
2735 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2736 _position = newPos;
2737
2738 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2739 }
2740 }
2741 }
2742 givefakepos--;
2743 if (givefakepos < 0)
2744 givefakepos = 0;
2745 givefakeori--;
2746 if (givefakeori < 0)
2747 givefakeori = 0;
2748
2749 m_softcolide = true;
2750 resetCollisionAccounting();
2751 }
2752
2753
2754 private void changeDisable(bool disable)
2755 {
2756 if (disable)
2757 {
2758 if (!m_disabled)
2759 disableBodySoft();
2760 }
2761 else
2762 {
2763 if (m_disabled)
2764 enableBodySoft();
2765 }
2766 }
2767
2768 private void changePhysicsStatus(bool NewStatus)
2769 {
2770 CheckDelaySelect();
2771
2772 m_isphysical = NewStatus;
2773
2774 if (!childPrim)
2775 {
2776 if (NewStatus)
2777 {
2778 if (Body == IntPtr.Zero)
2779 MakeBody();
2780 }
2781 else
2782 {
2783 if (Body != IntPtr.Zero)
2784 {
2785 DestroyBody();
2786 }
2787 Stop();
2788 }
2789 }
2790
2791 resetCollisionAccounting();
2792 }
2793
2794 private void changeprimsizeshape()
2795 {
2796 CheckDelaySelect();
2797
2798 OdePrim parent = (OdePrim)_parent;
2799
2800 bool chp = childPrim;
2801
2802 if (chp)
2803 {
2804 if (parent != null)
2805 {
2806 parent.DestroyBody();
2807 }
2808 }
2809 else
2810 {
2811 DestroyBody();
2812 }
2813
2814 RemoveGeom();
2815
2816 // we don't need to do space calculation because the client sends a position update also.
2817 if (_size.X <= 0)
2818 _size.X = 0.01f;
2819 if (_size.Y <= 0)
2820 _size.Y = 0.01f;
2821 if (_size.Z <= 0)
2822 _size.Z = 0.01f;
2823 // Construction of new prim
2824
2825 CreateGeom();
2826
2827 if (prim_geom != IntPtr.Zero)
2828 {
2829 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2830 d.Quaternion myrot = new d.Quaternion();
2831 myrot.X = _orientation.X;
2832 myrot.Y = _orientation.Y;
2833 myrot.Z = _orientation.Z;
2834 myrot.W = _orientation.W;
2835 d.GeomSetQuaternion(prim_geom, ref myrot);
2836 }
2837
2838 if (chp)
2839 {
2840 if (parent != null)
2841 {
2842 parent.MakeBody();
2843 }
2844 }
2845 else
2846 MakeBody();
2847
2848 m_softcolide = true;
2849 resetCollisionAccounting();
2850 }
2851
2852 private void changeSize(Vector3 newSize)
2853 {
2854 _size = newSize;
2855 changeprimsizeshape();
2856 }
2857
2858 private void changeShape(PrimitiveBaseShape newShape)
2859 {
2860 if(newShape != null)
2861 _pbs = newShape;
2862 changeprimsizeshape();
2863 }
2864
2865 private void changeFloatOnWater(bool newval)
2866 {
2867 m_collidesWater = newval;
2868
2869 if (prim_geom != IntPtr.Zero && !m_isphantom)
2870 {
2871 if (m_collidesWater)
2872 {
2873 m_collisionFlags |= CollisionCategories.Water;
2874 }
2875 else
2876 {
2877 m_collisionFlags &= ~CollisionCategories.Water;
2878 }
2879 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2880 }
2881 }
2882
2883 private void changeSetTorque(Vector3 newtorque)
2884 {
2885 if (!m_isSelected)
2886 {
2887 if (m_isphysical && Body != IntPtr.Zero)
2888 {
2889 if (m_disabled)
2890 enableBodySoft();
2891 else if (!d.BodyIsEnabled(Body))
2892 d.BodyEnable(Body);
2893
2894 }
2895 _torque = newtorque;
2896 }
2897 }
2898
2899 private void changeForce(Vector3 force)
2900 {
2901 m_force = force;
2902 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2903 d.BodyEnable(Body);
2904 }
2905
2906 private void changeAddForce(Vector3 force)
2907 {
2908 m_forceacc += force;
2909 if (!m_isSelected)
2910 {
2911 lock (this)
2912 {
2913 //m_log.Info("[PHYSICS]: dequeing forcelist");
2914 if (m_isphysical && Body != IntPtr.Zero)
2915 {
2916 if (m_disabled)
2917 enableBodySoft();
2918 else if (!d.BodyIsEnabled(Body))
2919 d.BodyEnable(Body);
2920 }
2921 }
2922
2923 m_collisionscore = 0;
2924 }
2925 }
2926
2927 private void changeAddAngularForce(Vector3 aforce)
2928 {
2929 m_angularForceacc += aforce;
2930 if (!m_isSelected)
2931 {
2932 lock (this)
2933 {
2934 if (m_isphysical && Body != IntPtr.Zero)
2935 {
2936 if (m_disabled)
2937 enableBodySoft();
2938 else if (!d.BodyIsEnabled(Body))
2939 d.BodyEnable(Body);
2940 }
2941 }
2942 m_collisionscore = 0;
2943 }
2944 }
2945
2946 private void changevelocity(Vector3 newVel)
2947 {
2948 if (!m_isSelected)
2949 {
2950 if (Body != IntPtr.Zero)
2951 {
2952 if (m_disabled)
2953 enableBodySoft();
2954 else if (!d.BodyIsEnabled(Body))
2955 d.BodyEnable(Body);
2956
2957 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
2958 }
2959 //resetCollisionAccounting();
2960 }
2961 _velocity = newVel;
2962 }
2963
2964 private void changeVolumedetetion(bool newVolDtc)
2965 {
2966 m_isVolumeDetect = newVolDtc;
2967 }
2968
2969 protected void changeBuilding(bool newbuilding)
2970 {
2971 if ((bool)newbuilding)
2972 {
2973 m_building = true;
2974 if (!childPrim)
2975 DestroyBody();
2976 }
2977 else
2978 {
2979 m_building = false;
2980 CheckDelaySelect();
2981 if (!childPrim)
2982 MakeBody();
2983 }
2984 if (!childPrim && childrenPrim.Count > 0)
2985 {
2986 foreach (OdePrim prm in childrenPrim)
2987 prm.changeBuilding(m_building); // call directly
2988 }
2989 }
2990
2991 public void changeSetVehicle(VehicleData vdata)
2992 {
2993 if (m_vehicle == null)
2994 m_vehicle = new ODEDynamics(this);
2995 m_vehicle.DoSetVehicle(vdata);
2996 }
2997 private void changeVehicleType(int value)
2998 {
2999 if (value == (int)Vehicle.TYPE_NONE)
3000 {
3001 if (m_vehicle != null)
3002 m_vehicle = null;
3003 }
3004 else
3005 {
3006 if (m_vehicle == null)
3007 m_vehicle = new ODEDynamics(this);
3008
3009 m_vehicle.ProcessTypeChange((Vehicle)value);
3010 }
3011 }
3012
3013 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3014 {
3015 if (m_vehicle == null)
3016 return;
3017
3018 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3019 }
3020
3021 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3022 {
3023 if (m_vehicle == null)
3024 return;
3025 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3026 }
3027
3028 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3029 {
3030 if (m_vehicle == null)
3031 return;
3032 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3033 }
3034
3035 private void changeVehicleFlags(strVehicleBoolParam bp)
3036 {
3037 if (m_vehicle == null)
3038 return;
3039 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3040 }
3041
3042 #endregion
3043
3044 public void Move()
3045 {
3046 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3047 !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds)
3048 // !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3049 {
3050// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3051
3052 float timestep = _parent_scene.ODE_STEPSIZE;
3053
3054 // check outside region
3055 d.Vector3 lpos;
3056 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3057
3058 if (lpos.Z < -100 || lpos.Z > 100000f)
3059 {
3060 m_outbounds = true;
3061
3062 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3063 _acceleration.X = 0;
3064 _acceleration.Y = 0;
3065 _acceleration.Z = 0;
3066
3067 _velocity.X = 0;
3068 _velocity.Y = 0;
3069 _velocity.Z = 0;
3070 m_rotationalVelocity.X = 0;
3071 m_rotationalVelocity.Y = 0;
3072 m_rotationalVelocity.Z = 0;
3073
3074 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3075 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3076 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3077 m_lastposition = _position;
3078 m_lastorientation = _orientation;
3079
3080 base.RequestPhysicsterseUpdate();
3081
3082 m_throttleUpdates = false;
3083 throttleCounter = 0;
3084 _zeroFlag = true;
3085
3086 disableBodySoft(); // disable it and colisions
3087 base.RaiseOutOfBounds(_position);
3088 return;
3089 }
3090
3091 if (lpos.X < 0f)
3092 {
3093 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3094 m_outbounds = true;
3095 }
3096 else if(lpos.X > _parent_scene.WorldExtents.X)
3097 {
3098 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3099 m_outbounds = true;
3100 }
3101 if (lpos.Y < 0f)
3102 {
3103 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3104 m_outbounds = true;
3105 }
3106 else if(lpos.Y > _parent_scene.WorldExtents.Y)
3107 {
3108 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3109 m_outbounds = true;
3110 }
3111
3112 if(m_outbounds)
3113 {
3114 m_lastposition = _position;
3115 m_lastorientation = _orientation;
3116
3117 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3118 m_rotationalVelocity.X = dtmp.X;
3119 m_rotationalVelocity.Y = dtmp.Y;
3120 m_rotationalVelocity.Z = dtmp.Z;
3121
3122 dtmp = d.BodyGetLinearVel(Body);
3123 _velocity.X = dtmp.X;
3124 _velocity.Y = dtmp.Y;
3125 _velocity.Z = dtmp.Z;
3126
3127 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3128 d.BodySetAngularVel(Body, 0, 0, 0);
3129 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3130 disableBodySoft(); // stop collisions
3131 base.RequestPhysicsterseUpdate();
3132 return;
3133 }
3134
3135
3136 float fx = 0;
3137 float fy = 0;
3138 float fz = 0;
3139
3140 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3141 {
3142 // 'VEHICLES' are dealt with in ODEDynamics.cs
3143 m_vehicle.Step();
3144 }
3145 else
3146 {
3147 float m_mass = _mass;
3148
3149 // fz = 0f;
3150 //m_log.Info(m_collisionFlags.ToString());
3151 if (m_usePID)
3152 {
3153
3154 // If the PID Controller isn't active then we set our force
3155 // calculating base velocity to the current position
3156
3157 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3158 {
3159 //PID_G = PID_G / m_PIDTau;
3160 m_PIDTau = 1;
3161 }
3162
3163 if ((PID_G - m_PIDTau) <= 0)
3164 {
3165 PID_G = m_PIDTau + 1;
3166 }
3167
3168 d.Vector3 vel = d.BodyGetLinearVel(Body);
3169 d.Vector3 pos = d.BodyGetPosition(Body);
3170 _target_velocity =
3171 new Vector3(
3172 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3173 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3174 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3175 );
3176
3177 // if velocity is zero, use position control; otherwise, velocity control
3178
3179 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3180 {
3181 // keep track of where we stopped. No more slippin' & slidin'
3182
3183 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3184 // react to the physics scene by moving it's position.
3185 // Avatar to Avatar collisions
3186 // Prim to avatar collisions
3187
3188 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3189 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3190 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3191 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3192 d.BodySetLinearVel(Body, 0, 0, 0);
3193 d.BodyAddForce(Body, 0, 0, fz);
3194 return;
3195 }
3196 else
3197 {
3198 _zeroFlag = false;
3199
3200 // We're flying and colliding with something
3201 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3202 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3203
3204 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3205
3206 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3207 }
3208 } // end if (m_usePID)
3209
3210 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3211 else if (m_useHoverPID)
3212 {
3213 //Console.WriteLine("Hover " + Name);
3214
3215 // If we're using the PID controller, then we have no gravity
3216
3217 // no lock; for now it's only called from within Simulate()
3218
3219 // If the PID Controller isn't active then we set our force
3220 // calculating base velocity to the current position
3221
3222 if ((m_PIDTau < 1))
3223 {
3224 PID_G = PID_G / m_PIDTau;
3225 }
3226
3227 if ((PID_G - m_PIDTau) <= 0)
3228 {
3229 PID_G = m_PIDTau + 1;
3230 }
3231
3232 // Where are we, and where are we headed?
3233 d.Vector3 pos = d.BodyGetPosition(Body);
3234 d.Vector3 vel = d.BodyGetLinearVel(Body);
3235
3236 // Non-Vehicles have a limited set of Hover options.
3237 // determine what our target height really is based on HoverType
3238 switch (m_PIDHoverType)
3239 {
3240 case PIDHoverType.Ground:
3241 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3242 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3243 break;
3244 case PIDHoverType.GroundAndWater:
3245 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3246 m_waterHeight = _parent_scene.GetWaterLevel();
3247 if (m_groundHeight > m_waterHeight)
3248 {
3249 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3250 }
3251 else
3252 {
3253 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3254 }
3255 break;
3256
3257 } // end switch (m_PIDHoverType)
3258
3259
3260 _target_velocity =
3261 new Vector3(0.0f, 0.0f,
3262 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3263 );
3264
3265 // if velocity is zero, use position control; otherwise, velocity control
3266
3267 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3268 {
3269 // keep track of where we stopped. No more slippin' & slidin'
3270
3271 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3272 // react to the physics scene by moving it's position.
3273 // Avatar to Avatar collisions
3274 // Prim to avatar collisions
3275
3276 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3277 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3278 // ? d.BodyAddForce(Body, 0, 0, fz);
3279 return;
3280 }
3281 else
3282 {
3283 _zeroFlag = false;
3284
3285 // We're flying and colliding with something
3286 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3287 }
3288 }
3289 else
3290 {
3291 float b = (1.0f - m_buoyancy);
3292 fx = _parent_scene.gravityx * b;
3293 fy = _parent_scene.gravityy * b;
3294 fz = _parent_scene.gravityz * b;
3295 }
3296
3297 fx *= m_mass;
3298 fy *= m_mass;
3299 fz *= m_mass;
3300
3301 // constant force
3302 fx += m_force.X;
3303 fy += m_force.Y;
3304 fz += m_force.Z;
3305
3306 fx += m_forceacc.X;
3307 fy += m_forceacc.Y;
3308 fz += m_forceacc.Z;
3309
3310 m_forceacc = Vector3.Zero;
3311
3312 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3313 if (fx != 0 || fy != 0 || fz != 0)
3314 {
3315 d.BodyAddForce(Body, fx, fy, fz);
3316 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3317 }
3318
3319 Vector3 trq;
3320
3321 trq = _torque;
3322 trq += m_angularForceacc;
3323 m_angularForceacc = Vector3.Zero;
3324 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3325 {
3326 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3327 }
3328
3329 }
3330 }
3331 else
3332 { // is not physical, or is not a body or is selected
3333 // _zeroPosition = d.BodyGetPosition(Body);
3334 return;
3335 //Console.WriteLine("Nothing " + Name);
3336
3337 }
3338 }
3339
3340
3341 public void UpdatePositionAndVelocity(float simulatedtime)
3342 {
3343 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
3344 if (_parent == null && !m_disabled && !m_building && !m_outbounds)
3345 {
3346 if (Body != IntPtr.Zero)
3347 {
3348 Vector3 pv = Vector3.Zero;
3349 bool lastZeroFlag = _zeroFlag;
3350
3351 d.Vector3 lpos;
3352 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3353
3354
3355 d.Quaternion ori;
3356 d.GeomCopyQuaternion(prim_geom, out ori);
3357 d.Vector3 vel = d.BodyGetLinearVel(Body);
3358 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3359
3360 if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
3361 && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
3362 && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
3363 && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)
3364 && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)
3365 && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)
3366 )
3367 {
3368 _zeroFlag = true;
3369 //Console.WriteLine("ZFT 2");
3370 m_throttleUpdates = false;
3371 }
3372 else
3373 {
3374 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3375 _zeroFlag = false;
3376 m_lastUpdateSent = false;
3377 //m_throttleUpdates = false;
3378 }
3379
3380 if (_zeroFlag)
3381 {
3382 m_lastposition = _position;
3383 m_lastorientation = _orientation;
3384
3385 _velocity.X = 0.0f;
3386 _velocity.Y = 0.0f;
3387 _velocity.Z = 0.0f;
3388
3389 _acceleration.X = 0;
3390 _acceleration.Y = 0;
3391 _acceleration.Z = 0;
3392
3393 m_rotationalVelocity.X = 0;
3394 m_rotationalVelocity.Y = 0;
3395 m_rotationalVelocity.Z = 0;
3396 if (!m_lastUpdateSent)
3397 {
3398 m_throttleUpdates = false;
3399 throttleCounter = 0;
3400 m_rotationalVelocity = pv;
3401
3402 base.RequestPhysicsterseUpdate();
3403
3404 m_lastUpdateSent = true;
3405 }
3406 }
3407 else
3408 {
3409 if (lastZeroFlag != _zeroFlag)
3410 {
3411 base.RequestPhysicsterseUpdate();
3412 }
3413
3414 m_lastVelocity = _velocity;
3415
3416 _position.X = lpos.X;
3417 _position.Y = lpos.Y;
3418 _position.Z = lpos.Z;
3419
3420 _velocity.X = vel.X;
3421 _velocity.Y = vel.Y;
3422 _velocity.Z = vel.Z;
3423
3424 _orientation.X = ori.X;
3425 _orientation.Y = ori.Y;
3426 _orientation.Z = ori.Z;
3427 _orientation.W = ori.W;
3428
3429 _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);
3430
3431 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
3432 {
3433 m_rotationalVelocity = pv;
3434 }
3435 else
3436 {
3437 m_rotationalVelocity.X = rotvel.X;
3438 m_rotationalVelocity.Y = rotvel.Y;
3439 m_rotationalVelocity.Z = rotvel.Z;
3440 }
3441
3442 m_lastUpdateSent = false;
3443 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3444 {
3445 m_lastposition = _position;
3446 m_lastorientation = _orientation;
3447 base.RequestPhysicsterseUpdate();
3448 }
3449 else
3450 {
3451 throttleCounter++;
3452 }
3453 }
3454 }
3455 else if (!m_lastUpdateSent || !_zeroFlag)
3456 {
3457 // Not a body.. so Make sure the client isn't interpolating
3458 _velocity.X = 0;
3459 _velocity.Y = 0;
3460 _velocity.Z = 0;
3461
3462 _acceleration.X = 0;
3463 _acceleration.Y = 0;
3464 _acceleration.Z = 0;
3465
3466 m_rotationalVelocity.X = 0;
3467 m_rotationalVelocity.Y = 0;
3468 m_rotationalVelocity.Z = 0;
3469 _zeroFlag = true;
3470
3471 if (!m_lastUpdateSent)
3472 {
3473 m_throttleUpdates = false;
3474 throttleCounter = 0;
3475
3476 base.RequestPhysicsterseUpdate();
3477
3478 m_lastUpdateSent = true;
3479 }
3480 }
3481 }
3482 }
3483
3484 internal static bool QuaternionIsFinite(Quaternion q)
3485 {
3486 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3487 return false;
3488 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3489 return false;
3490 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3491 return false;
3492 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3493 return false;
3494 return true;
3495 }
3496
3497 internal static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3498 {
3499 dst.c.W = src.c.W;
3500 dst.c.X = src.c.X;
3501 dst.c.Y = src.c.Y;
3502 dst.c.Z = src.c.Z;
3503 dst.mass = src.mass;
3504 dst.I.M00 = src.I.M00;
3505 dst.I.M01 = src.I.M01;
3506 dst.I.M02 = src.I.M02;
3507 dst.I.M10 = src.I.M10;
3508 dst.I.M11 = src.I.M11;
3509 dst.I.M12 = src.I.M12;
3510 dst.I.M20 = src.I.M20;
3511 dst.I.M21 = src.I.M21;
3512 dst.I.M22 = src.I.M22;
3513 }
3514
3515 private static void DMassDup(ref d.Mass src, out d.Mass dst)
3516 {
3517 dst = new d.Mass { };
3518
3519 dst.c.W = src.c.W;
3520 dst.c.X = src.c.X;
3521 dst.c.Y = src.c.Y;
3522 dst.c.Z = src.c.Z;
3523 dst.mass = src.mass;
3524 dst.I.M00 = src.I.M00;
3525 dst.I.M01 = src.I.M01;
3526 dst.I.M02 = src.I.M02;
3527 dst.I.M10 = src.I.M10;
3528 dst.I.M11 = src.I.M11;
3529 dst.I.M12 = src.I.M12;
3530 dst.I.M20 = src.I.M20;
3531 dst.I.M21 = src.I.M21;
3532 dst.I.M22 = src.I.M22;
3533 }
3534 private void donullchange()
3535 {
3536 }
3537
3538 public bool DoAChange(changes what, object arg)
3539 {
3540 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3541 {
3542 return false;
3543 }
3544
3545 // nasty switch
3546 switch (what)
3547 {
3548 case changes.Add:
3549 changeadd();
3550 break;
3551 case changes.Remove:
3552 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3553 //When we return true, it destroys all of the prims in the linkset anyway
3554 if (_parent != null)
3555 {
3556 OdePrim parent = (OdePrim)_parent;
3557 parent.ChildRemove(this, false);
3558 }
3559 else
3560 ChildRemove(this, false);
3561
3562 m_vehicle = null;
3563 RemoveGeom();
3564 m_targetSpace = IntPtr.Zero;
3565 if (m_eventsubscription > 0)
3566 UnSubscribeEvents();
3567 return true;
3568
3569 case changes.Link:
3570 OdePrim tmp = (OdePrim)arg;
3571 changeLink(tmp);
3572 break;
3573
3574 case changes.DeLink:
3575 changeLink(null);
3576 break;
3577
3578 case changes.Position:
3579 changePosition((Vector3)arg);
3580 break;
3581
3582 case changes.Orientation:
3583 changeOrientation((Quaternion)arg);
3584 break;
3585
3586 case changes.PosOffset:
3587 donullchange();
3588 break;
3589
3590 case changes.OriOffset:
3591 donullchange();
3592 break;
3593
3594 case changes.Velocity:
3595 changevelocity((Vector3)arg);
3596 break;
3597
3598 // case changes.Acceleration:
3599 // changeacceleration((Vector3)arg);
3600 // break;
3601 // case changes.AngVelocity:
3602 // changeangvelocity((Vector3)arg);
3603 // break;
3604
3605 case changes.Force:
3606 changeForce((Vector3)arg);
3607 break;
3608
3609 case changes.Torque:
3610 changeSetTorque((Vector3)arg);
3611 break;
3612
3613 case changes.AddForce:
3614 changeAddForce((Vector3)arg);
3615 break;
3616
3617 case changes.AddAngForce:
3618 changeAddAngularForce((Vector3)arg);
3619 break;
3620
3621 case changes.AngLock:
3622 changeAngularLock((Vector3)arg);
3623 break;
3624
3625 case changes.Size:
3626 changeSize((Vector3)arg);
3627 break;
3628
3629 case changes.Shape:
3630 changeShape((PrimitiveBaseShape)arg);
3631 break;
3632
3633 case changes.CollidesWater:
3634 changeFloatOnWater((bool)arg);
3635 break;
3636
3637 case changes.VolumeDtc:
3638 changeVolumedetetion((bool)arg);
3639 break;
3640
3641 case changes.Phantom:
3642 changePhantomStatus((bool)arg);
3643 break;
3644
3645 case changes.Physical:
3646 changePhysicsStatus((bool)arg);
3647 break;
3648
3649 case changes.Selected:
3650 changeSelectedStatus((bool)arg);
3651 break;
3652
3653 case changes.disabled:
3654 changeDisable((bool)arg);
3655 break;
3656
3657 case changes.building:
3658 changeBuilding((bool)arg);
3659 break;
3660
3661 case changes.VehicleType:
3662 changeVehicleType((int)arg);
3663 break;
3664
3665 case changes.VehicleFlags:
3666 changeVehicleFlags((strVehicleBoolParam) arg);
3667 break;
3668
3669 case changes.VehicleFloatParam:
3670 changeVehicleFloatParam((strVehicleFloatParam) arg);
3671 break;
3672
3673 case changes.VehicleVectorParam:
3674 changeVehicleVectorParam((strVehicleVectorParam) arg);
3675 break;
3676
3677 case changes.VehicleRotationParam:
3678 changeVehicleRotationParam((strVehicleQuatParam) arg);
3679 break;
3680
3681 case changes.SetVehicle:
3682 changeSetVehicle((VehicleData) arg);
3683 break;
3684 case changes.Null:
3685 donullchange();
3686 break;
3687
3688 default:
3689 donullchange();
3690 break;
3691 }
3692 return false;
3693 }
3694
3695 public void AddChange(changes what, object arg)
3696 {
3697 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3698 }
3699
3700
3701 private struct strVehicleBoolParam
3702 {
3703 public int param;
3704 public bool value;
3705 }
3706
3707 private struct strVehicleFloatParam
3708 {
3709 public int param;
3710 public float value;
3711 }
3712
3713 private struct strVehicleQuatParam
3714 {
3715 public int param;
3716 public Quaternion value;
3717 }
3718
3719 private struct strVehicleVectorParam
3720 {
3721 public int param;
3722 public Vector3 value;
3723 }
3724 }
3725} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..4b3f83b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,443 @@
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 OdeAPI;
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 ray requests
49 /// </summary>
50 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
51
52 /// <summary>
53 /// Scene that created this object.
54 /// </summary>
55 private OdeScene m_scene;
56
57 IntPtr ray;
58
59 private const int ColisionContactGeomsPerTest = 5;
60
61 /// <summary>
62 /// ODE near callback delegate
63 /// </summary>
64 private d.NearCallback nearCallback;
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66 private List<ContactResult> m_contactResults = new List<ContactResult>();
67
68 public ODERayCastRequestManager(OdeScene pScene)
69 {
70 m_scene = pScene;
71 nearCallback = near;
72 ray = d.CreateRay(IntPtr.Zero, 1.0f);
73 }
74
75 /// <summary>
76 /// Queues a raycast
77 /// </summary>
78 /// <param name="position">Origin of Ray</param>
79 /// <param name="direction">Ray normal</param>
80 /// <param name="length">Ray length</param>
81 /// <param name="retMethod">Return method to send the results</param>
82 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
83 {
84 ODERayRequest req = new ODERayRequest();
85 req.geom = IntPtr.Zero;
86 req.callbackMethod = retMethod;
87 req.Count = 0;
88 req.length = length;
89 req.Normal = direction;
90 req.Origin = position;
91
92 m_PendingRequests.Enqueue(req);
93 }
94
95 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
96 {
97 ODERayRequest req = new ODERayRequest();
98 req.geom = geom;
99 req.callbackMethod = retMethod;
100 req.length = length;
101 req.Normal = direction;
102 req.Origin = position;
103 req.Count = 0;
104
105 m_PendingRequests.Enqueue(req);
106 }
107
108 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
109 {
110 ODERayRequest req = new ODERayRequest();
111 req.geom = IntPtr.Zero;
112 req.callbackMethod = retMethod;
113 req.Count = 0;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117
118 m_PendingRequests.Enqueue(req);
119 }
120
121 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
122 {
123 ODERayRequest req = new ODERayRequest();
124 req.geom = geom;
125 req.callbackMethod = retMethod;
126 req.length = length;
127 req.Normal = direction;
128 req.Origin = position;
129 req.Count = 0;
130
131 m_PendingRequests.Enqueue(req);
132 }
133
134 /// <summary>
135 /// Queues a raycast
136 /// </summary>
137 /// <param name="position">Origin of Ray</param>
138 /// <param name="direction">Ray normal</param>
139 /// <param name="length">Ray length</param>
140 /// <param name="count"></param>
141 /// <param name="retMethod">Return method to send the results</param>
142 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
143 {
144 ODERayRequest req = new ODERayRequest();
145 req.geom = IntPtr.Zero;
146 req.callbackMethod = retMethod;
147 req.length = length;
148 req.Normal = direction;
149 req.Origin = position;
150 req.Count = count;
151
152 m_PendingRequests.Enqueue(req);
153 }
154
155 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
156 {
157 ODERayRequest req = new ODERayRequest();
158 req.geom = geom;
159 req.callbackMethod = retMethod;
160 req.length = length;
161 req.Normal = direction;
162 req.Origin = position;
163 req.Count = count;
164
165 m_PendingRequests.Enqueue(req);
166 }
167
168 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
169 {
170 ODERayRequest req = new ODERayRequest();
171 req.geom = IntPtr.Zero;
172 req.callbackMethod = retMethod;
173 req.length = length;
174 req.Normal = direction;
175 req.Origin = position;
176 req.Count = count;
177
178 m_PendingRequests.Enqueue(req);
179 }
180
181 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
182 {
183 ODERayRequest req = new ODERayRequest();
184 req.geom = geom;
185 req.callbackMethod = retMethod;
186 req.length = length;
187 req.Normal = direction;
188 req.Origin = position;
189 req.Count = count;
190
191 m_PendingRequests.Enqueue(req);
192 }
193
194 /// <summary>
195 /// Process all queued raycast requests
196 /// </summary>
197 /// <returns>Time in MS the raycasts took to process.</returns>
198 public int ProcessQueuedRequests()
199 {
200 int time = System.Environment.TickCount;
201
202 if (m_PendingRequests.Count <= 0)
203 return 0;
204
205 if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still
206 {
207 m_PendingRequests.Clear();
208 return 0;
209 }
210
211 ODERayRequest req;
212
213 int i = 50; // arbitary limit of processed tests per frame
214
215 while(m_PendingRequests.Dequeue(out req))
216 {
217 if (req.geom == IntPtr.Zero)
218 doSpaceRay(req);
219 else
220 doGeomRay(req);
221 if(--i < 0)
222 break;
223 }
224
225 lock (m_contactResults)
226 m_contactResults.Clear();
227
228 return System.Environment.TickCount - time;
229 }
230 /// <summary>
231 /// Method that actually initiates the raycast with full top space
232 /// </summary>
233 /// <param name="req"></param>
234 private void doSpaceRay(ODERayRequest req)
235 {
236 // Create the ray
237// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
238 d.GeomRaySetLength(ray, req.length);
239 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
240
241 // Collide test
242 d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback);
243
244 // Remove Ray
245// d.GeomDestroy(ray);
246
247 if (req.callbackMethod == null)
248 return;
249
250 if (req.callbackMethod is RaycastCallback)
251 {
252 // Define default results
253 bool hitYN = false;
254 uint hitConsumerID = 0;
255 float distance = 999999999999f;
256 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
257 Vector3 snormal = Vector3.Zero;
258
259 // Find closest contact and object.
260 lock (m_contactResults)
261 {
262 foreach (ContactResult cResult in m_contactResults)
263 {
264 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
265 {
266 closestcontact = cResult.Pos;
267 hitConsumerID = cResult.ConsumerID;
268 distance = cResult.Depth;
269 hitYN = true;
270 snormal = cResult.Normal;
271 }
272 }
273 m_contactResults.Clear();
274 }
275
276 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
277 }
278 else
279 {
280 ((RayCallback)req.callbackMethod)(m_contactResults);
281 lock (m_PendingRequests)
282 m_contactResults.Clear();
283 }
284 }
285
286 /// <summary>
287 /// Method that actually initiates the raycast with a geom
288 /// </summary>
289 /// <param name="req"></param>
290 private void doGeomRay(ODERayRequest req)
291 {
292 // Create the ray
293// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
294 d.GeomRaySetLength(ray, req.length);
295 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
296
297 // Collide test
298 d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
299
300 // Remove Ray
301// d.GeomDestroy(ray);
302
303 if (req.callbackMethod == null)
304 return;
305
306 if (req.callbackMethod is RaycastCallback)
307 {
308 // Define default results
309 bool hitYN = false;
310 uint hitConsumerID = 0;
311 float distance = 999999999999f;
312 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
313 Vector3 snormal = Vector3.Zero;
314
315 // Find closest contact and object.
316 lock (m_contactResults)
317 {
318 foreach (ContactResult cResult in m_contactResults)
319 {
320 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
321 {
322 closestcontact = cResult.Pos;
323 hitConsumerID = cResult.ConsumerID;
324 distance = cResult.Depth;
325 hitYN = true;
326 snormal = cResult.Normal;
327 }
328 }
329 m_contactResults.Clear();
330 }
331
332 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
333 }
334 else
335 {
336 ((RayCallback)req.callbackMethod)(m_contactResults);
337 lock (m_PendingRequests)
338 m_contactResults.Clear();
339 }
340 }
341
342 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
343 {
344 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
345 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
346 return false;
347
348 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
349 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
350 return true;
351 }
352
353 // This is the standard Near. g2 is the ray
354 private void near(IntPtr space, IntPtr g1, IntPtr g2)
355 {
356 //Don't test against heightfield Geom, or you'll be sorry!
357 // Exclude heightfield geom
358
359 if (g1 == IntPtr.Zero || g1 == g2)
360 return;
361
362 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
363 return;
364
365 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
366 if (d.GeomIsSpace(g1))
367 {
368 try
369 {
370 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
371 }
372 catch (Exception e)
373 {
374 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
375 }
376 return;
377 }
378
379 int count = 0;
380 try
381 {
382 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
383 }
384 catch (SEHException)
385 {
386 m_log.Error("[PHYSICS Ray]: 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.");
387 }
388 catch (Exception e)
389 {
390 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
391 return;
392 }
393
394 if (count == 0)
395 return;
396
397 PhysicsActor p1 = null;
398
399 if (g1 != IntPtr.Zero)
400 m_scene.actor_name_map.TryGetValue(g1, out p1);
401
402 d.ContactGeom curcontact = new d.ContactGeom();
403 // Loop over contacts, build results.
404 for (int i = 0; i < count; i++)
405 {
406 if (!GetCurContactGeom(i, ref curcontact))
407 break;
408 if (p1 != null) {
409 if (p1 is OdePrim)
410 {
411 ContactResult collisionresult = new ContactResult();
412
413 collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
414 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
415 collisionresult.Depth = curcontact.depth;
416 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
417 curcontact.normal.Z);
418 lock (m_contactResults)
419 m_contactResults.Add(collisionresult);
420 }
421 }
422 }
423 }
424
425 /// <summary>
426 /// Dereference the creator scene so that it can be garbage collected if needed.
427 /// </summary>
428 internal void Dispose()
429 {
430 m_scene = null;
431 }
432 }
433
434 public struct ODERayRequest
435 {
436 public IntPtr geom;
437 public Vector3 Origin;
438 public Vector3 Normal;
439 public int Count;
440 public float length;
441 public object callbackMethod;
442 }
443} \ 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..2b6bc59
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,1960 @@
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 FirstUserClass,
117 LastUserClass = FirstUserClass + MaxUserClasses - 1,
118 NumClasses,
119 MaxUserClasses = 4
120 }
121
122 public enum JointType : int
123 {
124 None,
125 Ball,
126 Hinge,
127 Slider,
128 Contact,
129 Universal,
130 Hinge2,
131 Fixed,
132 Null,
133 AMotor,
134 LMotor,
135 Plane2D
136 }
137
138 public enum JointParam : int
139 {
140 LoStop,
141 HiStop,
142 Vel,
143 FMax,
144 FudgeFactor,
145 Bounce,
146 CFM,
147 StopERP,
148 StopCFM,
149 SuspensionERP,
150 SuspensionCFM,
151 LoStop2 = 256,
152 HiStop2,
153 Vel2,
154 FMax2,
155 FudgeFactor2,
156 Bounce2,
157 CFM2,
158 StopERP2,
159 StopCFM2,
160 SuspensionERP2,
161 SuspensionCFM2,
162 LoStop3 = 512,
163 HiStop3,
164 Vel3,
165 FMax3,
166 FudgeFactor3,
167 Bounce3,
168 CFM3,
169 StopERP3,
170 StopCFM3,
171 SuspensionERP3,
172 SuspensionCFM3
173 }
174
175 public enum dSweepAndPruneAxis : int
176 {
177 XYZ = ((0)|(1<<2)|(2<<4)),
178 XZY = ((0)|(2<<2)|(1<<4)),
179 YXZ = ((1)|(0<<2)|(2<<4)),
180 YZX = ((1)|(2<<2)|(0<<4)),
181 ZXY = ((2)|(0<<2)|(1<<4)),
182 ZYX = ((2)|(1<<2)|(0<<4))
183 }
184
185 #endregion
186
187 #region Callbacks
188
189 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
190 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
191
192 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
193 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
194
195 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
196 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
197
198 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
199 public delegate ColliderFn GetColliderFnFn(int num);
200
201 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
202 public delegate void GeomDtorFn(IntPtr o);
203
204 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
205 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
206
207 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
208 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
209
210 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
211 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
212
213 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
214 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
215
216 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
217 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
218
219 #endregion
220
221 #region Structs
222
223 [StructLayout(LayoutKind.Sequential)]
224 public struct AABB
225 {
226 public dReal MinX, MaxX;
227 public dReal MinY, MaxY;
228 public dReal MinZ, MaxZ;
229 }
230
231
232 [StructLayout(LayoutKind.Sequential)]
233 public struct Contact
234 {
235 public SurfaceParameters surface;
236 public ContactGeom geom;
237 public Vector3 fdir1;
238 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
239 }
240
241
242 [StructLayout(LayoutKind.Sequential)]
243 public struct ContactGeom
244 {
245
246 public Vector3 pos;
247 public Vector3 normal;
248 public dReal depth;
249 public IntPtr g1;
250 public IntPtr g2;
251 public int side1;
252 public int side2;
253 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
254 }
255
256 [StructLayout(LayoutKind.Sequential)]
257 public struct GeomClass
258 {
259 public int bytes;
260 public GetColliderFnFn collider;
261 public GetAABBFn aabb;
262 public AABBTestFn aabb_test;
263 public GeomDtorFn dtor;
264 }
265
266
267 [StructLayout(LayoutKind.Sequential)]
268 public struct JointFeedback
269 {
270 public Vector3 f1;
271 public Vector3 t1;
272 public Vector3 f2;
273 public Vector3 t2;
274 }
275
276
277 [StructLayout(LayoutKind.Sequential)]
278 public struct Mass
279 {
280 public dReal mass;
281 public Vector4 c;
282 public Matrix3 I;
283 }
284
285
286 [StructLayout(LayoutKind.Sequential)]
287 public struct Matrix3
288 {
289 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
290 {
291 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
292 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
293 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
294 }
295 public dReal M00, M10, M20;
296 private dReal _m30;
297 public dReal M01, M11, M21;
298 private dReal _m31;
299 public dReal M02, M12, M22;
300 private dReal _m32;
301 }
302
303 [StructLayout(LayoutKind.Sequential)]
304 public struct Matrix4
305 {
306 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
307 dReal m01, dReal m11, dReal m21, dReal m31,
308 dReal m02, dReal m12, dReal m22, dReal m32,
309 dReal m03, dReal m13, dReal m23, dReal m33)
310 {
311 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
312 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
313 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
314 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
315 }
316 public dReal M00, M10, M20, M30;
317 public dReal M01, M11, M21, M31;
318 public dReal M02, M12, M22, M32;
319 public dReal M03, M13, M23, M33;
320 }
321
322 [StructLayout(LayoutKind.Sequential)]
323 public struct Quaternion
324 {
325 public dReal W, X, Y, Z;
326 }
327
328
329 [StructLayout(LayoutKind.Sequential)]
330 public struct SurfaceParameters
331 {
332 public ContactFlags mode;
333 public dReal mu;
334 public dReal mu2;
335 public dReal bounce;
336 public dReal bounce_vel;
337 public dReal soft_erp;
338 public dReal soft_cfm;
339 public dReal motion1;
340 public dReal motion2;
341 public dReal motionN;
342 public dReal slip1;
343 public dReal slip2;
344 }
345
346
347 [StructLayout(LayoutKind.Sequential)]
348 public struct Vector3
349 {
350 public Vector3(dReal x, dReal y, dReal z)
351 {
352 X = x; Y = y; Z = z; _w = 0.0f;
353 }
354 public dReal X, Y, Z;
355 private dReal _w;
356 }
357
358
359 [StructLayout(LayoutKind.Sequential)]
360 public struct Vector4
361 {
362 public Vector4(dReal x, dReal y, dReal z, dReal w)
363 {
364 X = x; Y = y; Z = z; W = w;
365 }
366 public dReal X, Y, Z, W;
367 }
368
369 #endregion
370
371 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
372 public static extern int AllocateODEDataForThread(uint ODEInitFlags);
373
374 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
375 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
376
377 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
378 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
379
380 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
381 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
382
383 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
384 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
385
386 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
387 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
388
389 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
390 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
391
392 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
393 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
394
395 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
396 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
397
398 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
399 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
400
401 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
402 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
403
404 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
405 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
406
407 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
408 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
409
410 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
411 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
412
413 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
414 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
415
416 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
417 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
418
419 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
420 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
421
422 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
423 public static extern IntPtr BodyiCreate(IntPtr world);
424 public static IntPtr BodyCreate(IntPtr world)
425 {
426 NTotalBodies++;
427 return BodyiCreate(world);
428 }
429
430 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
431 public static extern void BodyiDestroy(IntPtr body);
432 public static void BodyDestroy(IntPtr body)
433 {
434 NTotalBodies--;
435 BodyiDestroy(body);
436 }
437
438 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
439 public static extern void BodyDisable(IntPtr body);
440
441 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
442 public static extern void BodyEnable(IntPtr body);
443
444 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
445 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
446
447 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
448 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
449
450 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
451 public static extern void BodyGetAutoDisableDefaults(IntPtr body);
452
453 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
454 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
455
456 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
457 public static extern int BodyGetAutoDisableSteps(IntPtr body);
458
459 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
460 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
461
462 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
463 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
464 public static Vector3 BodyGetAngularVel(IntPtr body)
465 {
466 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
467 }
468
469 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
470 public static extern IntPtr BodyGetData(IntPtr body);
471
472 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
473 public static extern int BodyGetFiniteRotationMode(IntPtr body);
474
475 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
476 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
477
478 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
479 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
480 public static Vector3 BodyGetForce(IntPtr body)
481 {
482 unsafe { return *(BodyGetForceUnsafe(body)); }
483 }
484
485 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
486 public static extern bool BodyGetGravityMode(IntPtr body);
487
488 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
489 public static extern int BodyGetGyroscopicMode(IntPtr body);
490
491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
492 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
493
494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
495 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
496 public static Vector3 BodyGetLinearVel(IntPtr body)
497 {
498 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
499 }
500
501 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
502 public static extern void BodyGetMass(IntPtr body, out Mass mass);
503
504 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
505 public static extern int BodyGetNumJoints(IntPtr body);
506
507 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
508 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
509
510 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
511 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
512 public static Vector3 BodyGetPosition(IntPtr body)
513 {
514 unsafe { return *(BodyGetPositionUnsafe(body)); }
515 }
516
517 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
518 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
519
520 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
521 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
522 public static Quaternion BodyGetQuaternion(IntPtr body)
523 {
524 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
525 }
526
527 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
528 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
529
530 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
531 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
532
533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
534 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
535 public static Matrix3 BodyGetRotation(IntPtr body)
536 {
537 unsafe { return *(BodyGetRotationUnsafe(body)); }
538 }
539
540 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
541 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
542 public static Vector3 BodyGetTorque(IntPtr body)
543 {
544 unsafe { return *(BodyGetTorqueUnsafe(body)); }
545 }
546
547 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
548 public static extern IntPtr BodyGetWorld(IntPtr body);
549
550 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
551 public static extern IntPtr BodyGetFirstGeom(IntPtr body);
552
553 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
554 public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
555
556
557 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
558 public static extern bool BodyIsEnabled(IntPtr body);
559
560 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
561 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
562
563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
564 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
565
566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
567 public static extern void BodySetAutoDisableDefaults(IntPtr body);
568
569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
570 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
571
572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
573 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
574
575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
576 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
577
578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
579 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
580
581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
582 public static extern void BodySetData(IntPtr body, IntPtr data);
583
584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
585 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
586
587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
588 public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
589
590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
591 public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
592
593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
594 public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
595
596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
597 public static extern dReal BodyGetLinearDamping(IntPtr body);
598
599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
600 public static extern dReal BodyGetAngularDamping(IntPtr body);
601
602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
603 public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
604
605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
606 public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
607
608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
609 public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
610
611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
612 public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
613
614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
615 public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
616
617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
618 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
619
620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
621 public static extern void BodySetGravityMode(IntPtr body, bool mode);
622
623 /// <summary>
624 /// Sets the Gyroscopic term status on the body specified.
625 /// </summary>
626 /// <param name="body">Pointer to body</param>
627 /// <param name="enabled">NonZero enabled, Zero disabled</param>
628 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
629 public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
630
631 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
632 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
633
634 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
635 public static extern void BodySetMass(IntPtr body, ref Mass mass);
636
637 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
638 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
639
640 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
641 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
642
643 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
644 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
645
646 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
647 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
648
649 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
650 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
651
652 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
653 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
654
655 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
656 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
657
658 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
659 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
660
661 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
662 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
663 ref Vector3 side1, ref Vector3 p2,
664 ref Matrix3 R2, ref Vector3 side2,
665 ref Vector3 normal, out dReal depth, out int return_code,
666 int maxc, out ContactGeom contact, int skip);
667
668 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
669 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
670 ref Vector3 side1, ref Vector3 _p2,
671 ref Matrix3 R2, ref Vector3 side2);
672
673 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
674 public static extern void CleanupODEAllDataForThread();
675
676 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
677 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
678 ref Vector3 b1, ref Vector3 b2,
679 ref Vector3 cp1, ref Vector3 cp2);
680
681 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
682 public static extern void CloseODE();
683
684 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
685 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
686 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
687 public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
688
689 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
690 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
691
692 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
693 public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
694 public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
695 {
696 NTotalGeoms++;
697 return CreateiBox(space, lx, ly, lz);
698 }
699
700 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
701 public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
702 public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
703 {
704 NTotalGeoms++;
705 return CreateiCapsule(space, radius, length);
706 }
707
708 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
709 public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
710 public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
711 {
712 NTotalGeoms++;
713 return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
714 }
715
716 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
717 public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
718 public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
719 {
720 NTotalGeoms++;
721 return CreateiCylinder(space, radius, length);
722 }
723
724 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
725 public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
726 public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
727 {
728 NTotalGeoms++;
729 return CreateiHeightfield(space, data, bPlaceable);
730 }
731
732 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
733 public static extern IntPtr CreateiGeom(int classnum);
734 public static IntPtr CreateGeom(int classnum)
735 {
736 NTotalGeoms++;
737 return CreateiGeom(classnum);
738 }
739
740 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
741 public static extern int CreateGeomClass(ref GeomClass classptr);
742
743 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
744 public static extern IntPtr CreateGeomTransform(IntPtr space);
745
746 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
747 public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
748 public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
749 {
750 NTotalGeoms++;
751 return CreateiPlane(space, a, b, c, d);
752 }
753
754 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
755 public static extern IntPtr CreateiRay(IntPtr space, dReal length);
756 public static IntPtr CreateRay(IntPtr space, dReal length)
757 {
758 NTotalGeoms++;
759 return CreateiRay(space, length);
760 }
761
762 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
763 public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
764 public static IntPtr CreateSphere(IntPtr space, dReal radius)
765 {
766 NTotalGeoms++;
767 return CreateiSphere(space, radius);
768 }
769
770 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
771 public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
772 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
773 public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
774 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
775 {
776 NTotalGeoms++;
777 return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
778 }
779 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
780 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
781
782 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
783 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
784
785 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
786 public static extern int FactorCholesky(ref dReal A00, int n);
787
788 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
789 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
790
791 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
792 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
793
794 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
795 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
796
797 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
798 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
799
800 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
801 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
802
803 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
804 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
805
806 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
807 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
808
809 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
810 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
811
812 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
813 public static extern void GeomClearOffset(IntPtr geom);
814
815 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
816 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
817
818 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
819 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
820
821 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
822 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
823
824 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
825 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
826
827 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
828 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
829
830 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
831 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
832
833 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
834 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
835
836 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
837 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
838
839 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
840 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
841
842 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
843 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
844
845 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
846 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
847
848 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
849 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
850
851 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
852 public static extern void GeomiDestroy(IntPtr geom);
853 public static void GeomDestroy(IntPtr geom)
854 {
855 NTotalGeoms--;
856 GeomiDestroy(geom);
857 }
858
859
860 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
861 public static extern void GeomDisable(IntPtr geom);
862
863 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
864 public static extern void GeomEnable(IntPtr geom);
865
866 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
867 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
868
869 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
870 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
871
872 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
873 public static extern IntPtr GeomGetBody(IntPtr geom);
874
875 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
876 public static extern int GeomGetCategoryBits(IntPtr geom);
877
878 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
879 public static extern IntPtr GeomGetClassData(IntPtr geom);
880
881 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
882 public static extern int GeomGetCollideBits(IntPtr geom);
883
884 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
885 public static extern GeomClassID GeomGetClass(IntPtr geom);
886
887 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
888 public static extern IntPtr GeomGetData(IntPtr geom);
889
890 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
891 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
892 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
893 {
894 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
895 }
896
897 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
898 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
899 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
900 {
901 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
902 }
903
904 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
905 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
906 public static Vector3 GeomGetPosition(IntPtr geom)
907 {
908 unsafe { return *(GeomGetPositionUnsafe(geom)); }
909 }
910
911 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
912 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
913
914 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
915 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
916
917 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
918 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
919 public static Matrix3 GeomGetRotation(IntPtr geom)
920 {
921 unsafe { return *(GeomGetRotationUnsafe(geom)); }
922 }
923
924 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
925 public static extern IntPtr GeomGetSpace(IntPtr geom);
926
927 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
928 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
929 dReal width, dReal depth, int widthSamples, int depthSamples,
930 dReal scale, dReal offset, dReal thickness, int bWrap);
931
932 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
933 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
934 dReal width, dReal depth, int widthSamples, int depthSamples,
935 dReal scale, dReal offset, dReal thickness, int bWrap);
936
937 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
938 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
939 dReal width, dReal depth, int widthSamples, int depthSamples,
940 dReal scale, dReal offset, dReal thickness, int bWrap);
941
942 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
943 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
944 dReal width, dReal depth, int widthSamples, int depthSamples,
945 dReal scale, dReal offset, dReal thickness, int bWrap);
946
947 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
948 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
949 dReal width, dReal depth, int widthSamples, int depthSamples,
950 dReal scale, dReal offset, dReal thickness, int bWrap);
951
952 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
953 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
954 dReal width, dReal depth, int widthSamples, int depthSamples,
955 dReal scale, dReal offset, dReal thickness, int bWrap);
956
957 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
958 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
959 dReal width, dReal depth, int widthSamples, int depthSamples,
960 dReal scale, dReal offset, dReal thickness, int bWrap);
961
962 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
963 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
964 dReal width, dReal depth, int widthSamples, int depthSamples,
965 dReal scale, dReal offset, dReal thickness, int bWrap);
966
967 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
968 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
969 dReal width, dReal depth, int widthSamples, int depthSamples,
970 dReal scale, dReal offset, dReal thickness, int bWrap);
971
972 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
973 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
974 dReal width, dReal depth, int widthSamples, int depthSamples,
975 dReal scale, dReal offset, dReal thickness, int bWrap);
976
977 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
978 public static extern IntPtr GeomHeightfieldDataCreate();
979
980 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
981 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
982
983 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
984 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
985
986 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
987 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
988
989 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
990 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
991
992 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
993 public static extern bool GeomIsEnabled(IntPtr geom);
994
995 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
996 public static extern bool GeomIsOffset(IntPtr geom);
997
998 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
999 public static extern bool GeomIsSpace(IntPtr geom);
1000
1001 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1002 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
1003
1004 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1005 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
1006
1007 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
1008 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1009
1010 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
1011 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
1012
1013 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1014 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
1015
1016 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1017 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
1018
1019 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
1020 public static extern int GeomRayGetClosestHit(IntPtr ray);
1021
1022 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
1023 public static extern dReal GeomRayGetLength(IntPtr ray);
1024
1025 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
1026 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
1027
1028 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
1029 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
1030
1031 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
1032 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
1033
1034 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
1035 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
1036
1037 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
1038 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
1039
1040 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
1041 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
1042
1043 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
1044 public static extern void GeomSetCategoryBits(IntPtr geom, int bits);
1045
1046 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1047 public static extern void GeomSetCollideBits(IntPtr geom, int bits);
1048
1049 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
1050 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
1051
1052 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
1053 public static extern void GeomSetData(IntPtr geom, IntPtr data);
1054
1055 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
1056 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1057
1058 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1059 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
1060
1061 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1062 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
1063
1064 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1065 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
1066
1067 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1068 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
1069
1070 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
1071 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
1072
1073 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1074 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
1075
1076 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1077 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
1078
1079 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1080 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
1081
1082 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1083 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
1084
1085 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
1086 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1087
1088 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1089 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
1090
1091 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1092 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
1093
1094 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1095 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
1096
1097 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1098 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
1099
1100 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
1101 public static extern dReal GeomSphereGetRadius(IntPtr geom);
1102
1103 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
1104 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1105
1106 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
1107 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
1108
1109 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
1110 public static extern int GeomTransformGetCleanup(IntPtr geom);
1111
1112 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
1113 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
1114
1115 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
1116 public static extern int GeomTransformGetInfo(IntPtr geom);
1117
1118 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
1119 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
1120
1121 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
1122 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
1123
1124 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
1125 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
1126
1127 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1128 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1129 double[] vertices, int vertexStride, int vertexCount,
1130 int[] indices, int indexCount, int triStride);
1131
1132 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1133 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1134 IntPtr vertices, int vertexStride, int vertexCount,
1135 IntPtr indices, int indexCount, int triStride);
1136
1137 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1138 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
1139 double[] vertices, int vertexStride, int vertexCount,
1140 int[] indices, int indexCount, int triStride,
1141 double[] normals);
1142
1143 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1144 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1145 IntPtr vertices, int vertexStride, int vertexCount,
1146 IntPtr indices, int indexCount, int triStride,
1147 IntPtr normals);
1148
1149 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1150 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1151 dReal[] vertices, int vertexStride, int vertexCount,
1152 int[] indices, int indexCount, int triStride);
1153
1154 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1155 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1156 IntPtr vertices, int vertexStride, int vertexCount,
1157 IntPtr indices, int indexCount, int triStride);
1158
1159 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1160 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1161 dReal[] vertices, int vertexStride, int vertexCount,
1162 int[] indices, int indexCount, int triStride,
1163 dReal[] normals);
1164
1165 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1166 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1167 IntPtr vertices, int vertexStride, int vertexCount,
1168 IntPtr indices, int indexCount, int triStride,
1169 IntPtr normals);
1170
1171 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1172 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1173 float[] vertices, int vertexStride, int vertexCount,
1174 int[] indices, int indexCount, int triStride);
1175
1176 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1177 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1178 IntPtr vertices, int vertexStride, int vertexCount,
1179 IntPtr indices, int indexCount, int triStride);
1180
1181 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1182 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1183 float[] vertices, int vertexStride, int vertexCount,
1184 int[] indices, int indexCount, int triStride,
1185 float[] normals);
1186
1187 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1188 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1189 IntPtr vertices, int vertexStride, int vertexCount,
1190 IntPtr indices, int indexCount, int triStride,
1191 IntPtr normals);
1192
1193 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1194 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1195
1196 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1197 public static extern IntPtr GeomTriMeshDataCreate();
1198
1199 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1200 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1201
1202 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1203 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1204
1205 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1206 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1207
1208 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1209 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1210
1211 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1212 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1213
1214 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1215 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1216
1217 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1218 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1219
1220 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1221 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1222
1223 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1224 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1225
1226 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1227 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1228 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1229 {
1230 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1231 }
1232
1233 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1234 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1235
1236 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1237 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1238
1239 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1240 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1241
1242 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1243 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1244
1245 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1246 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1247
1248 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1249 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1250
1251 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1252 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1253
1254 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1255 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1256
1257 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1258 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1259
1260 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1261 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1262
1263 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1264 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1265
1266 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1267 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1268
1269 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
1270 public static extern string GetConfiguration(string str);
1271
1272 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1273 public static extern IntPtr HashSpaceCreate(IntPtr space);
1274
1275 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1276 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1277
1278 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1279 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1280
1281 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1282 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1283
1284 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1285 public static extern void InitODE();
1286
1287 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
1288 public static extern int InitODE2(uint ODEInitFlags);
1289
1290 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1291 public static extern int IsPositiveDefinite(ref dReal A, int n);
1292
1293 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1294 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1295
1296 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1297 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1298
1299 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1300 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1301
1302 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1303 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1304
1305 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1306 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1307
1308 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1309 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1310
1311 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1312 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1313
1314 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1315 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1316
1317 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1318 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1319
1320 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1321 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1322
1323 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1324 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1325 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1326 public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
1327
1328 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1329 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1330
1331 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1332 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1333
1334 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1335 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1336
1337 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1338 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1339
1340 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1341 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1342
1343 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1344 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1345
1346 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1347 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1348
1349 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1350 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1351
1352 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1353 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1354
1355 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1356 public static extern void JointDestroy(IntPtr j);
1357
1358 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1359 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1360
1361 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1362 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1363
1364 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1365 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1366
1367 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1368 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1369
1370 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1371 public static extern int JointGetAMotorMode(IntPtr j);
1372
1373 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1374 public static extern int JointGetAMotorNumAxes(IntPtr j);
1375
1376 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1377 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1378
1379 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1380 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1381
1382 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1383 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1384
1385 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1386 public static extern IntPtr JointGetBody(IntPtr j);
1387
1388 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1389 public static extern IntPtr JointGetData(IntPtr j);
1390
1391 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1392 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1393 public static JointFeedback JointGetFeedback(IntPtr j)
1394 {
1395 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1396 }
1397
1398 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1399 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1400
1401 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1402 public static extern dReal JointGetHingeAngle(IntPtr j);
1403
1404 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1405 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1406
1407 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1408 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1409
1410 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1411 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1412
1413 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1414 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1415
1416 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1417 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1418
1419 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1420 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1421
1422 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1423 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1424
1425 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1426 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1427
1428 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1429 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1430
1431 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1432 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1433
1434 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1435 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1436
1437 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1438 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1439
1440 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1441 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1442
1443 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1444 public static extern int JointGetLMotorNumAxes(IntPtr j);
1445
1446 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1447 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1448
1449 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1450 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1451
1452 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1453 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1454
1455 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1456 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1457
1458 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1459 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1460
1461 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1462 public static extern dReal JointGetPRPosition(IntPtr j);
1463
1464 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1465 public static extern dReal JointGetPRPositionRate(IntPtr j);
1466
1467 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1468 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1469
1470 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1471 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1472
1473 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1474 public static extern dReal JointGetSliderPosition(IntPtr j);
1475
1476 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1477 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1478
1479 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1480 public static extern JointType JointGetType(IntPtr j);
1481
1482 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1483 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1484
1485 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1486 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1487
1488 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1489 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1490
1491 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1492 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1493
1494 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1495 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1496
1497 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1498 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1499
1500 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1501 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1502
1503 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1504 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1505
1506 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1507 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1508
1509 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1510 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1511
1512 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1513 public static extern IntPtr JointGroupCreate(int max_size);
1514
1515 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1516 public static extern void JointGroupDestroy(IntPtr group);
1517
1518 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1519 public static extern void JointGroupEmpty(IntPtr group);
1520
1521 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1522 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1523
1524 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1525 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1526
1527 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1528 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1529
1530 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1531 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1532
1533 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1534 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1535
1536 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1537 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1538
1539 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1540 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1541
1542 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1543 public static extern void JointSetData(IntPtr j, IntPtr data);
1544
1545 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1546 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1547
1548 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1549 public static extern void JointSetFixed(IntPtr j);
1550
1551 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1552 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1553
1554 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1555 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1556
1557 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1558 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1559
1560 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1561 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1562
1563 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1564 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1565
1566 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1567 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1568
1569 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1570 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1571
1572 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1573 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1574
1575 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1576 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1577
1578 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1579 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1580
1581 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1582 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1583
1584 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1585 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1586
1587 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1588 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1589
1590 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1591 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1592
1593 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1594 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1595
1596 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1597 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1598
1599 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1600 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1601
1602 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1603 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1604
1605 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1606 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1607
1608 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1609 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1610
1611 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1612 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1613
1614 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1615 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1616
1617 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1618 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1619
1620 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1621 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1622
1623 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1624 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1625
1626 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1627 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1628
1629 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1630 public static extern void MassAdd(ref Mass a, ref Mass b);
1631
1632 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1633 public static extern void MassAdjust(ref Mass m, dReal newmass);
1634
1635 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1636 public static extern bool MassCheck(ref Mass m);
1637
1638 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1639 public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
1640
1641 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1642 public static extern void MassRotate(ref Mass mass, ref dReal M00);
1643
1644 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1645 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1646
1647 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1648 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1649
1650 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1651 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1652
1653 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1654 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1655
1656 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1657 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1658
1659 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1660 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1661
1662 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1663 public static extern void MassSetParameters(out Mass mass, dReal themass,
1664 dReal cgx, dReal cgy, dReal cgz,
1665 dReal i11, dReal i22, dReal i33,
1666 dReal i12, dReal i13, dReal i23);
1667
1668 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1669 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1670
1671 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1672 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1673
1674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1675 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1676
1677 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1678 public static extern void MassSetZero(out Mass mass);
1679
1680 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1681 public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
1682
1683 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1684 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1685
1686 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1687 private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
1688 public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
1689 {
1690 MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
1691 }
1692
1693 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1694 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1695
1696 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1697 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1698
1699 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1700 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1701
1702 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1703 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1704
1705 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1706 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1707
1708 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1709 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1710
1711 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1712 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1713
1714 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1715 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1716
1717 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1718 public static extern void QSetIdentity(out Quaternion q);
1719
1720 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1721 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1722
1723 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1724 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1725
1726 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1727 public static extern dReal RandReal();
1728
1729 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1730 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1731
1732 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1733 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1734
1735 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1736 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1737
1738 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1739 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1740
1741 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1742 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1743
1744 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1745 public static extern void RSetIdentity(out Matrix3 R);
1746
1747 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1748 public static extern void SetValue(out dReal a, int n);
1749
1750 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1751 public static extern void SetZero(out dReal a, int n);
1752
1753 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1754 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1755
1756 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1757 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1758
1759 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1760 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1761
1762 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1763 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1764
1765 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1766 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1767
1768 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1769 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1770
1771 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
1772 public static extern bool SpaceLockQuery(IntPtr space);
1773
1774 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1775 public static extern void SpaceClean(IntPtr space);
1776
1777 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1778 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1779
1780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1781 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1782
1783 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1784 public static extern void SpaceDestroy(IntPtr space);
1785
1786 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1787 public static extern bool SpaceGetCleanup(IntPtr space);
1788
1789 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1790 public static extern int SpaceGetNumGeoms(IntPtr space);
1791
1792 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1793 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1794
1795 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
1796 public static extern int SpaceGetSublevel(IntPtr space);
1797
1798 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1799 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1800
1801 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1802 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1803
1804 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1805 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1806
1807 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
1808 public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
1809
1810 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
1811 public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
1812
1813 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1814 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1815
1816 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1817 public static extern IntPtr WorldCreate();
1818
1819 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1820 public static extern void WorldDestroy(IntPtr world);
1821
1822 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1823 public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
1824
1825 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1826 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1827
1828 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1829 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1830
1831 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1832 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1833
1834 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1835 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1836
1837 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1838 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1839
1840 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1841 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1842
1843 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1844 public static extern dReal WorldGetCFM(IntPtr world);
1845
1846 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1847 public static extern dReal WorldGetERP(IntPtr world);
1848
1849 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1850 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1851
1852 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1853 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1854
1855 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1856 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1857
1858 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1859 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1860
1861 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
1862 public static extern dReal WorldGetAngularDamping(IntPtr world);
1863
1864 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1865 public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
1866
1867 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
1868 public static extern dReal WorldGetLinearDamping(IntPtr world);
1869
1870 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1871 public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
1872
1873 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1874 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1875
1876 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1877 public static extern dReal WorldGetQuickStepW(IntPtr world);
1878
1879 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1880 public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
1881
1882 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1883 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1884
1885 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1886 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1887
1888 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1889 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1890
1891 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
1892 public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
1893
1894 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1895 public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
1896
1897 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1898 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1899
1900 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1901 public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
1902
1903 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1904 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1905
1906 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1907 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1908
1909 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1910 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1911
1912 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1913 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1914
1915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1916 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1917
1918 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1919 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1920
1921 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1922 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1923
1924 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1925 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1926
1927 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
1928 public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
1929
1930 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1931 public static extern void WorldSetERP(IntPtr world, dReal erp);
1932
1933 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1934 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
1935
1936 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
1937 public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
1938
1939 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1940 public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
1941
1942 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1943 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
1944
1945 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
1946 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
1947
1948 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1949 public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
1950
1951 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
1952 public static extern void WorldStep(IntPtr world, dReal stepsize);
1953
1954 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
1955 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
1956
1957 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
1958 public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
1959 }
1960}
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..76d7746
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2356 @@
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 [Flags]
64 public enum CollisionCategories : int
65 {
66 Disabled = 0,
67 Geom = 0x00000001,
68 Body = 0x00000002,
69 Space = 0x00000004,
70 Character = 0x00000008,
71 Land = 0x00000010,
72 Water = 0x00000020,
73 Wind = 0x00000040,
74 Sensor = 0x00000080,
75 Selected = 0x00000100
76 }
77
78 /// <summary>
79 /// Material type for a primitive
80 /// </summary>
81 public enum Material : int
82 {
83 /// <summary></summary>
84 Stone = 0,
85 /// <summary></summary>
86 Metal = 1,
87 /// <summary></summary>
88 Glass = 2,
89 /// <summary></summary>
90 Wood = 3,
91 /// <summary></summary>
92 Flesh = 4,
93 /// <summary></summary>
94 Plastic = 5,
95 /// <summary></summary>
96 Rubber = 6,
97
98 light = 7 // compatibility with old viewers
99 }
100
101 public enum changes : int
102 {
103 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
104 Remove,
105 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
106 // or removes from a object if arg is null
107 DeLink,
108 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
109 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
110 PosOffset, // not in use
111 // arg Vector3 new position in local coords. Changes prim position in object
112 OriOffset, // not in use
113 // arg Vector3 new position in local coords. Changes prim position in object
114 Velocity,
115 AngVelocity,
116 Acceleration,
117 Force,
118 Torque,
119
120 AddForce,
121 AddAngForce,
122 AngLock,
123
124 Size,
125 Shape,
126
127 CollidesWater,
128 VolumeDtc,
129
130 Physical,
131 Phantom,
132 Selected,
133 disabled,
134 building,
135
136 VehicleType,
137 VehicleFloatParam,
138 VehicleVectorParam,
139 VehicleRotationParam,
140 VehicleFlags,
141 SetVehicle,
142
143 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
144 }
145
146 public struct ODEchangeitem
147 {
148 public PhysicsActor actor;
149 public OdeCharacter character;
150 public changes what;
151 public Object arg;
152 }
153
154 public class OdeScene : PhysicsScene
155 {
156 private readonly ILog m_log;
157 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
158
159// private int threadid = 0;
160 private Random fluidRandomizer = new Random(Environment.TickCount);
161
162 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
163 const float comumContactERP = 0.6f;
164 const float comumSoftContactERP = 0.1f;
165 const float comumContactCFM = 0.0001f;
166
167 float frictionMovementMult = 0.3f;
168
169 float TerrainBounce = 0.1f;
170 float TerrainFriction = 0.3f;
171
172 public float AvatarBounce = 0.3f;
173 public float AvatarFriction = 0;// 0.9f * 0.5f;
174
175 private const uint m_regionWidth = Constants.RegionSize;
176 private const uint m_regionHeight = Constants.RegionSize;
177
178 public float ODE_STEPSIZE = 0.020f;
179 private float metersInSpace = 25.6f;
180 private float m_timeDilation = 1.0f;
181
182 public float gravityx = 0f;
183 public float gravityy = 0f;
184 public float gravityz = -9.8f;
185
186 private float waterlevel = 0f;
187 private int framecount = 0;
188
189 internal IntPtr WaterGeom;
190
191 public float avPIDD = 2200f; // make it visible
192 public float avPIDP = 900f; // make it visible
193 private float avCapRadius = 0.37f;
194 private float avDensity = 3f;
195 private float avMovementDivisorWalk = 1.3f;
196 private float avMovementDivisorRun = 0.8f;
197 private float minimumGroundFlightOffset = 3f;
198 public float maximumMassObject = 10000.01f;
199
200 public bool meshSculptedPrim = true;
201 public bool forceSimplePrimMeshing = false;
202
203 public float meshSculptLOD = 32;
204 public float MeshSculptphysicalLOD = 32;
205
206 public float geomDefaultDensity = 10.000006836f;
207
208 public int geomContactPointsStartthrottle = 3;
209 public int geomUpdatesPerThrottledUpdate = 15;
210
211 public float bodyPIDD = 35f;
212 public float bodyPIDG = 25;
213
214// public int geomCrossingFailuresBeforeOutofbounds = 6;
215
216 public int bodyFramesAutoDisable = 20;
217
218 private float[] _watermap;
219
220 private d.NearCallback nearCallback;
221
222 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
223 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
224 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
225 private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
226
227 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
228
229 /// <summary>
230 /// A list of actors that should receive collision events.
231 /// </summary>
232 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
233
234 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
235 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
236 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
237
238 private float contactsurfacelayer = 0.002f;
239
240 private int contactsPerCollision = 80;
241 internal IntPtr ContactgeomsArray = IntPtr.Zero;
242 private IntPtr GlobalContactsArray = IntPtr.Zero;
243
244 const int maxContactsbeforedeath = 4000;
245 private volatile int m_global_contactcount = 0;
246
247
248 private readonly IntPtr contactgroup;
249
250 public ContactData[] m_materialContactsData = new ContactData[8];
251
252 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
253 private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
254 private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
255
256 private int m_physicsiterations = 10;
257 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
258 private readonly PhysicsActor PANull = new NullPhysicsActor();
259 private float step_time = 0.0f;
260
261 public IntPtr world;
262
263 private uint obj2LocalID = 0;
264 private OdeCharacter cc1;
265 private OdePrim cp1;
266 private OdeCharacter cc2;
267 private OdePrim cp2;
268
269 // split the spaces acording to contents type
270 // ActiveSpace contains characters and active prims
271 // StaticSpace contains land and other that is mostly static in enviroment
272 // this can contain subspaces, like the grid in staticspace
273 // as now space only contains this 2 top spaces
274
275 public IntPtr TopSpace; // the global space
276 public IntPtr ActiveSpace; // space for active prims
277 public IntPtr StaticSpace; // space for the static things around
278
279 // some speedup variables
280 private int spaceGridMaxX;
281 private int spaceGridMaxY;
282 private float spacesPerMeter;
283
284 // split static geometry collision into a grid as before
285 private IntPtr[,] staticPrimspace;
286
287 private Object OdeLock;
288 private static Object SimulationLock;
289
290 public IMesher mesher;
291
292 private IConfigSource m_config;
293
294 public bool physics_logging = false;
295 public int physics_logging_interval = 0;
296 public bool physics_logging_append_existing_logfile = false;
297
298 private Vector3 m_worldOffset = Vector3.Zero;
299 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
300 private PhysicsScene m_parentScene = null;
301
302 private ODERayCastRequestManager m_rayCastManager;
303
304
305/* maybe needed if ode uses tls
306 private void checkThread()
307 {
308
309 int th = Thread.CurrentThread.ManagedThreadId;
310 if(th != threadid)
311 {
312 threadid = th;
313 d.AllocateODEDataForThread(~0U);
314 }
315 }
316 */
317 /// <summary>
318 /// Initiailizes the scene
319 /// Sets many properties that ODE requires to be stable
320 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
321 /// </summary>
322 public OdeScene(string sceneIdentifier)
323 {
324 m_log
325 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
326
327// checkThread();
328 Name = sceneIdentifier;
329
330 OdeLock = new Object();
331 SimulationLock = new Object();
332
333 nearCallback = near;
334
335 m_rayCastManager = new ODERayCastRequestManager(this);
336 lock (OdeLock)
337 {
338 // Create the world and the first space
339 try
340 {
341 world = d.WorldCreate();
342 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
343
344 // now the major subspaces
345 ActiveSpace = d.HashSpaceCreate(TopSpace);
346 StaticSpace = d.HashSpaceCreate(TopSpace);
347 }
348 catch
349 {
350 // i must RtC#FM
351 }
352
353 d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does
354 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
355 d.HashSpaceSetLevels(StaticSpace, -2, 8);
356
357 // demote to second level
358 d.SpaceSetSublevel(ActiveSpace, 1);
359 d.SpaceSetSublevel(StaticSpace, 1);
360
361 contactgroup = d.JointGroupCreate(0);
362 //contactgroup
363
364 d.WorldSetAutoDisableFlag(world, false);
365 }
366
367 _watermap = new float[258 * 258];
368 }
369
370 // Initialize the mesh plugin
371// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
372 public override void Initialise(IMesher meshmerizer, IConfigSource config)
373 {
374// checkThread();
375 mesher = meshmerizer;
376 m_config = config;
377
378// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE"));
379 /*
380 if (region != null)
381 {
382 WorldExtents.X = region.RegionSizeX;
383 WorldExtents.Y = region.RegionSizeY;
384 }
385 */
386
387 // Defaults
388
389 int contactsPerCollision = 80;
390
391 if (m_config != null)
392 {
393 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
394 if (physicsconfig != null)
395 {
396 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
397 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
398 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
399
400 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
401
402 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
403
404 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
405 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
406
407 avDensity = physicsconfig.GetFloat("av_density", avDensity);
408 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
409 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
410 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
411
412 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
413
414 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
415 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
416// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
417
418 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
419 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
420
421 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
422 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
423
424 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
425 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
426 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
427 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
428
429 if (Environment.OSVersion.Platform == PlatformID.Unix)
430 {
431 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
432 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
433 }
434 else
435 {
436 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
437 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
438 }
439
440 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
441 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
442 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
443
444 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
445 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
446 }
447 }
448
449 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
450 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
451
452 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
453 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
454
455 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
456 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
457
458 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
459 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
460
461 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
462 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
463
464 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
465 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
466
467 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
468 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
469
470 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
471 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
472
473 m_materialContactsData[(int)Material.light].mu = 0.0f;
474 m_materialContactsData[(int)Material.light].bounce = 0.0f;
475
476 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
477
478 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
479 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
480
481 d.WorldSetLinearDamping(world, 0.001f);
482 d.WorldSetAngularDamping(world, 0.001f);
483 d.WorldSetAngularDampingThreshold(world, 0f);
484 d.WorldSetLinearDampingThreshold(world, 0f);
485 d.WorldSetMaxAngularSpeed(world, 256f);
486
487 d.WorldSetCFM(world,1e-6f); // a bit harder than default
488 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
489 d.WorldSetERP(world, 0.6f); // higher than original
490
491 // Set how many steps we go without running collision testing
492 // This is in addition to the step size.
493 // Essentially Steps * m_physicsiterations
494 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
495 d.WorldSetContactMaxCorrectingVel(world, 100.0f);
496
497 spacesPerMeter = 1 / metersInSpace;
498 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
499 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
500
501 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
502
503 // create all spaces now
504 int i, j;
505 IntPtr newspace;
506 for (i = 0; i < spaceGridMaxX; i++)
507 for (j = 0; j < spaceGridMaxY; j++)
508 {
509 newspace = d.HashSpaceCreate(StaticSpace);
510 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
511 waitForSpaceUnlock(newspace);
512 d.SpaceSetSublevel(newspace, 2);
513 d.HashSpaceSetLevels(newspace, -2, 8);
514 staticPrimspace[i, j] = newspace;
515 }
516 // let this now be real maximum values
517 spaceGridMaxX--;
518 spaceGridMaxY--;
519 }
520
521 internal void waitForSpaceUnlock(IntPtr space)
522 {
523 //if (space != IntPtr.Zero)
524 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
525 }
526
527 #region Collision Detection
528
529 // sets a global contact for a joint for contactgeom , and base contact description)
530
531 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp)
532 {
533 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
534 return IntPtr.Zero;
535
536 d.Contact newcontact = new d.Contact();
537 newcontact.geom.depth = contactGeom.depth;
538 newcontact.geom.g1 = contactGeom.g1;
539 newcontact.geom.g2 = contactGeom.g2;
540 newcontact.geom.pos = contactGeom.pos;
541 newcontact.geom.normal = contactGeom.normal;
542 newcontact.geom.side1 = contactGeom.side1;
543 newcontact.geom.side2 = contactGeom.side2;
544
545 // this needs bounce also
546 newcontact.surface.mode = comumContactFlags;
547 newcontact.surface.mu = mu;
548 newcontact.surface.bounce = bounce;
549 newcontact.surface.soft_cfm = comumContactCFM;
550 if (softerp)
551 newcontact.surface.soft_erp = comumSoftContactERP;
552 else
553 newcontact.surface.soft_erp = comumContactERP;
554
555 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
556 Marshal.StructureToPtr(newcontact, contact, true);
557 return d.JointCreateContactPtr(world, contactgroup, contact);
558 }
559
560
561
562 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
563 {
564 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
565 return false;
566
567 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
568 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
569 return true;
570 }
571
572 /// <summary>
573 /// This is our near callback. A geometry is near a body
574 /// </summary>
575 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
576 /// <param name="g1">a geometry or space</param>
577 /// <param name="g2">another geometry or space</param>
578 ///
579
580 private void near(IntPtr space, IntPtr g1, IntPtr g2)
581 {
582 // no lock here! It's invoked from within Simulate(), which is thread-locked
583
584 if (m_global_contactcount >= maxContactsbeforedeath)
585 return;
586
587 // Test if we're colliding a geom with a space.
588 // If so we have to drill down into the space recursively
589
590 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
591 return;
592
593 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
594 {
595 // We'll be calling near recursivly if one
596 // of them is a space to find all of the
597 // contact points in the space
598 try
599 {
600 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
601 }
602 catch (AccessViolationException)
603 {
604 m_log.Warn("[PHYSICS]: Unable to collide test a space");
605 return;
606 }
607 //here one should check collisions of geoms inside a space
608 // but on each space we only should have geoms that not colide amoung each other
609 // so we don't dig inside spaces
610 return;
611 }
612
613 // get geom bodies to check if we already a joint contact
614 // guess this shouldn't happen now
615 IntPtr b1 = d.GeomGetBody(g1);
616 IntPtr b2 = d.GeomGetBody(g2);
617
618 // d.GeomClassID id = d.GeomGetClass(g1);
619
620 // Figure out how many contact points we have
621 int count = 0;
622 try
623 {
624 // Colliding Geom To Geom
625 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
626
627 if (g1 == g2)
628 return; // Can't collide with yourself
629
630 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
631 return;
632
633 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
634 }
635 catch (SEHException)
636 {
637 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.");
638// ode.drelease(world);
639 base.TriggerPhysicsBasedRestart();
640 }
641 catch (Exception e)
642 {
643 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
644 return;
645 }
646
647 // id contacts done
648 if (count == 0)
649 return;
650
651 // try get physical actors
652 PhysicsActor p1;
653 PhysicsActor p2;
654
655 if (!actor_name_map.TryGetValue(g1, out p1))
656 {
657 p1 = PANull;
658 }
659
660 if (!actor_name_map.TryGetValue(g2, out p2))
661 {
662 p2 = PANull;
663 }
664
665 // update actors collision score
666 if (p1.CollisionScore >= float.MaxValue - count)
667 p1.CollisionScore = 0;
668 p1.CollisionScore += count;
669
670 if (p2.CollisionScore >= float.MaxValue - count)
671 p2.CollisionScore = 0;
672 p2.CollisionScore += count;
673
674
675 // get first contact
676 d.ContactGeom curContact = new d.ContactGeom();
677 if (!GetCurContactGeom(0, ref curContact))
678 return;
679 // for now it's the one with max depth
680 ContactPoint maxDepthContact = new ContactPoint(
681 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
682 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
683 curContact.depth
684 );
685 // do volume detection case
686 if (
687 (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) ||
688 (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
689 {
690 collision_accounting_events(p1, p2, maxDepthContact);
691 return;
692 }
693
694 // big messy collision analises
695 float mu = 0;
696 float bounce = 0;
697 ContactData contactdata1 = new ContactData(0, 0, false);
698 ContactData contactdata2 = new ContactData(0, 0, false);
699 bool erpSoft = false;
700
701 String name = null;
702 bool dop1foot = false;
703 bool dop2foot = false;
704 bool ignore = false;
705
706 switch (p1.PhysicsActorType)
707 {
708 case (int)ActorTypes.Agent:
709 switch (p2.PhysicsActorType)
710 {
711 case (int)ActorTypes.Agent:
712 p1.getContactData(ref contactdata1);
713 p2.getContactData(ref contactdata2);
714
715 bounce = contactdata1.bounce * contactdata2.bounce;
716
717 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
718
719 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
720 mu *= frictionMovementMult;
721
722 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
723 p1.CollidingObj = true;
724 p2.CollidingObj = true;
725 break;
726 case (int)ActorTypes.Prim:
727 p1.getContactData(ref contactdata1);
728 p2.getContactData(ref contactdata2);
729 bounce = contactdata1.bounce * contactdata2.bounce;
730
731 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
732
733 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
734 mu *= frictionMovementMult;
735 if (p2.Velocity.LengthSquared() > 0.0f)
736 p2.CollidingObj = true;
737
738 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
739
740 dop1foot = true;
741 break;
742 default:
743 ignore=true; // avatar to terrain and water ignored
744 break;
745 }
746 break;
747
748 case (int)ActorTypes.Prim:
749 switch (p2.PhysicsActorType)
750 {
751 case (int)ActorTypes.Agent:
752 p1.getContactData(ref contactdata1);
753 p2.getContactData(ref contactdata2);
754 bounce = contactdata1.bounce * contactdata2.bounce;
755
756 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
757
758 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
759 mu *= frictionMovementMult;
760
761 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
762 dop2foot = true;
763 if (p1.Velocity.LengthSquared() > 0.0f)
764 p1.CollidingObj = true;
765 break;
766 case (int)ActorTypes.Prim:
767 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
768 {
769 p1.CollidingObj = true;
770 p2.CollidingObj = true;
771 }
772 p1.getContactData(ref contactdata1);
773 p2.getContactData(ref contactdata2);
774 bounce = contactdata1.bounce * contactdata2.bounce;
775 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
776 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
777
778 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
779 mu *= frictionMovementMult;
780
781 break;
782 default:
783 if (geom_name_map.TryGetValue(g2, out name))
784 {
785 if (name == "Terrain")
786 {
787 p1.getContactData(ref contactdata1);
788 bounce = contactdata1.bounce * TerrainBounce;
789 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
790 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
791 mu *= frictionMovementMult;
792 erpSoft = contactdata1.softcolide;
793 p1.CollidingGround = true;
794 }
795 else if (name == "Water")
796 {
797 erpSoft = true;
798 }
799 }
800 else
801 ignore=true;
802 break;
803 }
804 break;
805
806 default:
807 if (geom_name_map.TryGetValue(g1, out name))
808 {
809 if (name == "Terrain")
810 {
811 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
812 {
813 p2.CollidingGround = true;
814 p2.getContactData(ref contactdata2);
815 bounce = contactdata2.bounce * TerrainBounce;
816 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
817 erpSoft = contactdata2.softcolide;
818
819 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
820 mu *= frictionMovementMult;
821 }
822 else
823 ignore = true;
824
825 }
826 else if (name == "Water" &&
827 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
828 {
829 erpSoft = true;
830 }
831 }
832 else
833 ignore = true;
834 break;
835 }
836
837 if (ignore)
838 return;
839
840 IntPtr Joint;
841
842 int i = 0;
843 while(true)
844 {
845 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
846 p1.IsColliding = true;
847 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
848 p2.IsColliding = true;
849
850 Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft);
851 d.JointAttach(Joint, b1, b2);
852
853 if (++m_global_contactcount >= maxContactsbeforedeath)
854 break;
855
856 if(++i >= count)
857 break;
858
859 if (!GetCurContactGeom(i, ref curContact))
860 break;
861
862 if (curContact.depth > maxDepthContact.PenetrationDepth)
863 {
864 maxDepthContact.Position.X = curContact.pos.X;
865 maxDepthContact.Position.Y = curContact.pos.Y;
866 maxDepthContact.Position.Z = curContact.pos.Z;
867 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
868 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
869 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
870 maxDepthContact.PenetrationDepth = curContact.depth;
871 }
872 }
873
874 collision_accounting_events(p1, p2, maxDepthContact);
875
876/*
877 if (notskipedcount > geomContactPointsStartthrottle)
878 {
879 // If there are more then 3 contact points, it's likely
880 // that we've got a pile of objects, so ...
881 // We don't want to send out hundreds of terse updates over and over again
882 // so lets throttle them and send them again after it's somewhat sorted out.
883 this needs checking so out for now
884 if (b1 != IntPtr.Zero)
885 p1.ThrottleUpdates = true;
886 if (b2 != IntPtr.Zero)
887 p2.ThrottleUpdates = true;
888
889 }
890 */
891 }
892
893 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
894 {
895 // obj1LocalID = 0;
896 //returncollisions = false;
897 obj2LocalID = 0;
898 //ctype = 0;
899 //cStartStop = 0;
900 if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
901 return;
902
903 switch ((ActorTypes)p1.PhysicsActorType)
904 {
905 case ActorTypes.Agent:
906 cc1 = (OdeCharacter)p1;
907 switch ((ActorTypes)p2.PhysicsActorType)
908 {
909 case ActorTypes.Agent:
910 cc2 = (OdeCharacter)p2;
911 obj2LocalID = cc2.m_localID;
912 if (p2.SubscribedEvents())
913 cc2.AddCollisionEvent(cc1.m_localID, contact);
914 break;
915
916 case ActorTypes.Prim:
917 if (p2 is OdePrim)
918 {
919 cp2 = (OdePrim)p2;
920 obj2LocalID = cp2.m_localID;
921 if (p2.SubscribedEvents())
922 cp2.AddCollisionEvent(cc1.m_localID, contact);
923 }
924 break;
925
926 case ActorTypes.Ground:
927 case ActorTypes.Unknown:
928 default:
929 obj2LocalID = 0;
930 break;
931 }
932 if (p1.SubscribedEvents())
933 {
934 contact.SurfaceNormal = -contact.SurfaceNormal;
935 cc1.AddCollisionEvent(obj2LocalID, contact);
936 }
937 break;
938
939 case ActorTypes.Prim:
940
941 if (p1 is OdePrim)
942 {
943 cp1 = (OdePrim)p1;
944
945 // obj1LocalID = cp2.m_localID;
946 switch ((ActorTypes)p2.PhysicsActorType)
947 {
948 case ActorTypes.Agent:
949 if (p2 is OdeCharacter)
950 {
951 cc2 = (OdeCharacter)p2;
952 obj2LocalID = cc2.m_localID;
953 if (p2.SubscribedEvents())
954 cc2.AddCollisionEvent(cp1.m_localID, contact);
955 }
956 break;
957 case ActorTypes.Prim:
958
959 if (p2 is OdePrim)
960 {
961 cp2 = (OdePrim)p2;
962 obj2LocalID = cp2.m_localID;
963 if (p2.SubscribedEvents())
964 cp2.AddCollisionEvent(cp1.m_localID, contact);
965 }
966 break;
967
968 case ActorTypes.Ground:
969 case ActorTypes.Unknown:
970 default:
971 obj2LocalID = 0;
972 break;
973 }
974 if (p1.SubscribedEvents())
975 {
976 contact.SurfaceNormal = -contact.SurfaceNormal;
977 cp1.AddCollisionEvent(obj2LocalID, contact);
978 }
979 }
980 break;
981 }
982 }
983
984 /// <summary>
985 /// This is our collision testing routine in ODE
986 /// </summary>
987 /// <param name="timeStep"></param>
988 private void collision_optimized()
989 {
990 lock (_characters)
991 {
992 try
993 {
994 foreach (OdeCharacter chr in _characters)
995 {
996 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
997 continue;
998
999 chr.IsColliding = false;
1000 // chr.CollidingGround = false; not done here
1001 chr.CollidingObj = false;
1002 // do colisions with static space
1003 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1004 }
1005 }
1006 catch (AccessViolationException)
1007 {
1008 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1009 }
1010
1011 }
1012
1013 lock (_activeprims)
1014 {
1015 foreach (OdePrim aprim in _activeprims)
1016 {
1017 aprim.CollisionScore = 0;
1018 aprim.IsColliding = false;
1019 }
1020 }
1021
1022 // collide active prims with static enviroment
1023 lock (_activegroups)
1024 {
1025 try
1026 {
1027 foreach (OdePrim prm in _activegroups)
1028 {
1029 if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds)
1030 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1031 }
1032 }
1033 catch (AccessViolationException)
1034 {
1035 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1036 }
1037 }
1038 // finally colide active things amoung them
1039 try
1040 {
1041 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1042 }
1043 catch (AccessViolationException)
1044 {
1045 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1046 }
1047// _perloopContact.Clear();
1048 }
1049
1050 #endregion
1051
1052
1053
1054 /// <summary>
1055 /// Add actor to the list that should receive collision events in the simulate loop.
1056 /// </summary>
1057 /// <param name="obj"></param>
1058 public void AddCollisionEventReporting(PhysicsActor obj)
1059 {
1060 lock (_collisionEventPrim)
1061 {
1062 if (!_collisionEventPrim.Contains(obj))
1063 _collisionEventPrim.Add(obj);
1064 }
1065 }
1066
1067 /// <summary>
1068 /// Remove actor from the list that should receive collision events in the simulate loop.
1069 /// </summary>
1070 /// <param name="obj"></param>
1071 public void RemoveCollisionEventReporting(PhysicsActor obj)
1072 {
1073 lock (_collisionEventPrim)
1074 {
1075 if (_collisionEventPrim.Contains(obj))
1076 _collisionEventPrim.Remove(obj);
1077 }
1078 }
1079
1080 #region Add/Remove Entities
1081
1082 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1083 {
1084 Vector3 pos;
1085 pos.X = position.X;
1086 pos.Y = position.Y;
1087 pos.Z = position.Z;
1088 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1089 newAv.Flying = isFlying;
1090 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1091
1092 return newAv;
1093 }
1094
1095 public void AddCharacter(OdeCharacter chr)
1096 {
1097 lock (_characters)
1098 {
1099 if (!_characters.Contains(chr))
1100 {
1101 _characters.Add(chr);
1102 if (chr.bad)
1103 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1104 }
1105 }
1106 }
1107
1108 public void RemoveCharacter(OdeCharacter chr)
1109 {
1110 lock (_characters)
1111 {
1112 if (_characters.Contains(chr))
1113 {
1114 _characters.Remove(chr);
1115 }
1116 }
1117 }
1118
1119 public void BadCharacter(OdeCharacter chr)
1120 {
1121 lock (_badCharacter)
1122 {
1123 if (!_badCharacter.Contains(chr))
1124 _badCharacter.Add(chr);
1125 }
1126 }
1127
1128 public override void RemoveAvatar(PhysicsActor actor)
1129 {
1130 //m_log.Debug("[PHYSICS]:ODELOCK");
1131 ((OdeCharacter) actor).Destroy();
1132 }
1133
1134 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1135 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1136 {
1137 Vector3 pos = position;
1138 Vector3 siz = size;
1139 Quaternion rot = rotation;
1140
1141 OdePrim newPrim;
1142 lock (OdeLock)
1143 {
1144 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);
1145
1146 lock (_prims)
1147 _prims.Add(newPrim);
1148 }
1149 return newPrim;
1150 }
1151
1152 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1153 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
1154 {
1155 Vector3 pos = position;
1156 Vector3 siz = size;
1157 Quaternion rot = rotation;
1158
1159 OdePrim newPrim;
1160 lock (OdeLock)
1161 {
1162 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);
1163
1164 lock (_prims)
1165 _prims.Add(newPrim);
1166 }
1167 return newPrim;
1168 }
1169
1170 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1171 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1172 {
1173 Vector3 pos = position;
1174 Vector3 siz = size;
1175 Quaternion rot = rotation;
1176
1177 OdePrim newPrim;
1178 lock (OdeLock)
1179 {
1180 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);
1181
1182 lock (_prims)
1183 _prims.Add(newPrim);
1184 }
1185 return newPrim;
1186 }
1187
1188 public void addActivePrim(OdePrim activatePrim)
1189 {
1190 // adds active prim..
1191 lock (_activeprims)
1192 {
1193 if (!_activeprims.Contains(activatePrim))
1194 _activeprims.Add(activatePrim);
1195 }
1196 }
1197
1198 public void addActiveGroups(OdePrim activatePrim)
1199 {
1200 lock (_activegroups)
1201 {
1202 if (!_activegroups.Contains(activatePrim))
1203 _activegroups.Add(activatePrim);
1204 }
1205 }
1206
1207 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1208 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1209 {
1210 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid);
1211 }
1212
1213
1214 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1215 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1216 {
1217#if SPAM
1218 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1219#endif
1220
1221 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1222 }
1223
1224 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1225 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1226 {
1227#if SPAM
1228 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1229#endif
1230
1231 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1232 }
1233
1234 public override float TimeDilation
1235 {
1236 get { return m_timeDilation; }
1237 }
1238
1239 public override bool SupportsNINJAJoints
1240 {
1241 get { return false; }
1242 }
1243
1244
1245 public void remActivePrim(OdePrim deactivatePrim)
1246 {
1247 lock (_activeprims)
1248 {
1249 _activeprims.Remove(deactivatePrim);
1250 }
1251 }
1252 public void remActiveGroup(OdePrim deactivatePrim)
1253 {
1254 lock (_activegroups)
1255 {
1256 _activegroups.Remove(deactivatePrim);
1257 }
1258 }
1259
1260 public override void RemovePrim(PhysicsActor prim)
1261 {
1262 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1263 // removed in the next physics simulate pass.
1264 if (prim is OdePrim)
1265 {
1266// lock (OdeLock)
1267 {
1268 OdePrim p = (OdePrim)prim;
1269 p.setPrimForRemoval();
1270 }
1271 }
1272 }
1273 /// <summary>
1274 /// This is called from within simulate but outside the locked portion
1275 /// We need to do our own locking here
1276 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1277 /// Simulate() -- justincc).
1278 ///
1279 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1280 ///
1281 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1282 /// that the space was using.
1283 /// </summary>
1284 /// <param name="prim"></param>
1285 public void RemovePrimThreadLocked(OdePrim prim)
1286 {
1287 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1288 lock (prim)
1289 {
1290 RemoveCollisionEventReporting(prim);
1291 lock (_prims)
1292 _prims.Remove(prim);
1293 }
1294
1295 }
1296 #endregion
1297
1298 #region Space Separation Calculation
1299
1300 /// <summary>
1301 /// Called when a static prim moves or becomes static
1302 /// Places the prim in a space one the static sub-spaces grid
1303 /// </summary>
1304 /// <param name="geom">the pointer to the geom that moved</param>
1305 /// <param name="pos">the position that the geom moved to</param>
1306 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1307 /// <returns>a pointer to the new space it's in</returns>
1308 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1309 {
1310 // moves a prim into another static sub-space or from another space into a static sub-space
1311
1312 // Called ODEPrim so
1313 // it's already in locked space.
1314
1315 if (geom == IntPtr.Zero) // shouldn't happen
1316 return IntPtr.Zero;
1317
1318 // get the static sub-space for current position
1319 IntPtr newspace = calculateSpaceForGeom(pos);
1320
1321 if (newspace == currentspace) // if we are there all done
1322 return newspace;
1323
1324 // else remove it from its current space
1325 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1326 {
1327 if (d.GeomIsSpace(currentspace))
1328 {
1329 waitForSpaceUnlock(currentspace);
1330 d.SpaceRemove(currentspace, geom);
1331
1332 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1333 {
1334 d.SpaceDestroy(currentspace);
1335 }
1336 }
1337 else
1338 {
1339 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1340 " Geom:" + geom);
1341 }
1342 }
1343 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1344 {
1345 currentspace = d.GeomGetSpace(geom);
1346 if (currentspace != IntPtr.Zero)
1347 {
1348 if (d.GeomIsSpace(currentspace))
1349 {
1350 waitForSpaceUnlock(currentspace);
1351 d.SpaceRemove(currentspace, geom);
1352
1353 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1354 {
1355 d.SpaceDestroy(currentspace);
1356 }
1357
1358 }
1359 }
1360 }
1361
1362 // put the geom in the newspace
1363 waitForSpaceUnlock(newspace);
1364 d.SpaceAdd(newspace, geom);
1365
1366 // let caller know this newspace
1367 return newspace;
1368 }
1369
1370 /// <summary>
1371 /// Calculates the space the prim should be in by its position
1372 /// </summary>
1373 /// <param name="pos"></param>
1374 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1375 public IntPtr calculateSpaceForGeom(Vector3 pos)
1376 {
1377 int x, y;
1378 x = (int)(pos.X * spacesPerMeter);
1379 if (x < 0)
1380 x = 0;
1381 else if (x > spaceGridMaxX)
1382 x = spaceGridMaxX;
1383
1384 y = (int)(pos.Y * spacesPerMeter);
1385 if (y < 0)
1386 y = 0;
1387 else if (y >spaceGridMaxY)
1388 y = spaceGridMaxY;
1389
1390 IntPtr tmpSpace = staticPrimspace[x, y];
1391 return tmpSpace;
1392 }
1393
1394 #endregion
1395
1396 /// <summary>
1397 /// Routine to figure out if we need to mesh this prim with our mesher
1398 /// </summary>
1399 /// <param name="pbs"></param>
1400 /// <returns></returns>
1401 public bool needsMeshing(PrimitiveBaseShape pbs)
1402 {
1403 // most of this is redundant now as the mesher will return null if it cant mesh a prim
1404 // but we still need to check for sculptie meshing being enabled so this is the most
1405 // convenient place to do it for now...
1406
1407 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
1408 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
1409 int iPropertiesNotSupportedDefault = 0;
1410
1411 if (pbs.SculptEntry)
1412 {
1413 if(!meshSculptedPrim)
1414 return false;
1415 }
1416
1417 // 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
1418 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
1419 {
1420 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1421 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1422 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1423 {
1424
1425 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1426 && pbs.ProfileHollow == 0
1427 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1428 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1429 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1430 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1431 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1432 {
1433#if SPAM
1434 m_log.Warn("NonMesh");
1435#endif
1436 return false;
1437 }
1438 }
1439 }
1440
1441 // following code doesn't give meshs to boxes and spheres ever
1442 // and it's odd.. so for now just return true if asked to force meshs
1443 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1444
1445 if (forceSimplePrimMeshing)
1446 return true;
1447
1448 if (pbs.ProfileHollow != 0)
1449 iPropertiesNotSupportedDefault++;
1450
1451 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1452 iPropertiesNotSupportedDefault++;
1453
1454 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1455 iPropertiesNotSupportedDefault++;
1456
1457 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1458 iPropertiesNotSupportedDefault++;
1459
1460 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1461 iPropertiesNotSupportedDefault++;
1462
1463 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1464 iPropertiesNotSupportedDefault++;
1465
1466 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1467 iPropertiesNotSupportedDefault++;
1468
1469 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))
1470 iPropertiesNotSupportedDefault++;
1471
1472 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1473 iPropertiesNotSupportedDefault++;
1474
1475 // test for torus
1476 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1477 {
1478 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1479 {
1480 iPropertiesNotSupportedDefault++;
1481 }
1482 }
1483 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1484 {
1485 if (pbs.PathCurve == (byte)Extrusion.Straight)
1486 {
1487 iPropertiesNotSupportedDefault++;
1488 }
1489
1490 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1491 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1492 {
1493 iPropertiesNotSupportedDefault++;
1494 }
1495 }
1496 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1497 {
1498 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1499 {
1500 iPropertiesNotSupportedDefault++;
1501 }
1502 }
1503 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1504 {
1505 if (pbs.PathCurve == (byte)Extrusion.Straight)
1506 {
1507 iPropertiesNotSupportedDefault++;
1508 }
1509 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1510 {
1511 iPropertiesNotSupportedDefault++;
1512 }
1513 }
1514
1515 if (pbs.SculptEntry && meshSculptedPrim)
1516 iPropertiesNotSupportedDefault++;
1517
1518 if (iPropertiesNotSupportedDefault == 0)
1519 {
1520#if SPAM
1521 m_log.Warn("NonMesh");
1522#endif
1523 return false;
1524 }
1525#if SPAM
1526 m_log.Debug("Mesh");
1527#endif
1528 return true;
1529 }
1530
1531 /// <summary>
1532 /// Called to queue a change to a actor
1533 /// to use in place of old taint mechanism so changes do have a time sequence
1534 /// </summary>
1535
1536 public void AddChange(PhysicsActor actor, changes what, Object arg)
1537 {
1538 ODEchangeitem item = new ODEchangeitem();
1539 item.actor = actor;
1540 item.what = what;
1541 item.arg = arg;
1542 ChangesQueue.Enqueue(item);
1543 }
1544
1545 /// <summary>
1546 /// Called after our prim properties are set Scale, position etc.
1547 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1548 /// This assures us that we have no race conditions
1549 /// </summary>
1550 /// <param name="prim"></param>
1551 public override void AddPhysicsActorTaint(PhysicsActor prim)
1552 {
1553 }
1554
1555 /// <summary>
1556 /// This is our main simulate loop
1557 /// It's thread locked by a Mutex in the scene.
1558 /// It holds Collisions, it instructs ODE to step through the physical reactions
1559 /// It moves the objects around in memory
1560 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1561 /// </summary>
1562 /// <param name="timeStep"></param>
1563 /// <returns></returns>
1564 public override float Simulate(float timeStep)
1565 {
1566 int statstart;
1567 int statchanges = 0;
1568 int statchmove = 0;
1569 int statactmove = 0;
1570 int statray = 0;
1571 int statcol = 0;
1572 int statstep = 0;
1573 int statmovchar = 0;
1574 int statmovprim;
1575 int totjcontact = 0;
1576
1577 // acumulate time so we can reduce error
1578 step_time += timeStep;
1579
1580 if (step_time < ODE_STEPSIZE)
1581 return 0;
1582
1583 if (framecount >= int.MaxValue)
1584 framecount = 0;
1585
1586 framecount++;
1587
1588 int curphysiteractions = m_physicsiterations;
1589
1590 if (step_time >= m_SkipFramesAtms)
1591 {
1592 // if in trouble reduce step resolution
1593 curphysiteractions /= 2;
1594 }
1595
1596 int nodeframes = 0;
1597
1598// checkThread();
1599
1600 lock (SimulationLock)
1601 {
1602 // adjust number of iterations per step
1603 try
1604 {
1605 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1606 }
1607 catch (StackOverflowException)
1608 {
1609 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1610// ode.drelease(world);
1611 base.TriggerPhysicsBasedRestart();
1612 }
1613
1614
1615 while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
1616 {
1617 try
1618 {
1619 statstart = Util.EnvironmentTickCount();
1620
1621 // clear pointer/counter to contacts to pass into joints
1622 m_global_contactcount = 0;
1623
1624 ODEchangeitem item;
1625
1626 if(ChangesQueue.Count >0)
1627 {
1628 int ttmpstart = Util.EnvironmentTickCount();
1629 int ttmp;
1630 int ttmp2;
1631
1632 while(ChangesQueue.Dequeue(out item))
1633 {
1634 if (item.actor != null)
1635 {
1636 try
1637 {
1638 if (item.actor is OdeCharacter)
1639 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1640 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1641 RemovePrimThreadLocked((OdePrim)item.actor);
1642 }
1643 catch
1644 {
1645 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1646 };
1647 }
1648 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1649 if (ttmp > 20)
1650 break;
1651 }
1652
1653 ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart);
1654 if (ttmp2 > 50)
1655 ttmp2 = 0;
1656
1657 }
1658
1659 statchanges += Util.EnvironmentTickCountSubtract(statstart);
1660
1661 statactmove += Util.EnvironmentTickCountSubtract(statstart);
1662 //if ((framecount % m_randomizeWater) == 0)
1663 // randomizeWater(waterlevel);
1664
1665 m_rayCastManager.ProcessQueuedRequests();
1666
1667
1668
1669 statray += Util.EnvironmentTickCountSubtract(statstart);
1670 collision_optimized();
1671 statcol += Util.EnvironmentTickCountSubtract(statstart);
1672
1673 lock (_collisionEventPrim)
1674 {
1675 foreach (PhysicsActor obj in _collisionEventPrim)
1676 {
1677 if (obj == null)
1678 continue;
1679
1680 switch ((ActorTypes)obj.PhysicsActorType)
1681 {
1682 case ActorTypes.Agent:
1683 OdeCharacter cobj = (OdeCharacter)obj;
1684 cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f));
1685 cobj.SendCollisions();
1686 break;
1687
1688 case ActorTypes.Prim:
1689 OdePrim pobj = (OdePrim)obj;
1690 pobj.SendCollisions();
1691 break;
1692 }
1693 }
1694 }
1695
1696 d.WorldQuickStep(world, ODE_STEPSIZE);
1697 statstep += Util.EnvironmentTickCountSubtract(statstart);
1698
1699 // Move characters
1700 lock (_characters)
1701 {
1702 List<OdeCharacter> defects = new List<OdeCharacter>();
1703 foreach (OdeCharacter actor in _characters)
1704 {
1705 if (actor != null)
1706 actor.Move(ODE_STEPSIZE, defects);
1707 }
1708 if (defects.Count != 0)
1709 {
1710 foreach (OdeCharacter defect in defects)
1711 {
1712 RemoveCharacter(defect);
1713 }
1714 }
1715 }
1716 statchmove += Util.EnvironmentTickCountSubtract(statstart);
1717
1718 // Move other active objects
1719 lock (_activegroups)
1720 {
1721 foreach (OdePrim aprim in _activegroups)
1722 {
1723 aprim.Move();
1724 }
1725 }
1726
1727 //ode.dunlock(world);
1728 }
1729 catch (Exception e)
1730 {
1731 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1732// ode.dunlock(world);
1733 }
1734
1735 d.JointGroupEmpty(contactgroup);
1736 totjcontact += m_global_contactcount;
1737
1738 step_time -= ODE_STEPSIZE;
1739 nodeframes++;
1740 }
1741
1742 statstart = Util.EnvironmentTickCount();
1743
1744 lock (_characters)
1745 {
1746 foreach (OdeCharacter actor in _characters)
1747 {
1748 if (actor != null)
1749 {
1750 if (actor.bad)
1751 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1752
1753 actor.UpdatePositionAndVelocity();
1754 }
1755 }
1756 }
1757
1758 lock (_badCharacter)
1759 {
1760 if (_badCharacter.Count > 0)
1761 {
1762 foreach (OdeCharacter chr in _badCharacter)
1763 {
1764 RemoveCharacter(chr);
1765 }
1766
1767 _badCharacter.Clear();
1768 }
1769 }
1770 statmovchar = Util.EnvironmentTickCountSubtract(statstart);
1771
1772 lock (_activegroups)
1773 {
1774 {
1775 foreach (OdePrim actor in _activegroups)
1776 {
1777 if (actor.IsPhysical)
1778 {
1779 actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
1780 }
1781 }
1782 }
1783 }
1784
1785 statmovprim = Util.EnvironmentTickCountSubtract(statstart);
1786
1787 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1788 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1789 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1790 int nbodies = d.NTotalBodies;
1791 int ngeoms = d.NTotalGeoms;
1792
1793 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1794 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1795 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1796 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1797 {
1798 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1799 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1800
1801 if (physics_logging_append_existing_logfile)
1802 {
1803 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1804 TextWriter fwriter = File.AppendText(fname);
1805 fwriter.WriteLine(header);
1806 fwriter.Close();
1807 }
1808
1809 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1810 }
1811
1812 // think time dilation is not a physics issue alone.. but ok let's fake something
1813 if (step_time < ODE_STEPSIZE) // we did the required loops
1814 m_timeDilation = 1.0f;
1815 else
1816 { // we didn't forget the lost ones and let user know something
1817 m_timeDilation = 1 - step_time / timeStep;
1818 if (m_timeDilation < 0)
1819 m_timeDilation = 0;
1820 step_time = 0;
1821 }
1822 }
1823
1824// return nodeframes * ODE_STEPSIZE; // return real simulated time
1825 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
1826 }
1827
1828 /// <summary>
1829 public override void GetResults()
1830 {
1831 }
1832
1833 public override bool IsThreaded
1834 {
1835 // for now we won't be multithreaded
1836 get { return (false); }
1837 }
1838
1839 public float GetTerrainHeightAtXY(float x, float y)
1840 {
1841 // assumes 1m size grid and constante size square regions
1842 // needs to know about sims around in future
1843 // region offset in mega position
1844
1845 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1846 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1847
1848 IntPtr heightFieldGeom = IntPtr.Zero;
1849
1850 // get region map
1851 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
1852 return 0f;
1853
1854 if (heightFieldGeom == IntPtr.Zero)
1855 return 0f;
1856
1857 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1858 return 0f;
1859
1860 // TerrainHeightField for ODE as offset 1m
1861 x += 1f - offsetX;
1862 y += 1f - offsetY;
1863
1864 // make position fit into array
1865 if (x < 0)
1866 x = 0;
1867 if (y < 0)
1868 y = 0;
1869
1870 // integer indexs
1871 int ix;
1872 int iy;
1873 // interpolators offset
1874 float dx;
1875 float dy;
1876
1877 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
1878
1879 // we still have square fixed size regions
1880 // also flip x and y because of how map is done for ODE fliped axis
1881 // so ix,iy,dx and dy are inter exchanged
1882 if (x < regsize - 1)
1883 {
1884 iy = (int)x;
1885 dy = x - (float)iy;
1886 }
1887 else // out world use external height
1888 {
1889 iy = regsize - 1;
1890 dy = 0;
1891 }
1892 if (y < regsize - 1)
1893 {
1894 ix = (int)y;
1895 dx = y - (float)ix;
1896 }
1897 else
1898 {
1899 ix = regsize - 1;
1900 dx = 0;
1901 }
1902
1903 float h0;
1904 float h1;
1905 float h2;
1906
1907 iy *= regsize;
1908 iy += ix; // all indexes have iy + ix
1909
1910 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
1911
1912 if ((dx + dy) <= 1.0f)
1913 {
1914 h0 = ((float)heights[iy]); // 0,0 vertice
1915 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
1916 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
1917 }
1918 else
1919 {
1920 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
1921 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
1922 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
1923 }
1924
1925 return h0 + h1 + h2;
1926 }
1927 public override void SetTerrain(float[] heightMap)
1928 {
1929 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
1930 {
1931 if (m_parentScene is OdeScene)
1932 {
1933 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
1934 }
1935 }
1936 else
1937 {
1938 SetTerrain(heightMap, m_worldOffset);
1939 }
1940 }
1941
1942 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
1943 {
1944 SetTerrain(heightMap, pOffset);
1945 }
1946
1947 public void SetTerrain(float[] heightMap, Vector3 pOffset)
1948 {
1949 // assumes 1m size grid and constante size square regions
1950 // needs to know about sims around in future
1951
1952 float[] _heightmap;
1953
1954 uint heightmapWidth = Constants.RegionSize + 2;
1955 uint heightmapHeight = Constants.RegionSize + 2;
1956
1957 uint heightmapWidthSamples = heightmapWidth + 1;
1958 uint heightmapHeightSamples = heightmapHeight + 1;
1959
1960 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
1961
1962 const float scale = 1.0f;
1963 const float offset = 0.0f;
1964 const float thickness = 10f;
1965 const int wrap = 0;
1966
1967 uint regionsize = Constants.RegionSize;
1968
1969 float hfmin = float.MaxValue;
1970 float hfmax = float.MinValue;
1971 float val;
1972 uint xx;
1973 uint yy;
1974
1975 uint maxXXYY = regionsize - 1;
1976 // flipping map adding one margin all around so things don't fall in edges
1977
1978 uint xt = 0;
1979 xx = 0;
1980
1981 for (uint x = 0; x < heightmapWidthSamples; x++)
1982 {
1983 if (x > 1 && xx < maxXXYY)
1984 xx++;
1985 yy = 0;
1986 for (uint y = 0; y < heightmapHeightSamples; y++)
1987 {
1988 if (y > 1 && y < maxXXYY)
1989 yy += regionsize;
1990
1991 val = heightMap[yy + xx];
1992 if (val < 0.0f)
1993 val = 0.0f; // no neg terrain as in chode
1994 _heightmap[xt + y] = val;
1995
1996 if (hfmin > val)
1997 hfmin = val;
1998 if (hfmax < val)
1999 hfmax = val;
2000 }
2001 xt += heightmapHeightSamples;
2002 }
2003 lock (OdeLock)
2004 {
2005 IntPtr GroundGeom = IntPtr.Zero;
2006 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2007 {
2008 RegionTerrain.Remove(pOffset);
2009 if (GroundGeom != IntPtr.Zero)
2010 {
2011 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2012 {
2013 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2014 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2015 TerrainHeightFieldHeights.Remove(GroundGeom);
2016 }
2017 d.SpaceRemove(StaticSpace, GroundGeom);
2018 d.GeomDestroy(GroundGeom);
2019 }
2020 }
2021 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2022
2023 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2024
2025 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2026 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2027 offset, thickness, wrap);
2028
2029 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2030 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2031 if (GroundGeom != IntPtr.Zero)
2032 {
2033 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2034 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2035
2036 }
2037 geom_name_map[GroundGeom] = "Terrain";
2038
2039 d.Matrix3 R = new d.Matrix3();
2040
2041 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2042 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2043
2044
2045 q1 = q1 * q2;
2046
2047 Vector3 v3;
2048 float angle;
2049 q1.GetAxisAngle(out v3, out angle);
2050
2051 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2052 d.GeomSetRotation(GroundGeom, ref R);
2053 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2054 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2055// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2056 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2057 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2058
2059 }
2060 }
2061
2062 public override void DeleteTerrain()
2063 {
2064 }
2065
2066 public float GetWaterLevel()
2067 {
2068 return waterlevel;
2069 }
2070
2071 public override bool SupportsCombining()
2072 {
2073 return true;
2074 }
2075/*
2076 public override void UnCombine(PhysicsScene pScene)
2077 {
2078 IntPtr localGround = IntPtr.Zero;
2079// float[] localHeightfield;
2080 bool proceed = false;
2081 List<IntPtr> geomDestroyList = new List<IntPtr>();
2082
2083 lock (OdeLock)
2084 {
2085 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2086 {
2087 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2088 {
2089 if (geom == localGround)
2090 {
2091// localHeightfield = TerrainHeightFieldHeights[geom];
2092 proceed = true;
2093 }
2094 else
2095 {
2096 geomDestroyList.Add(geom);
2097 }
2098 }
2099
2100 if (proceed)
2101 {
2102 m_worldOffset = Vector3.Zero;
2103 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2104 m_parentScene = null;
2105
2106 foreach (IntPtr g in geomDestroyList)
2107 {
2108 // removingHeightField needs to be done or the garbage collector will
2109 // collect the terrain data before we tell ODE to destroy it causing
2110 // memory corruption
2111 if (TerrainHeightFieldHeights.ContainsKey(g))
2112 {
2113// float[] removingHeightField = TerrainHeightFieldHeights[g];
2114 TerrainHeightFieldHeights.Remove(g);
2115
2116 if (RegionTerrain.ContainsKey(g))
2117 {
2118 RegionTerrain.Remove(g);
2119 }
2120
2121 d.GeomDestroy(g);
2122 //removingHeightField = new float[0];
2123 }
2124 }
2125
2126 }
2127 else
2128 {
2129 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2130 }
2131 }
2132 }
2133 }
2134*/
2135 public override void SetWaterLevel(float baseheight)
2136 {
2137 waterlevel = baseheight;
2138 randomizeWater(waterlevel);
2139 }
2140
2141 public void randomizeWater(float baseheight)
2142 {
2143 const uint heightmapWidth = m_regionWidth + 2;
2144 const uint heightmapHeight = m_regionHeight + 2;
2145 const uint heightmapWidthSamples = m_regionWidth + 2;
2146 const uint heightmapHeightSamples = m_regionHeight + 2;
2147 const float scale = 1.0f;
2148 const float offset = 0.0f;
2149 const float thickness = 2.9f;
2150 const int wrap = 0;
2151
2152 for (int i = 0; i < (258 * 258); i++)
2153 {
2154 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
2155 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
2156 }
2157
2158 lock (OdeLock)
2159 {
2160 if (WaterGeom != IntPtr.Zero)
2161 {
2162 d.SpaceRemove(StaticSpace, WaterGeom);
2163 }
2164 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2165 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
2166 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2167 offset, thickness, wrap);
2168 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
2169 WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2170 if (WaterGeom != IntPtr.Zero)
2171 {
2172 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
2173 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
2174
2175 }
2176 geom_name_map[WaterGeom] = "Water";
2177
2178 d.Matrix3 R = new d.Matrix3();
2179
2180 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2181 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2182
2183 q1 = q1 * q2;
2184 Vector3 v3;
2185 float angle;
2186 q1.GetAxisAngle(out v3, out angle);
2187
2188 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2189 d.GeomSetRotation(WaterGeom, ref R);
2190 d.GeomSetPosition(WaterGeom, 128, 128, 0);
2191
2192 }
2193
2194 }
2195
2196 public override void Dispose()
2197 {
2198 m_rayCastManager.Dispose();
2199 m_rayCastManager = null;
2200
2201 lock (OdeLock)
2202 {
2203 lock (_prims)
2204 {
2205 foreach (OdePrim prm in _prims)
2206 {
2207 RemovePrim(prm);
2208 }
2209 }
2210
2211 if (ContactgeomsArray != IntPtr.Zero)
2212 Marshal.FreeHGlobal(ContactgeomsArray);
2213 if (GlobalContactsArray != IntPtr.Zero)
2214 Marshal.FreeHGlobal(GlobalContactsArray);
2215
2216 d.WorldDestroy(world);
2217 //d.CloseODE();
2218 }
2219 }
2220
2221 public override Dictionary<uint, float> GetTopColliders()
2222 {
2223 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2224 int cnt = 0;
2225 lock (_prims)
2226 {
2227 foreach (OdePrim prm in _prims)
2228 {
2229 if (prm.CollisionScore > 0)
2230 {
2231 returncolliders.Add(prm.m_localID, prm.CollisionScore);
2232 cnt++;
2233 prm.CollisionScore = 0f;
2234 if (cnt > 25)
2235 {
2236 break;
2237 }
2238 }
2239 }
2240 }
2241 return returncolliders;
2242 }
2243
2244 public override bool SupportsRayCast()
2245 {
2246 return true;
2247 }
2248
2249 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2250 {
2251 if (retMethod != null)
2252 {
2253 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2254 }
2255 }
2256
2257 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2258 {
2259 if (retMethod != null)
2260 {
2261 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2262 }
2263 }
2264
2265 // don't like this
2266 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2267 {
2268 ContactResult[] ourResults = null;
2269 RayCallback retMethod = delegate(List<ContactResult> results)
2270 {
2271 ourResults = new ContactResult[results.Count];
2272 results.CopyTo(ourResults, 0);
2273 };
2274 int waitTime = 0;
2275 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2276 while (ourResults == null && waitTime < 1000)
2277 {
2278 Thread.Sleep(1);
2279 waitTime++;
2280 }
2281 if (ourResults == null)
2282 return new List<ContactResult>();
2283 return new List<ContactResult>(ourResults);
2284 }
2285
2286 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2287 {
2288 if (retMethod != null && actor !=null)
2289 {
2290 IntPtr geom;
2291 if (actor is OdePrim)
2292 geom = ((OdePrim)actor).prim_geom;
2293 else if (actor is OdeCharacter)
2294 geom = ((OdePrim)actor).prim_geom;
2295 else
2296 return;
2297 if (geom == IntPtr.Zero)
2298 return;
2299 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2300 }
2301 }
2302
2303 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2304 {
2305 if (retMethod != null && actor != null)
2306 {
2307 IntPtr geom;
2308 if (actor is OdePrim)
2309 geom = ((OdePrim)actor).prim_geom;
2310 else if (actor is OdeCharacter)
2311 geom = ((OdePrim)actor).prim_geom;
2312 else
2313 return;
2314 if (geom == IntPtr.Zero)
2315 return;
2316
2317 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2318 }
2319 }
2320
2321 // don't like this
2322 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2323 {
2324 if (actor != null)
2325 {
2326 IntPtr geom;
2327 if (actor is OdePrim)
2328 geom = ((OdePrim)actor).prim_geom;
2329 else if (actor is OdeCharacter)
2330 geom = ((OdePrim)actor).prim_geom;
2331 else
2332 return new List<ContactResult>();
2333 if (geom == IntPtr.Zero)
2334 return new List<ContactResult>();
2335
2336 ContactResult[] ourResults = null;
2337 RayCallback retMethod = delegate(List<ContactResult> results)
2338 {
2339 ourResults = new ContactResult[results.Count];
2340 results.CopyTo(ourResults, 0);
2341 };
2342 int waitTime = 0;
2343 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2344 while (ourResults == null && waitTime < 1000)
2345 {
2346 Thread.Sleep(1);
2347 waitTime++;
2348 }
2349 if (ourResults == null)
2350 return new List<ContactResult>();
2351 return new List<ContactResult>(ourResults);
2352 }
2353 return new List<ContactResult>();
2354 }
2355 }
2356}