aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/Ode
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode')
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs1409
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments630
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs974
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs3387
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs434
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs48
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/OdePlugin.cs90
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/OdeScene.cs4319
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs128
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/drawstuff.cs98
11 files changed, 11575 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs
new file mode 100644
index 0000000..076da78
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/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")]
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.8.2.*")]
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
new file mode 100644
index 0000000..05eaf2a
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs
@@ -0,0 +1,1409 @@
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 public enum dParam : int
43 {
44 LowStop = 0,
45 HiStop = 1,
46 Vel = 2,
47 FMax = 3,
48 FudgeFactor = 4,
49 Bounce = 5,
50 CFM = 6,
51 StopERP = 7,
52 StopCFM = 8,
53 LoStop2 = 256,
54 HiStop2 = 257,
55 Vel2 = 258,
56 FMax2 = 259,
57 StopERP2 = 7 + 256,
58 StopCFM2 = 8 + 256,
59 LoStop3 = 512,
60 HiStop3 = 513,
61 Vel3 = 514,
62 FMax3 = 515,
63 StopERP3 = 7 + 512,
64 StopCFM3 = 8 + 512
65 }
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 bool _zeroFlag = false;
74 private bool m_lastUpdateSent = false;
75 private Vector3 _velocity;
76 private Vector3 m_taintTargetVelocity;
77 private Vector3 _target_velocity;
78 private Vector3 _acceleration;
79 private Vector3 m_rotationalVelocity;
80 private float m_mass = 80f;
81 private float m_density = 60f;
82 private bool m_pidControllerActive = true;
83 private float PID_D = 800.0f;
84 private float PID_P = 900.0f;
85 //private static float POSTURE_SERVO = 10000.0f;
86 private float CAPSULE_RADIUS = 0.37f;
87 private float CAPSULE_LENGTH = 2.140599f;
88 private float m_tensor = 3800000f;
89// private float heightFudgeFactor = 0.52f;
90 private float walkDivisor = 1.3f;
91 private 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_wascolliding = false;
96 private bool m_wascollidingGround = false;
97 private bool m_iscollidingObj = false;
98 private bool m_alwaysRun = false;
99 private bool m_hackSentFall = false;
100 private bool m_hackSentFly = false;
101 private int m_requestedUpdateFrequency = 0;
102 private Vector3 m_taintPosition;
103 internal bool m_avatarplanted = false;
104 /// <summary>
105 /// Hold set forces so we can process them outside physics calculations. This prevents race conditions if we set force
106 /// while calculatios are going on
107 /// </summary>
108 private Vector3 m_taintForce;
109
110 // taints and their non-tainted counterparts
111 private bool m_isPhysical = false; // the current physical status
112 private bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
113 internal float MinimumGroundFlightOffset = 3f;
114
115 private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
116
117 /// <summary>
118 /// Base movement for calculating tilt.
119 /// </summary>
120 private float m_tiltBaseMovement = (float)Math.Sqrt(2);
121
122 /// <summary>
123 /// Used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
124 /// </summary>
125 private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f;
126
127 private float m_buoyancy = 0f;
128
129 // private CollisionLocker ode;
130 private bool[] m_colliderarr = new bool[11];
131 private bool[] m_colliderGroundarr = new bool[11];
132
133 // Default we're a Character
134 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
135
136 // Default, Collide with Other Geometries, spaces, bodies and characters.
137 private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
138 | CollisionCategories.Space
139 | CollisionCategories.Body
140 | CollisionCategories.Character
141 | CollisionCategories.Land);
142 /// <summary>
143 /// Body for dynamics simulation
144 /// </summary>
145 internal IntPtr Body { get; private set; }
146
147 private OdeScene _parent_scene;
148
149 /// <summary>
150 /// Collision geometry
151 /// </summary>
152 internal IntPtr Shell { get; private set; }
153
154 private IntPtr Amotor = IntPtr.Zero;
155 private d.Mass ShellMass;
156
157 private int m_eventsubscription = 0;
158 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
159
160 // unique UUID of this character object
161 internal UUID m_uuid { get; private set; }
162 internal bool bad = false;
163
164 /// <summary>
165 /// ODE Avatar.
166 /// </summary>
167 /// <param name="avName"></param>
168 /// <param name="parent_scene"></param>
169 /// <param name="pos"></param>
170 /// <param name="vel"></param>
171 /// <param name="size"></param>
172 /// <param name="pid_d"></param>
173 /// <param name="pid_p"></param>
174 /// <param name="capsule_radius"></param>
175 /// <param name="tensor"></param>
176 /// <param name="density">
177 /// Only used right now to return information to LSL. Not actually used to set mass in ODE!
178 /// </param>
179 /// <param name="walk_divisor"></param>
180 /// <param name="rundivisor"></param>
181 public OdeCharacter(
182 String avName, OdeScene parent_scene, Vector3 pos, Vector3 vel, Vector3 size, float pid_d, float pid_p,
183 float capsule_radius, float tensor, float density,
184 float walk_divisor, float rundivisor)
185 {
186 m_uuid = UUID.Random();
187
188 if (pos.IsFinite())
189 {
190 if (pos.Z > 9999999f)
191 {
192 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
193 }
194 if (pos.Z < -90000f)
195 {
196 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
197 }
198
199 _position = pos;
200 m_taintPosition = pos;
201 }
202 else
203 {
204 _position
205 = new Vector3(
206 (float)_parent_scene.WorldExtents.X * 0.5f,
207 (float)_parent_scene.WorldExtents.Y * 0.5f,
208 parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
209 m_taintPosition = _position;
210
211 m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
212 }
213
214 _velocity = vel;
215 m_taintTargetVelocity = vel;
216
217 _parent_scene = parent_scene;
218
219 PID_D = pid_d;
220 PID_P = pid_p;
221 CAPSULE_RADIUS = capsule_radius;
222 m_tensor = tensor;
223 m_density = density;
224// heightFudgeFactor = height_fudge_factor;
225 walkDivisor = walk_divisor;
226 runDivisor = rundivisor;
227
228 // m_StandUpRotation =
229 // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
230 // 0.5f);
231
232 // We can set taint and actual to be the same here, since the entire character will be set up when the
233 // m_tainted_isPhysical is processed.
234 SetTaintedCapsuleLength(size);
235 CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
236
237 m_isPhysical = false; // current status: no ODE information exists
238 m_tainted_isPhysical = true; // new tainted status: need to create ODE information
239
240 _parent_scene.AddPhysicsActorTaint(this);
241
242 Name = avName;
243 }
244
245 public override int PhysicsActorType
246 {
247 get { return (int) ActorTypes.Agent; }
248 set { return; }
249 }
250
251 /// <summary>
252 /// If this is set, the avatar will move faster
253 /// </summary>
254 public override bool SetAlwaysRun
255 {
256 get { return m_alwaysRun; }
257 set { m_alwaysRun = value; }
258 }
259
260 public override bool Grabbed
261 {
262 set { return; }
263 }
264
265 public override bool Selected
266 {
267 set { return; }
268 }
269
270 public override float Buoyancy
271 {
272 get { return m_buoyancy; }
273 set { m_buoyancy = value; }
274 }
275
276 public override bool FloatOnWater
277 {
278 set { return; }
279 }
280
281 public override bool IsPhysical
282 {
283 get { return false; }
284 set { return; }
285 }
286
287 public override bool ThrottleUpdates
288 {
289 get { return false; }
290 set { return; }
291 }
292
293 public override bool Flying
294 {
295 get { return flying; }
296 set
297 {
298 flying = value;
299// m_log.DebugFormat("[ODE CHARACTER]: Set OdeCharacter Flying to {0}", flying);
300 }
301 }
302
303 /// <summary>
304 /// Returns if the avatar is colliding in general.
305 /// This includes the ground and objects and avatar.
306 /// </summary>
307 public override bool IsColliding
308 {
309 get { return m_iscolliding; }
310 set
311 {
312 int i;
313 int truecount = 0;
314 int falsecount = 0;
315
316 if (m_colliderarr.Length >= 10)
317 {
318 for (i = 0; i < 10; i++)
319 {
320 m_colliderarr[i] = m_colliderarr[i + 1];
321 }
322 }
323 m_colliderarr[10] = value;
324
325 for (i = 0; i < 11; i++)
326 {
327 if (m_colliderarr[i])
328 {
329 truecount++;
330 }
331 else
332 {
333 falsecount++;
334 }
335 }
336
337 // Equal truecounts and false counts means we're colliding with something.
338
339 if (falsecount > 1.2*truecount)
340 {
341 m_iscolliding = false;
342 }
343 else
344 {
345 m_iscolliding = true;
346 }
347
348 if (m_wascolliding != m_iscolliding)
349 {
350 //base.SendCollisionUpdate(new CollisionEventUpdate());
351 }
352
353 m_wascolliding = m_iscolliding;
354 }
355 }
356
357 /// <summary>
358 /// Returns if an avatar is colliding with the ground
359 /// </summary>
360 public override bool CollidingGround
361 {
362 get { return m_iscollidingGround; }
363 set
364 {
365 // Collisions against the ground are not really reliable
366 // So, to get a consistant value we have to average the current result over time
367 // Currently we use 1 second = 10 calls to this.
368 int i;
369 int truecount = 0;
370 int falsecount = 0;
371
372 if (m_colliderGroundarr.Length >= 10)
373 {
374 for (i = 0; i < 10; i++)
375 {
376 m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
377 }
378 }
379 m_colliderGroundarr[10] = value;
380
381 for (i = 0; i < 11; i++)
382 {
383 if (m_colliderGroundarr[i])
384 {
385 truecount++;
386 }
387 else
388 {
389 falsecount++;
390 }
391 }
392
393 // Equal truecounts and false counts means we're colliding with something.
394
395 if (falsecount > 1.2*truecount)
396 {
397 m_iscollidingGround = false;
398 }
399 else
400 {
401 m_iscollidingGround = true;
402 }
403 if (m_wascollidingGround != m_iscollidingGround)
404 {
405 //base.SendCollisionUpdate(new CollisionEventUpdate());
406 }
407 m_wascollidingGround = m_iscollidingGround;
408 }
409 }
410
411 /// <summary>
412 /// Returns if the avatar is colliding with an object
413 /// </summary>
414 public override bool CollidingObj
415 {
416 get { return m_iscollidingObj; }
417 set
418 {
419 m_iscollidingObj = value;
420 if (value && !m_avatarplanted)
421 m_pidControllerActive = false;
422 else
423 m_pidControllerActive = true;
424 }
425 }
426
427 /// <summary>
428 /// turn the PID controller on or off.
429 /// The PID Controller will turn on all by itself in many situations
430 /// </summary>
431 /// <param name="status"></param>
432 public void SetPidStatus(bool status)
433 {
434 m_pidControllerActive = status;
435 }
436
437 public override bool Stopped
438 {
439 get { return _zeroFlag; }
440 }
441
442 /// <summary>
443 /// This 'puts' an avatar somewhere in the physics space.
444 /// Not really a good choice unless you 'know' it's a good
445 /// spot otherwise you're likely to orbit the avatar.
446 /// </summary>
447 public override Vector3 Position
448 {
449 get { return _position; }
450 set
451 {
452 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
453 {
454 if (value.IsFinite())
455 {
456 if (value.Z > 9999999f)
457 {
458 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
459 }
460 if (value.Z < -90000f)
461 {
462 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
463 }
464
465 m_taintPosition = value;
466 _parent_scene.AddPhysicsActorTaint(this);
467 }
468 else
469 {
470 m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Position from Scene on character {0}", Name);
471 }
472 }
473 }
474 }
475
476 public override Vector3 RotationalVelocity
477 {
478 get { return m_rotationalVelocity; }
479 set { m_rotationalVelocity = value; }
480 }
481
482 /// <summary>
483 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
484 /// and use it to offset landings properly
485 /// </summary>
486 public override Vector3 Size
487 {
488 get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
489 set
490 {
491 SetTaintedCapsuleLength(value);
492
493 // If we reset velocity here, then an avatar stalls when it crosses a border for the first time
494 // (as the height of the new root agent is set).
495// Velocity = Vector3.Zero;
496
497 _parent_scene.AddPhysicsActorTaint(this);
498 }
499 }
500
501 private void SetTaintedCapsuleLength(Vector3 size)
502 {
503 if (size.IsFinite())
504 {
505 m_pidControllerActive = true;
506
507 m_tainted_CAPSULE_LENGTH = size.Z - CAPSULE_RADIUS * 2.0f;
508
509 // m_log.InfoFormat("[ODE CHARACTER]: Size = {0}, Capsule Length = {1} (Capsule Radius = {2})",
510 // size, m_tainted_CAPSULE_LENGTH, CAPSULE_RADIUS);
511 }
512 else
513 {
514 m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size for {0} in {1}", Name, _parent_scene.Name);
515 }
516 }
517
518 private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
519 {
520 movementVector.Z = 0f;
521 float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
522 if (magnitude < 0.1f) return;
523
524 // normalize the velocity vector
525 float invMagnitude = 1.0f / magnitude;
526 movementVector.X *= invMagnitude;
527 movementVector.Y *= invMagnitude;
528
529 // if we change the capsule heading too often, the capsule can fall down
530 // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
531 // meaning only 4 possible capsule tilt orientations
532 if (movementVector.X > 0)
533 {
534 // east
535 if (movementVector.Y > 0)
536 {
537 // northeast
538 movementVector.X = m_tiltBaseMovement;
539 movementVector.Y = m_tiltBaseMovement;
540 }
541 else
542 {
543 // southeast
544 movementVector.X = m_tiltBaseMovement;
545 movementVector.Y = -m_tiltBaseMovement;
546 }
547 }
548 else
549 {
550 // west
551 if (movementVector.Y > 0)
552 {
553 // northwest
554 movementVector.X = -m_tiltBaseMovement;
555 movementVector.Y = m_tiltBaseMovement;
556 }
557 else
558 {
559 // southwest
560 movementVector.X = -m_tiltBaseMovement;
561 movementVector.Y = -m_tiltBaseMovement;
562 }
563 }
564
565 // movementVector.Z is zero
566
567 // calculate tilt components based on desired amount of tilt and current (snapped) heading.
568 // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
569 float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
570 float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
571
572 //m_log.Debug("[ODE CHARACTER]: changing avatar tilt");
573 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
574 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
575 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
576 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
577 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
578 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
579 }
580
581 /// <summary>
582 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
583 /// This may be used in calculations in the scene/scenepresence
584 /// </summary>
585 public override float Mass
586 {
587 get
588 {
589 float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH);
590 return m_density * AVvolume;
591 }
592 }
593
594 public override void link(PhysicsActor obj) {}
595
596 public override void delink() {}
597
598 public override void LockAngularMotion(Vector3 axis) {}
599
600// This code is very useful. Written by DanX0r. We're just not using it right now.
601// Commented out to prevent a warning.
602//
603// private void standupStraight()
604// {
605// // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
606// // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
607// // change appearance and when you enter the simulator
608// // After this routine is done, the amotor stabilizes much quicker
609// d.Vector3 feet;
610// d.Vector3 head;
611// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
612// d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
613// float posture = head.Z - feet.Z;
614
615// // restoring force proportional to lack of posture:
616// float servo = (2.5f - posture) * POSTURE_SERVO;
617// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
618// d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
619// //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
620// //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyFArotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
621// }
622
623 public override Vector3 Force
624 {
625 get { return _target_velocity; }
626 set { return; }
627 }
628
629 public override int VehicleType
630 {
631 get { return 0; }
632 set { return; }
633 }
634
635 public override void VehicleFloatParam(int param, float value)
636 {
637 }
638
639 public override void VehicleVectorParam(int param, Vector3 value)
640 {
641 }
642
643 public override void VehicleRotationParam(int param, Quaternion rotation)
644 {
645 }
646
647 public override void VehicleFlags(int param, bool remove)
648 {
649 }
650
651 public override void SetVolumeDetect(int param)
652 {
653 }
654
655 public override Vector3 CenterOfMass
656 {
657 get { return Vector3.Zero; }
658 }
659
660 public override Vector3 GeometricCenter
661 {
662 get { return Vector3.Zero; }
663 }
664
665 public override PrimitiveBaseShape Shape
666 {
667 set { return; }
668 }
669
670 public override Vector3 TargetVelocity
671 {
672 get
673 {
674 return m_taintTargetVelocity;
675 }
676
677 set
678 {
679 Velocity = value;
680 }
681 }
682
683
684 public override Vector3 Velocity
685 {
686 get
687 {
688 // There's a problem with Vector3.Zero! Don't Use it Here!
689 if (_zeroFlag)
690 return Vector3.Zero;
691 m_lastUpdateSent = false;
692 return _velocity;
693 }
694
695 set
696 {
697 if (value.IsFinite())
698 {
699 m_pidControllerActive = true;
700 m_taintTargetVelocity = value;
701 _parent_scene.AddPhysicsActorTaint(this);
702 }
703 else
704 {
705 m_log.WarnFormat("[ODE CHARACTER]: Got a NaN velocity from Scene for {0}", Name);
706 }
707
708// m_log.DebugFormat("[PHYSICS]: Set target velocity of {0}", m_taintTargetVelocity);
709 }
710 }
711
712 public override Vector3 Torque
713 {
714 get { return Vector3.Zero; }
715 set { return; }
716 }
717
718 public override float CollisionScore
719 {
720 get { return 0f; }
721 set { }
722 }
723
724 public override bool Kinematic
725 {
726 get { return false; }
727 set { }
728 }
729
730 public override Quaternion Orientation
731 {
732 get { return Quaternion.Identity; }
733 set {
734 //Matrix3 or = Orientation.ToRotationMatrix();
735 //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
736 //d.BodySetRotation(Body, ref ord);
737 }
738 }
739
740 public override Vector3 Acceleration
741 {
742 get { return _acceleration; }
743 set { _acceleration = value; }
744 }
745
746 /// <summary>
747 /// Adds the force supplied to the Target Velocity
748 /// The PID controller takes this target velocity and tries to make it a reality
749 /// </summary>
750 /// <param name="force"></param>
751 public override void AddForce(Vector3 force, bool pushforce)
752 {
753 if (force.IsFinite())
754 {
755 if (pushforce)
756 {
757 m_pidControllerActive = false;
758 force *= 100f;
759 m_taintForce += force;
760 _parent_scene.AddPhysicsActorTaint(this);
761
762 // If uncommented, things get pushed off world
763 //
764 // m_log.Debug("Push!");
765 // m_taintTargetVelocity.X += force.X;
766 // m_taintTargetVelocity.Y += force.Y;
767 // m_taintTargetVelocity.Z += force.Z;
768 }
769 else
770 {
771 m_pidControllerActive = true;
772 m_taintTargetVelocity += force;
773 }
774 }
775 else
776 {
777 m_log.WarnFormat("[ODE CHARACTER]: Got a NaN force applied to {0}", Name);
778 }
779 //m_lastUpdateSent = false;
780 }
781
782 public override void AddAngularForce(Vector3 force, bool pushforce)
783 {
784 }
785
786 public override void SetMomentum(Vector3 momentum)
787 {
788 }
789
790 /// <summary>
791 /// Called from Simulate
792 /// This is the avatar's movement control + PID Controller
793 /// </summary>
794 /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
795 /// position or velocity).
796 /// </param>
797 internal void Move(List<OdeCharacter> defects)
798 {
799 // no lock; for now it's only called from within Simulate()
800
801 // If the PID Controller isn't active then we set our force
802 // calculating base velocity to the current position
803
804 if (Body == IntPtr.Zero)
805 return;
806
807 if (m_pidControllerActive == false)
808 {
809 _zeroPosition = d.BodyGetPosition(Body);
810 }
811 //PidStatus = true;
812
813 d.Vector3 localpos = d.BodyGetPosition(Body);
814 Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
815
816 if (!localPos.IsFinite())
817 {
818 m_log.WarnFormat(
819 "[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite! Removing from physics scene.",
820 localPos, Name);
821
822 defects.Add(this);
823
824 return;
825 }
826
827 Vector3 vec = Vector3.Zero;
828 d.Vector3 vel = d.BodyGetLinearVel(Body);
829
830// m_log.DebugFormat(
831// "[ODE CHARACTER]: Current velocity in Move() is <{0},{1},{2}>, target {3} for {4}",
832// vel.X, vel.Y, vel.Z, _target_velocity, Name);
833
834 float movementdivisor = 1f;
835
836 if (!m_alwaysRun)
837 {
838 movementdivisor = walkDivisor;
839 }
840 else
841 {
842 movementdivisor = runDivisor;
843 }
844
845 // if velocity is zero, use position control; otherwise, velocity control
846 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
847 {
848 // keep track of where we stopped. No more slippin' & slidin'
849 if (!_zeroFlag)
850 {
851 _zeroFlag = true;
852 _zeroPosition = d.BodyGetPosition(Body);
853 }
854
855 if (m_pidControllerActive)
856 {
857 // We only want to deactivate the PID Controller if we think we want to have our surrogate
858 // react to the physics scene by moving it's position.
859 // Avatar to Avatar collisions
860 // Prim to avatar collisions
861
862 d.Vector3 pos = d.BodyGetPosition(Body);
863 vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
864 vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
865 if (flying)
866 {
867 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
868 }
869 }
870 //PidStatus = true;
871 }
872 else
873 {
874 m_pidControllerActive = true;
875 _zeroFlag = false;
876 if (m_iscolliding && !flying)
877 {
878 // We're standing on something
879 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
880 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
881 }
882 else if (m_iscolliding && flying)
883 {
884 // We're flying and colliding with something
885 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
886 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
887 }
888 else if (!m_iscolliding && flying)
889 {
890 // we're in mid air suspended
891 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
892 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
893
894// m_log.DebugFormat(
895// "[ODE CHARACTER]: !m_iscolliding && flying, vec {0}, _target_velocity {1}, movementdivisor {2}, vel {3}",
896// vec, _target_velocity, movementdivisor, vel);
897 }
898
899 if (flying)
900 {
901 // This also acts as anti-gravity so that we hover when flying rather than fall.
902 vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
903 }
904 else
905 {
906 if (m_iscolliding && _target_velocity.Z > 0.0f)
907 {
908 // We're colliding with something and we're not flying but we're moving
909 // This means we're walking or running.
910 d.Vector3 pos = d.BodyGetPosition(Body);
911 vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
912 vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
913 vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
914 }
915 else if (!m_iscolliding)
916 {
917 // we're not colliding and we're not flying so that means we're falling!
918 // m_iscolliding includes collisions with the ground.
919 vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
920 vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
921 }
922 }
923 }
924
925 if (flying)
926 {
927 // Anti-gravity so that we hover when flying rather than fall.
928 vec.Z += ((-1 * _parent_scene.gravityz) * m_mass);
929
930 //Added for auto fly height. Kitto Flora
931 //d.Vector3 pos = d.BodyGetPosition(Body);
932 float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
933
934 if (_position.Z < target_altitude)
935 {
936 vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
937 }
938 // end add Kitto Flora
939 }
940
941 if (vec.IsFinite())
942 {
943 // Apply the total force acting on this avatar
944 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
945
946 if (!_zeroFlag)
947 AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
948 }
949 else
950 {
951 m_log.WarnFormat(
952 "[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}. Removing character from physics scene.",
953 vec, Name);
954
955 defects.Add(this);
956
957 return;
958 }
959
960 d.Vector3 newVel = d.BodyGetLinearVel(Body);
961 if (newVel.X >= 256 || newVel.X <= 256 || newVel.Y >= 256 || newVel.Y <= 256 || newVel.Z >= 256 || newVel.Z <= 256)
962 {
963// m_log.DebugFormat(
964// "[ODE CHARACTER]: Limiting falling velocity from {0} to {1} for {2}", newVel.Z, -9.8, Name);
965
966 newVel.X = Util.Clamp<float>(newVel.X, -255f, 255f);
967 newVel.Y = Util.Clamp<float>(newVel.Y, -255f, 255f);
968
969 if (!flying)
970 newVel.Z
971 = Util.Clamp<float>(
972 newVel.Z, -_parent_scene.AvatarTerminalVelocity, _parent_scene.AvatarTerminalVelocity);
973 else
974 newVel.Z = Util.Clamp<float>(newVel.Z, -255f, 255f);
975
976 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
977 }
978 }
979
980 /// <summary>
981 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
982 /// </summary>
983 /// <param name="defects">The character will be added to this list if there is something wrong (non-finite
984 /// position or velocity).
985 /// </param>
986 internal void UpdatePositionAndVelocity(List<OdeCharacter> defects)
987 {
988 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
989 d.Vector3 newPos;
990 try
991 {
992 newPos = d.BodyGetPosition(Body);
993 }
994 catch (NullReferenceException)
995 {
996 bad = true;
997 defects.Add(this);
998 newPos = new d.Vector3(_position.X, _position.Y, _position.Z);
999 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1000 m_log.WarnFormat("[ODE CHARACTER]: Avatar Null reference for Avatar {0}, physical actor {1}", Name, m_uuid);
1001
1002 return;
1003 }
1004
1005 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
1006 if (newPos.X < 0.0f) newPos.X = 0.0f;
1007 if (newPos.Y < 0.0f) newPos.Y = 0.0f;
1008 if (newPos.X > (int)_parent_scene.WorldExtents.X - 0.05f) newPos.X = (int)_parent_scene.WorldExtents.X - 0.05f;
1009 if (newPos.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) newPos.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
1010
1011 _position.X = newPos.X;
1012 _position.Y = newPos.Y;
1013 _position.Z = newPos.Z;
1014
1015 // I think we need to update the taintPosition too -- Diva 12/24/10
1016 m_taintPosition = _position;
1017
1018 // Did we move last? = zeroflag
1019 // This helps keep us from sliding all over
1020
1021 if (_zeroFlag)
1022 {
1023 _velocity = Vector3.Zero;
1024
1025 // Did we send out the 'stopped' message?
1026 if (!m_lastUpdateSent)
1027 {
1028 m_lastUpdateSent = true;
1029 //base.RequestPhysicsterseUpdate();
1030 }
1031 }
1032 else
1033 {
1034 m_lastUpdateSent = false;
1035 d.Vector3 newVelocity;
1036
1037 try
1038 {
1039 newVelocity = d.BodyGetLinearVel(Body);
1040 }
1041 catch (NullReferenceException)
1042 {
1043 newVelocity.X = _velocity.X;
1044 newVelocity.Y = _velocity.Y;
1045 newVelocity.Z = _velocity.Z;
1046 }
1047
1048 _velocity.X = newVelocity.X;
1049 _velocity.Y = newVelocity.Y;
1050 _velocity.Z = newVelocity.Z;
1051
1052 if (_velocity.Z < -6 && !m_hackSentFall)
1053 {
1054 m_hackSentFall = true;
1055 m_pidControllerActive = false;
1056 }
1057 else if (flying && !m_hackSentFly)
1058 {
1059 //m_hackSentFly = true;
1060 //base.SendCollisionUpdate(new CollisionEventUpdate());
1061 }
1062 else
1063 {
1064 m_hackSentFly = false;
1065 m_hackSentFall = false;
1066 }
1067 }
1068 }
1069
1070 /// <summary>
1071 /// This creates the Avatar's physical Surrogate in ODE at the position supplied
1072 /// </summary>
1073 /// <remarks>
1074 /// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
1075 /// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
1076 /// place that is safe to call this routine AvatarGeomAndBodyCreation.
1077 /// </remarks>
1078 /// <param name="npositionX"></param>
1079 /// <param name="npositionY"></param>
1080 /// <param name="npositionZ"></param>
1081 /// <param name="tensor"></param>
1082 private void CreateOdeStructures(float npositionX, float npositionY, float npositionZ, float tensor)
1083 {
1084 if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
1085 {
1086 m_log.ErrorFormat(
1087 "[ODE CHARACTER]: Creating ODE structures for {0} even though some already exist. Shell = {1}, Body = {2}, Amotor = {3}",
1088 Name, Shell, Body, Amotor);
1089 }
1090
1091 int dAMotorEuler = 1;
1092// _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1093 if (CAPSULE_LENGTH <= 0)
1094 {
1095 m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
1096 CAPSULE_LENGTH = 0.01f;
1097 }
1098
1099 if (CAPSULE_RADIUS <= 0)
1100 {
1101 m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
1102 CAPSULE_RADIUS = 0.01f;
1103 }
1104
1105// lock (OdeScene.UniversalColliderSyncObject)
1106 Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
1107
1108 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
1109 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
1110
1111 d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
1112 Body = d.BodyCreate(_parent_scene.world);
1113 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
1114
1115 _position.X = npositionX;
1116 _position.Y = npositionY;
1117 _position.Z = npositionZ;
1118
1119 m_taintPosition = _position;
1120
1121 d.BodySetMass(Body, ref ShellMass);
1122 d.Matrix3 m_caprot;
1123 // 90 Stand up on the cap of the capped cyllinder
1124 if (_parent_scene.IsAvCapsuleTilted)
1125 {
1126 d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
1127 }
1128 else
1129 {
1130 d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
1131 }
1132
1133 d.GeomSetRotation(Shell, ref m_caprot);
1134 d.BodySetRotation(Body, ref m_caprot);
1135
1136 d.GeomSetBody(Shell, Body);
1137
1138 // The purpose of the AMotor here is to keep the avatar's physical
1139 // surrogate from rotating while moving
1140 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1141 d.JointAttach(Amotor, Body, IntPtr.Zero);
1142 d.JointSetAMotorMode(Amotor, dAMotorEuler);
1143 d.JointSetAMotorNumAxes(Amotor, 3);
1144 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
1145 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
1146 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
1147 d.JointSetAMotorAngle(Amotor, 0, 0);
1148 d.JointSetAMotorAngle(Amotor, 1, 0);
1149 d.JointSetAMotorAngle(Amotor, 2, 0);
1150
1151 // These lowstops and high stops are effectively (no wiggle room)
1152 if (_parent_scene.IsAvCapsuleTilted)
1153 {
1154 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
1155 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
1156 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
1157 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
1158 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
1159 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
1160 }
1161 else
1162 {
1163 #region Documentation of capsule motor LowStop and HighStop parameters
1164 // Intentionally introduce some tilt into the capsule by setting
1165 // the motor stops to small epsilon values. This small tilt prevents
1166 // the capsule from falling into the terrain; a straight-up capsule
1167 // (with -0..0 motor stops) falls into the terrain for reasons yet
1168 // to be comprehended in their entirety.
1169 #endregion
1170 AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
1171 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
1172 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
1173 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
1174 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
1175 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
1176 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
1177 }
1178
1179 // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the
1180 // capped cyllinder will fall over
1181 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
1182 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
1183
1184 //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
1185 //d.QfromR(
1186 //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
1187 //
1188 //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
1189 //standupStraight();
1190
1191 _parent_scene.geom_name_map[Shell] = Name;
1192 _parent_scene.actor_name_map[Shell] = this;
1193 }
1194
1195 /// <summary>
1196 /// Cleanup the things we use in the scene.
1197 /// </summary>
1198 internal void Destroy()
1199 {
1200 m_tainted_isPhysical = false;
1201 _parent_scene.AddPhysicsActorTaint(this);
1202 }
1203
1204 /// <summary>
1205 /// Used internally to destroy the ODE structures associated with this character.
1206 /// </summary>
1207 internal void DestroyOdeStructures()
1208 {
1209 // Create avatar capsule and related ODE data
1210 if (Shell == IntPtr.Zero || Body == IntPtr.Zero || Amotor == IntPtr.Zero)
1211 {
1212 m_log.ErrorFormat(
1213 "[ODE CHARACTER]: Destroying ODE structures for {0} even though some are already null. Shell = {1}, Body = {2}, Amotor = {3}",
1214 Name, Shell, Body, Amotor);
1215 }
1216
1217 // destroy avatar capsule and related ODE data
1218 if (Amotor != IntPtr.Zero)
1219 {
1220 // Kill the Amotor
1221 d.JointDestroy(Amotor);
1222 Amotor = IntPtr.Zero;
1223 }
1224
1225 //kill the Geometry
1226// _parent_scene.waitForSpaceUnlock(_parent_scene.space);
1227
1228 if (Body != IntPtr.Zero)
1229 {
1230 //kill the body
1231 d.BodyDestroy(Body);
1232 Body = IntPtr.Zero;
1233 }
1234
1235 if (Shell != IntPtr.Zero)
1236 {
1237// lock (OdeScene.UniversalColliderSyncObject)
1238 d.GeomDestroy(Shell);
1239
1240 _parent_scene.geom_name_map.Remove(Shell);
1241 _parent_scene.actor_name_map.Remove(Shell);
1242
1243 Shell = IntPtr.Zero;
1244 }
1245 }
1246
1247 public override void CrossingFailure()
1248 {
1249 }
1250
1251 public override Vector3 PIDTarget { set { return; } }
1252 public override bool PIDActive
1253 {
1254 get { return false; }
1255 set { return; }
1256 }
1257 public override float PIDTau { set { return; } }
1258
1259 public override float PIDHoverHeight { set { return; } }
1260 public override bool PIDHoverActive { set { return; } }
1261 public override PIDHoverType PIDHoverType { set { return; } }
1262 public override float PIDHoverTau { set { return; } }
1263
1264 public override Quaternion APIDTarget{ set { return; } }
1265
1266 public override bool APIDActive{ set { return; } }
1267
1268 public override float APIDStrength{ set { return; } }
1269
1270 public override float APIDDamping{ set { return; } }
1271
1272 public override void SubscribeEvents(int ms)
1273 {
1274 m_requestedUpdateFrequency = ms;
1275 m_eventsubscription = ms;
1276
1277 // Don't clear collision event reporting here. This is called directly from scene code and so can lead
1278 // to a race condition with the simulate loop
1279
1280 _parent_scene.AddCollisionEventReporting(this);
1281 }
1282
1283 public override void UnSubscribeEvents()
1284 {
1285 _parent_scene.RemoveCollisionEventReporting(this);
1286
1287 // Don't clear collision event reporting here. This is called directly from scene code and so can lead
1288 // to a race condition with the simulate loop
1289
1290 m_requestedUpdateFrequency = 0;
1291 m_eventsubscription = 0;
1292 }
1293
1294 internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1295 {
1296 if (m_eventsubscription > 0)
1297 {
1298// m_log.DebugFormat(
1299// "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
1300
1301 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1302 }
1303 }
1304
1305 internal void SendCollisions()
1306 {
1307 if (m_eventsubscription > m_requestedUpdateFrequency)
1308 {
1309 base.SendCollisionUpdate(CollisionEventsThisFrame);
1310
1311 CollisionEventsThisFrame.Clear();
1312 m_eventsubscription = 0;
1313 }
1314 }
1315
1316 public override bool SubscribedEvents()
1317 {
1318 if (m_eventsubscription > 0)
1319 return true;
1320 return false;
1321 }
1322
1323 internal void ProcessTaints()
1324 {
1325 if (m_taintPosition != _position)
1326 {
1327 if (Body != IntPtr.Zero)
1328 {
1329 d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
1330 _position = m_taintPosition;
1331 }
1332 }
1333
1334 if (m_taintForce != Vector3.Zero)
1335 {
1336 if (Body != IntPtr.Zero)
1337 {
1338 // FIXME: This is not a good solution since it's subject to a race condition if a force is another
1339 // thread sets a new force while we're in this loop (since it could be obliterated by
1340 // m_taintForce = Vector3.Zero. Need to lock ProcessTaints() when we set a new tainted force.
1341 d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z);
1342 }
1343
1344 m_taintForce = Vector3.Zero;
1345 }
1346
1347 if (m_taintTargetVelocity != _target_velocity)
1348 _target_velocity = m_taintTargetVelocity;
1349
1350 if (m_tainted_isPhysical != m_isPhysical)
1351 {
1352 if (m_tainted_isPhysical)
1353 {
1354 CreateOdeStructures(_position.X, _position.Y, _position.Z, m_tensor);
1355 _parent_scene.AddCharacter(this);
1356 }
1357 else
1358 {
1359 _parent_scene.RemoveCharacter(this);
1360 DestroyOdeStructures();
1361 }
1362
1363 m_isPhysical = m_tainted_isPhysical;
1364 }
1365
1366 if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
1367 {
1368 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1369 {
1370// m_log.DebugFormat(
1371// "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}",
1372// CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name);
1373
1374 m_pidControllerActive = true;
1375
1376 // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
1377 DestroyOdeStructures();
1378
1379 float prevCapsule = CAPSULE_LENGTH;
1380 CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
1381
1382 CreateOdeStructures(
1383 _position.X,
1384 _position.Y,
1385 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
1386
1387 // As with Size, we reset velocity. However, this isn't strictly necessary since it doesn't
1388 // appear to stall initial region crossings when done here. Being done for consistency.
1389// Velocity = Vector3.Zero;
1390 }
1391 else
1392 {
1393 m_log.Warn("[ODE CHARACTER]: trying to change capsule size for " + Name + ", but the following ODE data is missing - "
1394 + (Shell==IntPtr.Zero ? "Shell ":"")
1395 + (Body==IntPtr.Zero ? "Body ":"")
1396 + (Amotor==IntPtr.Zero ? "Amotor ":""));
1397 }
1398 }
1399 }
1400
1401 internal void AddCollisionFrameTime(int p)
1402 {
1403 // protect it from overflow crashing
1404 if (m_eventsubscription + p >= int.MaxValue)
1405 m_eventsubscription = 0;
1406 m_eventsubscription += p;
1407 }
1408 }
1409}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments
new file mode 100644
index 0000000..1060aa6
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.c_comments
@@ -0,0 +1,630 @@
1/*
2 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
3 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
4 * ODEPrim.cs contains methods dealing with Prim editing, Prim
5 * characteristics and Kinetic motion.
6 * ODEDynamics.cs contains methods dealing with Prim Physical motion
7 * (dynamics) and the associated settings. Old Linear and angular
8 * motors for dynamic motion have been replace with MoveLinear()
9 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
10 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
11 * switch between 'VEHICLE' parameter use and general dynamics
12 * settings use.
13 *
14 * Copyright (c) Contributors, http://opensimulator.org/
15 * See CONTRIBUTORS.TXT for a full list of copyright holders.
16 *
17 * Redistribution and use in source and binary forms, with or without
18 * modification, are permitted provided that the following conditions are met:
19 * * Redistributions of source code must retain the above copyright
20 * notice, this list of conditions and the following disclaimer.
21 * * Redistributions in binary form must reproduce the above copyright
22 * notice, this list of conditions and the following disclaimer in the
23 * documentation and/or other materials provided with the distribution.
24 * * Neither the name of the OpenSimulator Project nor the
25 * names of its contributors may be used to endorse or promote products
26 * derived from this software without specific prior written permission.
27 *
28 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
29 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
30 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
31 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
32 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
33 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
35 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
37 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
38 */
39
40using System;
41using System.Collections.Generic;
42using System.Reflection;
43using System.Runtime.InteropServices;
44using log4net;
45using OpenMetaverse;
46using Ode.NET;
47using OpenSim.Framework;
48using OpenSim.Region.Physics.Manager;
49
50namespace OpenSim.Region.Physics.OdePlugin
51{
52 public class ODEDynamics
53 {
54 public Vehicle Type
55 {
56 get { return m_type; }
57 }
58
59 public IntPtr Body
60 {
61 get { return m_body; }
62 }
63
64 private int frcount = 0; // Used to limit dynamics debug output to
65 // every 100th frame
66
67 // private OdeScene m_parentScene = null;
68 private IntPtr m_body = IntPtr.Zero;
69 private IntPtr m_jointGroup = IntPtr.Zero;
70 private IntPtr m_aMotor = IntPtr.Zero;
71
72
73 // Vehicle properties
74 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
75 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
76 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
77 // HOVER_TERRAIN_ONLY
78 // HOVER_GLOBAL_HEIGHT
79 // NO_DEFLECTION_UP
80 // HOVER_WATER_ONLY
81 // HOVER_UP_ONLY
82 // LIMIT_MOTOR_UP
83 // LIMIT_ROLL_ONLY
84
85 // Linear properties
86 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
87 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
88 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
89 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
90 private float m_linearMotorDecayTimescale = 0;
91 private float m_linearMotorTimescale = 0;
92 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
93 // private bool m_LinearMotorSetLastFrame = false;
94 // private Vector3 m_linearMotorOffset = Vector3.Zero;
95
96 //Angular properties
97 private Vector3 m_angularMotorDirection = Vector3.Zero;
98 private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
99 private Vector3 m_angularFrictionTimescale = Vector3.Zero;
100 private float m_angularMotorDecayTimescale = 0;
101 private float m_angularMotorTimescale = 0;
102 private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
103
104 //Deflection properties
105 // private float m_angularDeflectionEfficiency = 0;
106 // private float m_angularDeflectionTimescale = 0;
107 // private float m_linearDeflectionEfficiency = 0;
108 // private float m_linearDeflectionTimescale = 0;
109
110 //Banking properties
111 // private float m_bankingEfficiency = 0;
112 // private float m_bankingMix = 0;
113 // private float m_bankingTimescale = 0;
114
115 //Hover and Buoyancy properties
116 private float m_VhoverHeight = 0f;
117 private float m_VhoverEfficiency = 0f;
118 private float m_VhoverTimescale = 0f;
119 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
120 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
121 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
122 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
123 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
124
125 //Attractor properties
126 private float m_verticalAttractionEfficiency = 0;
127 private float m_verticalAttractionTimescale = 0;
128
129
130
131
132
133 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
134 {
135 switch (pParam)
136 {
137 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
138 if (pValue < 0.01f) pValue = 0.01f;
139 // m_angularDeflectionEfficiency = pValue;
140 break;
141 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
142 if (pValue < 0.01f) pValue = 0.01f;
143 // m_angularDeflectionTimescale = pValue;
144 break;
145 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
146 if (pValue < 0.01f) pValue = 0.01f;
147 m_angularMotorDecayTimescale = pValue;
148 break;
149 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
150 if (pValue < 0.01f) pValue = 0.01f;
151 m_angularMotorTimescale = pValue;
152 break;
153 case Vehicle.BANKING_EFFICIENCY:
154 if (pValue < 0.01f) pValue = 0.01f;
155 // m_bankingEfficiency = pValue;
156 break;
157 case Vehicle.BANKING_MIX:
158 if (pValue < 0.01f) pValue = 0.01f;
159 // m_bankingMix = pValue;
160 break;
161 case Vehicle.BANKING_TIMESCALE:
162 if (pValue < 0.01f) pValue = 0.01f;
163 // m_bankingTimescale = pValue;
164 break;
165 case Vehicle.BUOYANCY:
166 if (pValue < -1f) pValue = -1f;
167 if (pValue > 1f) pValue = 1f;
168 m_VehicleBuoyancy = pValue;
169 break;
170 case Vehicle.HOVER_EFFICIENCY:
171 if (pValue < 0f) pValue = 0f;
172 if (pValue > 1f) pValue = 1f;
173 m_VhoverEfficiency = pValue;
174 break;
175 case Vehicle.HOVER_HEIGHT:
176 m_VhoverHeight = pValue;
177 break;
178 case Vehicle.HOVER_TIMESCALE:
179 if (pValue < 0.01f) pValue = 0.01f;
180 m_VhoverTimescale = pValue;
181 break;
182 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
183 if (pValue < 0.01f) pValue = 0.01f;
184 // m_linearDeflectionEfficiency = pValue;
185 break;
186 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
187 if (pValue < 0.01f) pValue = 0.01f;
188 // m_linearDeflectionTimescale = pValue;
189 break;
190 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
191 if (pValue < 0.01f) pValue = 0.01f;
192 m_linearMotorDecayTimescale = pValue;
193 break;
194 case Vehicle.LINEAR_MOTOR_TIMESCALE:
195 if (pValue < 0.01f) pValue = 0.01f;
196 m_linearMotorTimescale = pValue;
197 break;
198 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
199 if (pValue < 0.0f) pValue = 0.0f;
200 if (pValue > 1.0f) pValue = 1.0f;
201 m_verticalAttractionEfficiency = pValue;
202 break;
203 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
204 if (pValue < 0.01f) pValue = 0.01f;
205 m_verticalAttractionTimescale = pValue;
206 break;
207
208 // These are vector properties but the engine lets you use a single float value to
209 // set all of the components to the same value
210 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
211 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
212 break;
213 case Vehicle.ANGULAR_MOTOR_DIRECTION:
214 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
215 m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
216 break;
217 case Vehicle.LINEAR_FRICTION_TIMESCALE:
218 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
219 break;
220 case Vehicle.LINEAR_MOTOR_DIRECTION:
221 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
222 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
223 break;
224 case Vehicle.LINEAR_MOTOR_OFFSET:
225 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
226 break;
227
228 }
229
230 }//end ProcessFloatVehicleParam
231
232 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
233 {
234 switch (pParam)
235 {
236 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
237 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
238 break;
239 case Vehicle.ANGULAR_MOTOR_DIRECTION:
240 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
241 m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
242 break;
243 case Vehicle.LINEAR_FRICTION_TIMESCALE:
244 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
245 break;
246 case Vehicle.LINEAR_MOTOR_DIRECTION:
247 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
248 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
249 break;
250 case Vehicle.LINEAR_MOTOR_OFFSET:
251 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
252 break;
253 }
254
255 }//end ProcessVectorVehicleParam
256
257 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
258 {
259 switch (pParam)
260 {
261 case Vehicle.REFERENCE_FRAME:
262 // m_referenceFrame = pValue;
263 break;
264 }
265
266 }//end ProcessRotationVehicleParam
267
268 internal void ProcessTypeChange(Vehicle pType)
269 {
270Console.WriteLine("ProcessTypeChange to " + pType);
271
272 // Set Defaults For Type
273 m_type = pType;
274 switch (pType)
275 {
276 case Vehicle.TYPE_SLED:
277 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
278 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
279 m_linearMotorDirection = Vector3.Zero;
280 m_linearMotorTimescale = 1000;
281 m_linearMotorDecayTimescale = 120;
282 m_angularMotorDirection = Vector3.Zero;
283 m_angularMotorTimescale = 1000;
284 m_angularMotorDecayTimescale = 120;
285 m_VhoverHeight = 0;
286 m_VhoverEfficiency = 1;
287 m_VhoverTimescale = 10;
288 m_VehicleBuoyancy = 0;
289 // m_linearDeflectionEfficiency = 1;
290 // m_linearDeflectionTimescale = 1;
291 // m_angularDeflectionEfficiency = 1;
292 // m_angularDeflectionTimescale = 1000;
293 // m_bankingEfficiency = 0;
294 // m_bankingMix = 1;
295 // m_bankingTimescale = 10;
296 // m_referenceFrame = Quaternion.Identity;
297 m_flags &=
298 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
299 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
300 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
301 break;
302 case Vehicle.TYPE_CAR:
303 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
304 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
305 m_linearMotorDirection = Vector3.Zero;
306 m_linearMotorTimescale = 1;
307 m_linearMotorDecayTimescale = 60;
308 m_angularMotorDirection = Vector3.Zero;
309 m_angularMotorTimescale = 1;
310 m_angularMotorDecayTimescale = 0.8f;
311 m_VhoverHeight = 0;
312 m_VhoverEfficiency = 0;
313 m_VhoverTimescale = 1000;
314 m_VehicleBuoyancy = 0;
315 // // m_linearDeflectionEfficiency = 1;
316 // // m_linearDeflectionTimescale = 2;
317 // // m_angularDeflectionEfficiency = 0;
318 // m_angularDeflectionTimescale = 10;
319 m_verticalAttractionEfficiency = 1;
320 m_verticalAttractionTimescale = 10;
321 // m_bankingEfficiency = -0.2f;
322 // m_bankingMix = 1;
323 // m_bankingTimescale = 1;
324 // m_referenceFrame = Quaternion.Identity;
325 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
326 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
327 VehicleFlag.LIMIT_MOTOR_UP);
328 break;
329 case Vehicle.TYPE_BOAT:
330 m_linearFrictionTimescale = new Vector3(10, 3, 2);
331 m_angularFrictionTimescale = new Vector3(10,10,10);
332 m_linearMotorDirection = Vector3.Zero;
333 m_linearMotorTimescale = 5;
334 m_linearMotorDecayTimescale = 60;
335 m_angularMotorDirection = Vector3.Zero;
336 m_angularMotorTimescale = 4;
337 m_angularMotorDecayTimescale = 4;
338 m_VhoverHeight = 0;
339 m_VhoverEfficiency = 0.5f;
340 m_VhoverTimescale = 2;
341 m_VehicleBuoyancy = 1;
342 // m_linearDeflectionEfficiency = 0.5f;
343 // m_linearDeflectionTimescale = 3;
344 // m_angularDeflectionEfficiency = 0.5f;
345 // m_angularDeflectionTimescale = 5;
346 m_verticalAttractionEfficiency = 0.5f;
347 m_verticalAttractionTimescale = 5;
348 // m_bankingEfficiency = -0.3f;
349 // m_bankingMix = 0.8f;
350 // m_bankingTimescale = 1;
351 // m_referenceFrame = Quaternion.Identity;
352 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
353 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
355 VehicleFlag.LIMIT_MOTOR_UP);
356 break;
357 case Vehicle.TYPE_AIRPLANE:
358 m_linearFrictionTimescale = new Vector3(200, 10, 5);
359 m_angularFrictionTimescale = new Vector3(20, 20, 20);
360 m_linearMotorDirection = Vector3.Zero;
361 m_linearMotorTimescale = 2;
362 m_linearMotorDecayTimescale = 60;
363 m_angularMotorDirection = Vector3.Zero;
364 m_angularMotorTimescale = 4;
365 m_angularMotorDecayTimescale = 4;
366 m_VhoverHeight = 0;
367 m_VhoverEfficiency = 0.5f;
368 m_VhoverTimescale = 1000;
369 m_VehicleBuoyancy = 0;
370 // m_linearDeflectionEfficiency = 0.5f;
371 // m_linearDeflectionTimescale = 3;
372 // m_angularDeflectionEfficiency = 1;
373 // m_angularDeflectionTimescale = 2;
374 m_verticalAttractionEfficiency = 0.9f;
375 m_verticalAttractionTimescale = 2;
376 // m_bankingEfficiency = 1;
377 // m_bankingMix = 0.7f;
378 // m_bankingTimescale = 2;
379 // m_referenceFrame = Quaternion.Identity;
380 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
381 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
382 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
383 break;
384 case Vehicle.TYPE_BALLOON:
385 m_linearFrictionTimescale = new Vector3(5, 5, 5);
386 m_angularFrictionTimescale = new Vector3(10, 10, 10);
387 m_linearMotorDirection = Vector3.Zero;
388 m_linearMotorTimescale = 5;
389 m_linearMotorDecayTimescale = 60;
390 m_angularMotorDirection = Vector3.Zero;
391 m_angularMotorTimescale = 6;
392 m_angularMotorDecayTimescale = 10;
393 m_VhoverHeight = 5;
394 m_VhoverEfficiency = 0.8f;
395 m_VhoverTimescale = 10;
396 m_VehicleBuoyancy = 1;
397 // m_linearDeflectionEfficiency = 0;
398 // m_linearDeflectionTimescale = 5;
399 // m_angularDeflectionEfficiency = 0;
400 // m_angularDeflectionTimescale = 5;
401 m_verticalAttractionEfficiency = 1;
402 m_verticalAttractionTimescale = 1000;
403 // m_bankingEfficiency = 0;
404 // m_bankingMix = 0.7f;
405 // m_bankingTimescale = 5;
406 // m_referenceFrame = Quaternion.Identity;
407 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
408 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
409 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
410 break;
411
412 }
413 }//end SetDefaultsForType
414
415 internal void Enable(IntPtr pBody, OdeScene pParentScene)
416 {
417//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy);
418 if (m_type == Vehicle.TYPE_NONE)
419 return;
420
421 m_body = pBody;
422 //KF: This used to set up the linear and angular joints
423 }
424
425 internal void Step(float pTimestep, OdeScene pParentScene)
426 {
427 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
428 return;
429 frcount++; // used to limit debug comment output
430 if (frcount > 100)
431 frcount = 0;
432
433 MoveLinear(pTimestep, pParentScene);
434 MoveAngular(pTimestep);
435 }// end Step
436
437 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
438 {
439 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
440 {
441 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
442
443 // add drive to body
444 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
445 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
446
447 // This will work temporarily, but we really need to compare speed on an axis
448 // KF: Limit body velocity to applied velocity?
449 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
450 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
451 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
452 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
453 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
454 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
455
456 // decay applied velocity
457 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
458 //Console.WriteLine("decay: " + decayfraction);
459 m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
460 //Console.WriteLine("actual: " + m_linearMotorDirection);
461 }
462 else
463 { // requested is not significant
464 // if what remains of applied is small, zero it.
465 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
466 m_lastLinearVelocityVector = Vector3.Zero;
467 }
468
469
470 // convert requested object velocity to world-referenced vector
471 m_dir = m_lastLinearVelocityVector;
472 d.Quaternion rot = d.BodyGetQuaternion(Body);
473 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
474 m_dir *= rotq; // apply obj rotation to velocity vector
475
476 // add Gravity andBuoyancy
477 // KF: So far I have found no good method to combine a script-requested
478 // .Z velocity and gravity. Therefore only 0g will used script-requested
479 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
480 Vector3 grav = Vector3.Zero;
481 if(m_VehicleBuoyancy < 1.0f)
482 {
483 // There is some gravity, make a gravity force vector
484 // that is applied after object velocity.
485 d.Mass objMass;
486 d.BodyGetMass(Body, out objMass);
487 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
488 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
489 // Preserve the current Z velocity
490 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
491 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
492 } // else its 1.0, no gravity.
493
494 // Check if hovering
495 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
496 {
497 // We should hover, get the target height
498 d.Vector3 pos = d.BodyGetPosition(Body);
499 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
500 {
501 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
502 }
503 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
504 {
505 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
506 }
507 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
508 {
509 m_VhoverTargetHeight = m_VhoverHeight;
510 }
511
512 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
513 {
514 // If body is aready heigher, use its height as target height
515 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
516 }
517
518// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
519// m_VhoverTimescale = 0f; // time to acheive height
520// pTimestep is time since last frame,in secs
521 float herr0 = pos.Z - m_VhoverTargetHeight;
522//if(frcount == 0) Console.WriteLine("herr0=" + herr0);
523 // Replace Vertical speed with correction figure if significant
524 if(Math.Abs(herr0) > 0.01f )
525 {
526 d.Mass objMass;
527 d.BodyGetMass(Body, out objMass);
528 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
529 // m_VhoverEfficiency is not yet implemented
530 }
531 else
532 {
533 m_dir.Z = 0f;
534 }
535 }
536
537 // Apply velocity
538 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
539//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z);
540 // apply gravity force
541 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
542//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z);
543
544
545 // apply friction
546 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
547 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
548 } // end MoveLinear()
549
550 private void MoveAngular(float pTimestep)
551 {
552
553 // m_angularMotorDirection is the latest value from the script, and is decayed here
554 // m_angularMotorDirectionLASTSET is the latest value from the script
555 // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here
556
557 if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
558 {
559 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
560 // ramp up to new value
561 Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
562 m_lastAngularVelocityVector += (addAmount * 10f);
563//if(frcount == 0) Console.WriteLine("add: " + addAmount);
564
565 // limit applied value to what was set by script
566 // This will work temporarily, but we really need to compare speed on an axis
567 if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
568 m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
569 if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y))
570 m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
571 if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
572 m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
573
574 // decay the requested value
575 Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
576 //Console.WriteLine("decay: " + decayfraction);
577 m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
578 //Console.WriteLine("actual: " + m_linearMotorDirection);
579 }
580 // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ?
581
582 // Vertical attractor section
583
584// d.Mass objMass;
585// d.BodyGetMass(Body, out objMass);
586// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
587 float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
588 // get present body rotation
589 d.Quaternion rot = d.BodyGetQuaternion(Body);
590 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
591 // make a vector pointing up
592 Vector3 verterr = Vector3.Zero;
593 verterr.Z = 1.0f;
594 // rotate it to Body Angle
595 verterr = verterr * rotq;
596 // 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.
597 // 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
598 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
599 if (verterr.Z < 0.0f)
600 {
601 verterr.X = 2.0f - verterr.X;
602 verterr.Y = 2.0f - verterr.Y;
603 }
604 // Error is 0 (no error) to +/- 2 (max error)
605 // scale it by servo
606 verterr = verterr * servo;
607
608 // rotate to object frame
609 // verterr = verterr * rotq;
610
611 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
612 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
613 m_lastAngularVelocityVector.X += verterr.Y;
614 m_lastAngularVelocityVector.Y -= verterr.X;
615/*
616if(frcount == 0)
617 {
618// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector);
619 Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}",
620 Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency));
621 }
622 */
623 d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z);
624 // apply friction
625 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
626 m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
627
628 } //end MoveAngular
629 }
630}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
new file mode 100644
index 0000000..2342bfa
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs
@@ -0,0 +1,974 @@
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
41using System;
42using System.Collections.Generic;
43using System.Reflection;
44using System.Runtime.InteropServices;
45using log4net;
46using OpenMetaverse;
47using Ode.NET;
48using OpenSim.Framework;
49using OpenSim.Region.Physics.Manager;
50
51namespace OpenSim.Region.Physics.OdePlugin
52{
53 public class ODEDynamics
54 {
55 public Vehicle Type
56 {
57 get { return m_type; }
58 }
59
60 public IntPtr Body
61 {
62 get { return m_body; }
63 }
64
65 private int frcount = 0; // Used to limit dynamics debug output to
66 // every 100th frame
67
68 // private OdeScene m_parentScene = null;
69 private IntPtr m_body = IntPtr.Zero;
70// private IntPtr m_jointGroup = IntPtr.Zero;
71// private IntPtr m_aMotor = IntPtr.Zero;
72
73
74 // Vehicle properties
75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
76 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
77 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
78 // HOVER_TERRAIN_ONLY
79 // HOVER_GLOBAL_HEIGHT
80 // NO_DEFLECTION_UP
81 // HOVER_WATER_ONLY
82 // HOVER_UP_ONLY
83 // LIMIT_MOTOR_UP
84 // LIMIT_ROLL_ONLY
85 private VehicleFlag m_Hoverflags = (VehicleFlag)0;
86 private Vector3 m_BlockingEndPoint = Vector3.Zero;
87 private Quaternion m_RollreferenceFrame = Quaternion.Identity;
88 // Linear properties
89 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
90 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
91 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
92 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
93 private float m_linearMotorDecayTimescale = 0;
94 private float m_linearMotorTimescale = 0;
95 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
96 private d.Vector3 m_lastPositionVector = new d.Vector3();
97 // private bool m_LinearMotorSetLastFrame = false;
98 // private Vector3 m_linearMotorOffset = Vector3.Zero;
99
100 //Angular properties
101 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
102 private int m_angularMotorApply = 0; // application frame counter
103 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
104 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
105 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
106 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
107 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
108 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
109
110 //Deflection properties
111 // private float m_angularDeflectionEfficiency = 0;
112 // private float m_angularDeflectionTimescale = 0;
113 // private float m_linearDeflectionEfficiency = 0;
114 // private float m_linearDeflectionTimescale = 0;
115
116 //Banking properties
117 // private float m_bankingEfficiency = 0;
118 // private float m_bankingMix = 0;
119 // private float m_bankingTimescale = 0;
120
121 //Hover and Buoyancy properties
122 private float m_VhoverHeight = 0f;
123// private float m_VhoverEfficiency = 0f;
124 private float m_VhoverTimescale = 0f;
125 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
126 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
127 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
128 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
129 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
130
131 //Attractor properties
132 private float m_verticalAttractionEfficiency = 1.0f; // damped
133 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
134
135 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
136 {
137 switch (pParam)
138 {
139 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
140 if (pValue < 0.01f) pValue = 0.01f;
141 // m_angularDeflectionEfficiency = pValue;
142 break;
143 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
144 if (pValue < 0.01f) pValue = 0.01f;
145 // m_angularDeflectionTimescale = pValue;
146 break;
147 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
148 if (pValue < 0.01f) pValue = 0.01f;
149 m_angularMotorDecayTimescale = pValue;
150 break;
151 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
152 if (pValue < 0.01f) pValue = 0.01f;
153 m_angularMotorTimescale = pValue;
154 break;
155 case Vehicle.BANKING_EFFICIENCY:
156 if (pValue < 0.01f) pValue = 0.01f;
157 // m_bankingEfficiency = pValue;
158 break;
159 case Vehicle.BANKING_MIX:
160 if (pValue < 0.01f) pValue = 0.01f;
161 // m_bankingMix = pValue;
162 break;
163 case Vehicle.BANKING_TIMESCALE:
164 if (pValue < 0.01f) pValue = 0.01f;
165 // m_bankingTimescale = pValue;
166 break;
167 case Vehicle.BUOYANCY:
168 if (pValue < -1f) pValue = -1f;
169 if (pValue > 1f) pValue = 1f;
170 m_VehicleBuoyancy = pValue;
171 break;
172// case Vehicle.HOVER_EFFICIENCY:
173// if (pValue < 0f) pValue = 0f;
174// if (pValue > 1f) pValue = 1f;
175// m_VhoverEfficiency = pValue;
176// break;
177 case Vehicle.HOVER_HEIGHT:
178 m_VhoverHeight = pValue;
179 break;
180 case Vehicle.HOVER_TIMESCALE:
181 if (pValue < 0.01f) pValue = 0.01f;
182 m_VhoverTimescale = pValue;
183 break;
184 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
185 if (pValue < 0.01f) pValue = 0.01f;
186 // m_linearDeflectionEfficiency = pValue;
187 break;
188 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
189 if (pValue < 0.01f) pValue = 0.01f;
190 // m_linearDeflectionTimescale = pValue;
191 break;
192 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
193 if (pValue < 0.01f) pValue = 0.01f;
194 m_linearMotorDecayTimescale = pValue;
195 break;
196 case Vehicle.LINEAR_MOTOR_TIMESCALE:
197 if (pValue < 0.01f) pValue = 0.01f;
198 m_linearMotorTimescale = pValue;
199 break;
200 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
201 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
202 if (pValue > 1.0f) pValue = 1.0f;
203 m_verticalAttractionEfficiency = pValue;
204 break;
205 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
206 if (pValue < 0.01f) pValue = 0.01f;
207 m_verticalAttractionTimescale = pValue;
208 break;
209
210 // These are vector properties but the engine lets you use a single float value to
211 // set all of the components to the same value
212 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
213 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
214 break;
215 case Vehicle.ANGULAR_MOTOR_DIRECTION:
216 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
217 m_angularMotorApply = 10;
218 break;
219 case Vehicle.LINEAR_FRICTION_TIMESCALE:
220 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
221 break;
222 case Vehicle.LINEAR_MOTOR_DIRECTION:
223 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
224 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
225 break;
226 case Vehicle.LINEAR_MOTOR_OFFSET:
227 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
228 break;
229
230 }
231 }//end ProcessFloatVehicleParam
232
233 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
234 {
235 switch (pParam)
236 {
237 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
238 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
239 break;
240 case Vehicle.ANGULAR_MOTOR_DIRECTION:
241 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
242 // Limit requested angular speed to 2 rps= 4 pi rads/sec
243 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
244 if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
245 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
246 if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
247 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
248 if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
249 m_angularMotorApply = 10;
250 break;
251 case Vehicle.LINEAR_FRICTION_TIMESCALE:
252 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
253 break;
254 case Vehicle.LINEAR_MOTOR_DIRECTION:
255 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
256 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
257 break;
258 case Vehicle.LINEAR_MOTOR_OFFSET:
259 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
260 break;
261 case Vehicle.BLOCK_EXIT:
262 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
263 break;
264 }
265 }//end ProcessVectorVehicleParam
266
267 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
268 {
269 switch (pParam)
270 {
271 case Vehicle.REFERENCE_FRAME:
272 // m_referenceFrame = pValue;
273 break;
274 case Vehicle.ROLL_FRAME:
275 m_RollreferenceFrame = pValue;
276 break;
277 }
278 }//end ProcessRotationVehicleParam
279
280 internal void ProcessVehicleFlags(int pParam, bool remove)
281 {
282 if (remove)
283 {
284 if (pParam == -1)
285 {
286 m_flags = (VehicleFlag)0;
287 m_Hoverflags = (VehicleFlag)0;
288 return;
289 }
290 if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
291 {
292 if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0)
293 m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT);
294 }
295 if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
296 {
297 if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0)
298 m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY);
299 }
300 if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
301 {
302 if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0)
303 m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY);
304 }
305 if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
306 {
307 if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0)
308 m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY);
309 }
310 if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
311 {
312 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0)
313 m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP);
314 }
315 if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY)
316 {
317 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0)
318 m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
319 }
320 if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
321 {
322 if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0)
323 m_flags &= ~(VehicleFlag.MOUSELOOK_BANK);
324 }
325 if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
326 {
327 if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0)
328 m_flags &= ~(VehicleFlag.MOUSELOOK_STEER);
329 }
330 if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
331 {
332 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0)
333 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP);
334 }
335 if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
336 {
337 if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0)
338 m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED);
339 }
340 if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
341 {
342 if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0)
343 m_flags &= ~(VehicleFlag.NO_X);
344 }
345 if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
346 {
347 if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0)
348 m_flags &= ~(VehicleFlag.NO_Y);
349 }
350 if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
351 {
352 if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0)
353 m_flags &= ~(VehicleFlag.NO_Z);
354 }
355 if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
356 {
357 if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0)
358 m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT);
359 }
360 if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
361 {
362 if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0)
363 m_flags &= ~(VehicleFlag.NO_DEFLECTION);
364 }
365 if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
366 {
367 if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0)
368 m_flags &= ~(VehicleFlag.LOCK_ROTATION);
369 }
370 }
371 else
372 {
373 if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
374 {
375 m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags);
376 }
377 if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
378 {
379 m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags);
380 }
381 if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
382 {
383 m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags);
384 }
385 if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
386 {
387 m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags);
388 }
389 if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
390 {
391 m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags);
392 }
393 if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
394 {
395 m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags);
396 }
397 if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
398 {
399 m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags);
400 }
401 if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
402 {
403 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags);
404 }
405 if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
406 {
407 m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags);
408 }
409 if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
410 {
411 m_flags |= (VehicleFlag.NO_X);
412 }
413 if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
414 {
415 m_flags |= (VehicleFlag.NO_Y);
416 }
417 if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
418 {
419 m_flags |= (VehicleFlag.NO_Z);
420 }
421 if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
422 {
423 m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT);
424 }
425 if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
426 {
427 m_flags |= (VehicleFlag.NO_DEFLECTION);
428 }
429 if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
430 {
431 m_flags |= (VehicleFlag.LOCK_ROTATION);
432 }
433 }
434 }//end ProcessVehicleFlags
435
436 internal void ProcessTypeChange(Vehicle pType)
437 {
438 // Set Defaults For Type
439 m_type = pType;
440 switch (pType)
441 {
442 case Vehicle.TYPE_NONE:
443 m_linearFrictionTimescale = new Vector3(0, 0, 0);
444 m_angularFrictionTimescale = new Vector3(0, 0, 0);
445 m_linearMotorDirection = Vector3.Zero;
446 m_linearMotorTimescale = 0;
447 m_linearMotorDecayTimescale = 0;
448 m_angularMotorDirection = Vector3.Zero;
449 m_angularMotorTimescale = 0;
450 m_angularMotorDecayTimescale = 0;
451 m_VhoverHeight = 0;
452 m_VhoverTimescale = 0;
453 m_VehicleBuoyancy = 0;
454 m_flags = (VehicleFlag)0;
455 break;
456
457 case Vehicle.TYPE_SLED:
458 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
459 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
460 m_linearMotorDirection = Vector3.Zero;
461 m_linearMotorTimescale = 1000;
462 m_linearMotorDecayTimescale = 120;
463 m_angularMotorDirection = Vector3.Zero;
464 m_angularMotorTimescale = 1000;
465 m_angularMotorDecayTimescale = 120;
466 m_VhoverHeight = 0;
467// m_VhoverEfficiency = 1;
468 m_VhoverTimescale = 10;
469 m_VehicleBuoyancy = 0;
470 // m_linearDeflectionEfficiency = 1;
471 // m_linearDeflectionTimescale = 1;
472 // m_angularDeflectionEfficiency = 1;
473 // m_angularDeflectionTimescale = 1000;
474 // m_bankingEfficiency = 0;
475 // m_bankingMix = 1;
476 // m_bankingTimescale = 10;
477 // m_referenceFrame = Quaternion.Identity;
478 m_Hoverflags &=
479 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
480 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
481 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
482 break;
483 case Vehicle.TYPE_CAR:
484 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
485 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
486 m_linearMotorDirection = Vector3.Zero;
487 m_linearMotorTimescale = 1;
488 m_linearMotorDecayTimescale = 60;
489 m_angularMotorDirection = Vector3.Zero;
490 m_angularMotorTimescale = 1;
491 m_angularMotorDecayTimescale = 0.8f;
492 m_VhoverHeight = 0;
493// m_VhoverEfficiency = 0;
494 m_VhoverTimescale = 1000;
495 m_VehicleBuoyancy = 0;
496 // // m_linearDeflectionEfficiency = 1;
497 // // m_linearDeflectionTimescale = 2;
498 // // m_angularDeflectionEfficiency = 0;
499 // m_angularDeflectionTimescale = 10;
500 m_verticalAttractionEfficiency = 1f;
501 m_verticalAttractionTimescale = 10f;
502 // m_bankingEfficiency = -0.2f;
503 // m_bankingMix = 1;
504 // m_bankingTimescale = 1;
505 // m_referenceFrame = Quaternion.Identity;
506 m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
507 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
508 VehicleFlag.LIMIT_MOTOR_UP);
509 m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY);
510 break;
511 case Vehicle.TYPE_BOAT:
512 m_linearFrictionTimescale = new Vector3(10, 3, 2);
513 m_angularFrictionTimescale = new Vector3(10,10,10);
514 m_linearMotorDirection = Vector3.Zero;
515 m_linearMotorTimescale = 5;
516 m_linearMotorDecayTimescale = 60;
517 m_angularMotorDirection = Vector3.Zero;
518 m_angularMotorTimescale = 4;
519 m_angularMotorDecayTimescale = 4;
520 m_VhoverHeight = 0;
521// m_VhoverEfficiency = 0.5f;
522 m_VhoverTimescale = 2;
523 m_VehicleBuoyancy = 1;
524 // m_linearDeflectionEfficiency = 0.5f;
525 // m_linearDeflectionTimescale = 3;
526 // m_angularDeflectionEfficiency = 0.5f;
527 // m_angularDeflectionTimescale = 5;
528 m_verticalAttractionEfficiency = 0.5f;
529 m_verticalAttractionTimescale = 5f;
530 // m_bankingEfficiency = -0.3f;
531 // m_bankingMix = 0.8f;
532 // m_bankingTimescale = 1;
533 // m_referenceFrame = Quaternion.Identity;
534 m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
535 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
536 m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
537 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
538 VehicleFlag.LIMIT_MOTOR_UP);
539 m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY);
540 break;
541 case Vehicle.TYPE_AIRPLANE:
542 m_linearFrictionTimescale = new Vector3(200, 10, 5);
543 m_angularFrictionTimescale = new Vector3(20, 20, 20);
544 m_linearMotorDirection = Vector3.Zero;
545 m_linearMotorTimescale = 2;
546 m_linearMotorDecayTimescale = 60;
547 m_angularMotorDirection = Vector3.Zero;
548 m_angularMotorTimescale = 4;
549 m_angularMotorDecayTimescale = 4;
550 m_VhoverHeight = 0;
551// m_VhoverEfficiency = 0.5f;
552 m_VhoverTimescale = 1000;
553 m_VehicleBuoyancy = 0;
554 // m_linearDeflectionEfficiency = 0.5f;
555 // m_linearDeflectionTimescale = 3;
556 // m_angularDeflectionEfficiency = 1;
557 // m_angularDeflectionTimescale = 2;
558 m_verticalAttractionEfficiency = 0.9f;
559 m_verticalAttractionTimescale = 2f;
560 // m_bankingEfficiency = 1;
561 // m_bankingMix = 0.7f;
562 // m_bankingTimescale = 2;
563 // m_referenceFrame = Quaternion.Identity;
564 m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
565 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
566 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
567 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
568 break;
569 case Vehicle.TYPE_BALLOON:
570 m_linearFrictionTimescale = new Vector3(5, 5, 5);
571 m_angularFrictionTimescale = new Vector3(10, 10, 10);
572 m_linearMotorDirection = Vector3.Zero;
573 m_linearMotorTimescale = 5;
574 m_linearMotorDecayTimescale = 60;
575 m_angularMotorDirection = Vector3.Zero;
576 m_angularMotorTimescale = 6;
577 m_angularMotorDecayTimescale = 10;
578 m_VhoverHeight = 5;
579// m_VhoverEfficiency = 0.8f;
580 m_VhoverTimescale = 10;
581 m_VehicleBuoyancy = 1;
582 // m_linearDeflectionEfficiency = 0;
583 // m_linearDeflectionTimescale = 5;
584 // m_angularDeflectionEfficiency = 0;
585 // m_angularDeflectionTimescale = 5;
586 m_verticalAttractionEfficiency = 1f;
587 m_verticalAttractionTimescale = 100f;
588 // m_bankingEfficiency = 0;
589 // m_bankingMix = 0.7f;
590 // m_bankingTimescale = 5;
591 // m_referenceFrame = Quaternion.Identity;
592 m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
593 VehicleFlag.HOVER_UP_ONLY);
594 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
595 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
596 m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
597 break;
598
599 }
600 }//end SetDefaultsForType
601
602 internal void Enable(IntPtr pBody, OdeScene pParentScene)
603 {
604 if (m_type == Vehicle.TYPE_NONE)
605 return;
606
607 m_body = pBody;
608 }
609
610 internal void Step(float pTimestep, OdeScene pParentScene)
611 {
612 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
613 return;
614 frcount++; // used to limit debug comment output
615 if (frcount > 100)
616 frcount = 0;
617
618 MoveLinear(pTimestep, pParentScene);
619 MoveAngular(pTimestep);
620 LimitRotation(pTimestep);
621 }// end Step
622
623 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
624 {
625 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
626 {
627 if (!d.BodyIsEnabled(Body))
628 d.BodyEnable(Body);
629
630 // add drive to body
631 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
632 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
633
634 // This will work temporarily, but we really need to compare speed on an axis
635 // KF: Limit body velocity to applied velocity?
636 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
637 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
638 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
639 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
640 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
641 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
642
643 // decay applied velocity
644 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
645 //Console.WriteLine("decay: " + decayfraction);
646 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
647 //Console.WriteLine("actual: " + m_linearMotorDirection);
648 }
649 else
650 { // requested is not significant
651 // if what remains of applied is small, zero it.
652 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
653 m_lastLinearVelocityVector = Vector3.Zero;
654 }
655
656 // convert requested object velocity to world-referenced vector
657 m_dir = m_lastLinearVelocityVector;
658 d.Quaternion rot = d.BodyGetQuaternion(Body);
659 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
660 m_dir *= rotq; // apply obj rotation to velocity vector
661
662 // add Gravity andBuoyancy
663 // KF: So far I have found no good method to combine a script-requested
664 // .Z velocity and gravity. Therefore only 0g will used script-requested
665 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
666 Vector3 grav = Vector3.Zero;
667 // There is some gravity, make a gravity force vector
668 // that is applied after object velocity.
669 d.Mass objMass;
670 d.BodyGetMass(Body, out objMass);
671 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
672 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
673 // Preserve the current Z velocity
674 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
675 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
676
677 d.Vector3 pos = d.BodyGetPosition(Body);
678// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
679 Vector3 posChange = new Vector3();
680 posChange.X = pos.X - m_lastPositionVector.X;
681 posChange.Y = pos.Y - m_lastPositionVector.Y;
682 posChange.Z = pos.Z - m_lastPositionVector.Z;
683 double Zchange = Math.Abs(posChange.Z);
684 if (m_BlockingEndPoint != Vector3.Zero)
685 {
686 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
687 {
688 pos.X -= posChange.X + 1;
689 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
690 }
691 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
692 {
693 pos.Y -= posChange.Y + 1;
694 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
695 }
696 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
697 {
698 pos.Z -= posChange.Z + 1;
699 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
700 }
701 if (pos.X <= 0)
702 {
703 pos.X += posChange.X + 1;
704 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
705 }
706 if (pos.Y <= 0)
707 {
708 pos.Y += posChange.Y + 1;
709 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
710 }
711 }
712 if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
713 {
714 pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
715 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
716 }
717
718 // Check if hovering
719 if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
720 {
721 // We should hover, get the target height
722 if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
723 {
724 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
725 }
726 if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
727 {
728 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
729 }
730 if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
731 {
732 m_VhoverTargetHeight = m_VhoverHeight;
733 }
734
735 if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
736 {
737 // If body is aready heigher, use its height as target height
738 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
739 }
740 if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
741 {
742 if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
743 {
744 d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
745 }
746 }
747 else
748 {
749 float herr0 = pos.Z - m_VhoverTargetHeight;
750 // Replace Vertical speed with correction figure if significant
751 if (Math.Abs(herr0) > 0.01f)
752 {
753 m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
754 //KF: m_VhoverEfficiency is not yet implemented
755 }
756 else
757 {
758 m_dir.Z = 0f;
759 }
760 }
761
762// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
763// m_VhoverTimescale = 0f; // time to acheive height
764// pTimestep is time since last frame,in secs
765 }
766
767 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
768 {
769 //Start Experimental Values
770 if (Zchange > .3)
771 {
772 grav.Z = (float)(grav.Z * 3);
773 }
774 if (Zchange > .15)
775 {
776 grav.Z = (float)(grav.Z * 2);
777 }
778 if (Zchange > .75)
779 {
780 grav.Z = (float)(grav.Z * 1.5);
781 }
782 if (Zchange > .05)
783 {
784 grav.Z = (float)(grav.Z * 1.25);
785 }
786 if (Zchange > .025)
787 {
788 grav.Z = (float)(grav.Z * 1.125);
789 }
790 float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
791 float postemp = (pos.Z - terraintemp);
792 if (postemp > 2.5f)
793 {
794 grav.Z = (float)(grav.Z * 1.037125);
795 }
796 //End Experimental Values
797 }
798 if ((m_flags & (VehicleFlag.NO_X)) != 0)
799 {
800 m_dir.X = 0;
801 }
802 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
803 {
804 m_dir.Y = 0;
805 }
806 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
807 {
808 m_dir.Z = 0;
809 }
810
811 m_lastPositionVector = d.BodyGetPosition(Body);
812
813 // Apply velocity
814 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
815 // apply gravity force
816 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
817
818
819 // apply friction
820 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
821 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
822 } // end MoveLinear()
823
824 private void MoveAngular(float pTimestep)
825 {
826 /*
827 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
828 private int m_angularMotorApply = 0; // application frame counter
829 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
830 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
831 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
832 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
833 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
834 */
835
836 // Get what the body is doing, this includes 'external' influences
837 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
838 // Vector3 angularVelocity = Vector3.Zero;
839
840 if (m_angularMotorApply > 0)
841 {
842 // ramp up to new value
843 // current velocity += error / (time to get there / step interval)
844 // requested speed - last motor speed
845 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
846 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
847 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
848
849 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
850 // velocity may still be acheived.
851 }
852 else
853 {
854 // no motor recently applied, keep the body velocity
855 /* m_angularMotorVelocity.X = angularVelocity.X;
856 m_angularMotorVelocity.Y = angularVelocity.Y;
857 m_angularMotorVelocity.Z = angularVelocity.Z; */
858
859 // and decay the velocity
860 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
861 } // end motor section
862
863 // Vertical attractor section
864 Vector3 vertattr = Vector3.Zero;
865
866 if (m_verticalAttractionTimescale < 300)
867 {
868 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
869 // get present body rotation
870 d.Quaternion rot = d.BodyGetQuaternion(Body);
871 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
872 // make a vector pointing up
873 Vector3 verterr = Vector3.Zero;
874 verterr.Z = 1.0f;
875 // rotate it to Body Angle
876 verterr = verterr * rotq;
877 // 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.
878 // 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
879 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
880 if (verterr.Z < 0.0f)
881 {
882 verterr.X = 2.0f - verterr.X;
883 verterr.Y = 2.0f - verterr.Y;
884 }
885 // Error is 0 (no error) to +/- 2 (max error)
886 // scale it by VAservo
887 verterr = verterr * VAservo;
888//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
889
890 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
891 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
892 vertattr.X = verterr.Y;
893 vertattr.Y = - verterr.X;
894 vertattr.Z = 0f;
895
896 // scaling appears better usingsquare-law
897 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
898 vertattr.X += bounce * angularVelocity.X;
899 vertattr.Y += bounce * angularVelocity.Y;
900
901 } // else vertical attractor is off
902
903 // m_lastVertAttractor = vertattr;
904
905 // Bank section tba
906 // Deflection section tba
907
908 // Sum velocities
909 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
910
911 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
912 {
913 m_lastAngularVelocity.X = 0;
914 m_lastAngularVelocity.Y = 0;
915 }
916
917 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
918 {
919 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
920 }
921 else
922 {
923 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
924 }
925
926 // apply friction
927 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
928 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
929
930 // Apply to the body
931 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
932
933 } //end MoveAngular
934 internal void LimitRotation(float timestep)
935 {
936 d.Quaternion rot = d.BodyGetQuaternion(Body);
937 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
938 d.Quaternion m_rot = new d.Quaternion();
939 bool changed = false;
940 m_rot.X = rotq.X;
941 m_rot.Y = rotq.Y;
942 m_rot.Z = rotq.Z;
943 m_rot.W = rotq.W;
944 if (m_RollreferenceFrame != Quaternion.Identity)
945 {
946 if (rotq.X >= m_RollreferenceFrame.X)
947 {
948 m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
949 }
950 if (rotq.Y >= m_RollreferenceFrame.Y)
951 {
952 m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
953 }
954 if (rotq.X <= -m_RollreferenceFrame.X)
955 {
956 m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
957 }
958 if (rotq.Y <= -m_RollreferenceFrame.Y)
959 {
960 m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
961 }
962 changed = true;
963 }
964 if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
965 {
966 m_rot.X = 0;
967 m_rot.Y = 0;
968 changed = true;
969 }
970 if (changed)
971 d.BodySetQuaternion(Body, ref m_rot);
972 }
973 }
974}
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
new file mode 100644
index 0000000..f934b8a
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -0,0 +1,3387 @@
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 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
31 * ODEPrim.cs contains methods dealing with Prim editing, Prim
32 * characteristics and Kinetic motion.
33 * ODEDynamics.cs contains methods dealing with Prim Physical motion
34 * (dynamics) and the associated settings. Old Linear and angular
35 * motors for dynamic motion have been replace with MoveLinear()
36 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
37 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
38 * switch between 'VEHICLE' parameter use and general dynamics
39 * settings use.
40 */
41
42//#define SPAM
43
44using System;
45using System.Collections.Generic;
46using System.Reflection;
47using System.Runtime.InteropServices;
48using System.Threading;
49using log4net;
50using OpenMetaverse;
51using Ode.NET;
52using OpenSim.Framework;
53using OpenSim.Region.Physics.Manager;
54
55namespace OpenSim.Region.Physics.OdePlugin
56{
57 /// <summary>
58 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
59 /// </summary>
60 public class OdePrim : PhysicsActor
61 {
62 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
63
64 private bool m_isphysical;
65
66 public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
67 private int m_expectedCollisionContacts = 0;
68
69 /// <summary>
70 /// Gets collide bits so that we can still perform land collisions if a mesh fails to load.
71 /// </summary>
72 private int BadMeshAssetCollideBits
73 {
74 get { return m_isphysical ? (int)CollisionCategories.Land : 0; }
75 }
76
77 /// <summary>
78 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
79 /// </summary>
80 public override bool IsPhysical
81 {
82 get { return m_isphysical; }
83 set
84 {
85 m_isphysical = value;
86 if (!m_isphysical) // Zero the remembered last velocity
87 m_lastVelocity = Vector3.Zero;
88 }
89 }
90
91 private Vector3 _position;
92 private Vector3 _velocity;
93 private Vector3 _torque;
94 private Vector3 m_lastVelocity;
95 private Vector3 m_lastposition;
96 private Quaternion m_lastorientation = new Quaternion();
97 private Vector3 m_rotationalVelocity;
98 private Vector3 _size;
99 private Vector3 _acceleration;
100 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
101 private Quaternion _orientation;
102 private Vector3 m_taintposition;
103 private Vector3 m_taintsize;
104 private Vector3 m_taintVelocity;
105 private Vector3 m_taintTorque;
106 private Quaternion m_taintrot;
107 private Vector3 m_angularlock = Vector3.One;
108 private Vector3 m_taintAngularLock = Vector3.One;
109 private IntPtr Amotor = IntPtr.Zero;
110
111 private bool m_assetFailed = false;
112
113 private Vector3 m_PIDTarget;
114 private float m_PIDTau;
115 private float PID_D = 35f;
116 private float PID_G = 25f;
117
118 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
119 // and are for non-VEHICLES only.
120
121 private float m_PIDHoverHeight;
122 private float m_PIDHoverTau;
123 private bool m_useHoverPID;
124 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
125 private float m_targetHoverHeight;
126 private float m_groundHeight;
127 private float m_waterHeight;
128 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
129
130 // private float m_tensor = 5f;
131 private int body_autodisable_frames = 20;
132
133
134 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
135 | CollisionCategories.Space
136 | CollisionCategories.Body
137 | CollisionCategories.Character
138 );
139 private bool m_taintshape;
140 private bool m_taintPhysics;
141 private bool m_collidesLand = true;
142 private bool m_collidesWater;
143
144 // Default we're a Geometry
145 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
146
147 // Default, Collide with Other Geometries, spaces and Bodies
148 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
149
150 public bool m_taintremove { get; private set; }
151 public bool m_taintdisable { get; private set; }
152 internal bool m_disabled;
153 public bool m_taintadd { get; private set; }
154 public bool m_taintselected { get; private set; }
155 public bool m_taintCollidesWater { get; private set; }
156
157 private bool m_taintforce = false;
158 private bool m_taintaddangularforce = false;
159 private Vector3 m_force;
160 private List<Vector3> m_forcelist = new List<Vector3>();
161 private List<Vector3> m_angularforcelist = new List<Vector3>();
162
163 private PrimitiveBaseShape _pbs;
164 private OdeScene _parent_scene;
165
166 /// <summary>
167 /// The physics space which contains prim geometries
168 /// </summary>
169 public IntPtr m_targetSpace = IntPtr.Zero;
170
171 /// <summary>
172 /// The prim geometry, used for collision detection.
173 /// </summary>
174 /// <remarks>
175 /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
176 /// mesh change) or when the physical prim is being removed from the scene.
177 /// </remarks>
178 public IntPtr prim_geom { get; private set; }
179
180 public IntPtr _triMeshData { get; private set; }
181
182 private IntPtr _linkJointGroup = IntPtr.Zero;
183 private PhysicsActor _parent;
184 private PhysicsActor m_taintparent;
185
186 private List<OdePrim> childrenPrim = new List<OdePrim>();
187
188 private bool iscolliding;
189 private bool m_isSelected;
190
191 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
192
193 private bool m_throttleUpdates;
194 private int throttleCounter;
195 public int m_interpenetrationcount { get; private set; }
196 internal float m_collisionscore;
197 public int m_roundsUnderMotionThreshold { get; private set; }
198 private int m_crossingfailures;
199
200 public bool outofBounds { get; private set; }
201 private float m_density = 10.000006836f; // Aluminum g/cm3;
202
203 public bool _zeroFlag { get; private set; }
204 private bool m_lastUpdateSent;
205
206 public IntPtr Body = IntPtr.Zero;
207 private Vector3 _target_velocity;
208 private d.Mass pMass;
209
210 private int m_eventsubscription;
211 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
212
213 /// <summary>
214 /// Signal whether there were collisions on the previous frame, so we know if we need to send the
215 /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
216 /// </summary>
217 /// <remarks>
218 /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
219 /// </remarks>
220 private bool m_collisionsOnPreviousFrame;
221
222 private IntPtr m_linkJoint = IntPtr.Zero;
223
224 internal volatile bool childPrim;
225
226 private ODEDynamics m_vehicle;
227
228 internal int m_material = (int)Material.Wood;
229
230 public OdePrim(
231 String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
232 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
233 {
234 Name = primName;
235 m_vehicle = new ODEDynamics();
236 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
237
238 if (!pos.IsFinite())
239 {
240 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
241 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
242 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
243 }
244 _position = pos;
245 m_taintposition = pos;
246 PID_D = parent_scene.bodyPIDD;
247 PID_G = parent_scene.bodyPIDG;
248 m_density = parent_scene.geomDefaultDensity;
249 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
250 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
251
252 prim_geom = IntPtr.Zero;
253
254 if (!pos.IsFinite())
255 {
256 size = new Vector3(0.5f, 0.5f, 0.5f);
257 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
258 }
259
260 if (size.X <= 0) size.X = 0.01f;
261 if (size.Y <= 0) size.Y = 0.01f;
262 if (size.Z <= 0) size.Z = 0.01f;
263
264 _size = size;
265 m_taintsize = _size;
266
267 if (!QuaternionIsFinite(rotation))
268 {
269 rotation = Quaternion.Identity;
270 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
271 }
272
273 _orientation = rotation;
274 m_taintrot = _orientation;
275 _pbs = pbs;
276
277 _parent_scene = parent_scene;
278 m_targetSpace = (IntPtr)0;
279
280 if (pos.Z < 0)
281 {
282 IsPhysical = false;
283 }
284 else
285 {
286 IsPhysical = pisPhysical;
287 // If we're physical, we need to be in the master space for now.
288 // linksets *should* be in a space together.. but are not currently
289 if (IsPhysical)
290 m_targetSpace = _parent_scene.space;
291 }
292
293 m_taintadd = true;
294 m_assetFailed = false;
295 _parent_scene.AddPhysicsActorTaint(this);
296 }
297
298 public override int PhysicsActorType
299 {
300 get { return (int) ActorTypes.Prim; }
301 set { return; }
302 }
303
304 public override bool SetAlwaysRun
305 {
306 get { return false; }
307 set { return; }
308 }
309
310 public override bool Grabbed
311 {
312 set { return; }
313 }
314
315 public override bool Selected
316 {
317 set
318 {
319 // This only makes the object not collidable if the object
320 // is physical or the object is modified somehow *IN THE FUTURE*
321 // without this, if an avatar selects prim, they can walk right
322 // through it while it's selected
323 m_collisionscore = 0;
324
325 if ((IsPhysical && !_zeroFlag) || !value)
326 {
327 m_taintselected = value;
328 _parent_scene.AddPhysicsActorTaint(this);
329 }
330 else
331 {
332 m_taintselected = value;
333 m_isSelected = value;
334 }
335
336 if (m_isSelected)
337 disableBodySoft();
338 }
339 }
340
341 /// <summary>
342 /// Set a new geometry for this prim.
343 /// </summary>
344 /// <param name="geom"></param>
345 private void SetGeom(IntPtr geom)
346 {
347 prim_geom = geom;
348//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
349
350 if (m_assetFailed)
351 {
352 d.GeomSetCategoryBits(prim_geom, 0);
353 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
354 }
355 else
356 {
357 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
358 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
359 }
360
361 _parent_scene.geom_name_map[prim_geom] = Name;
362 _parent_scene.actor_name_map[prim_geom] = this;
363
364 if (childPrim)
365 {
366 if (_parent != null && _parent is OdePrim)
367 {
368 OdePrim parent = (OdePrim)_parent;
369//Console.WriteLine("SetGeom calls ChildSetGeom");
370 parent.ChildSetGeom(this);
371 }
372 }
373 //m_log.Warn("Setting Geom to: " + prim_geom);
374 }
375
376 private void enableBodySoft()
377 {
378 if (!childPrim)
379 {
380 if (IsPhysical && Body != IntPtr.Zero)
381 {
382 d.BodyEnable(Body);
383 if (m_vehicle.Type != Vehicle.TYPE_NONE)
384 m_vehicle.Enable(Body, _parent_scene);
385 }
386
387 m_disabled = false;
388 }
389 }
390
391 private void disableBodySoft()
392 {
393 m_disabled = true;
394
395 if (IsPhysical && Body != IntPtr.Zero)
396 {
397 d.BodyDisable(Body);
398 }
399 }
400
401 /// <summary>
402 /// Make a prim subject to physics.
403 /// </summary>
404 private void enableBody()
405 {
406 // Don't enable this body if we're a child prim
407 // this should be taken care of in the parent function not here
408 if (!childPrim)
409 {
410 // Sets the geom to a body
411 Body = d.BodyCreate(_parent_scene.world);
412
413 setMass();
414 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
415 d.Quaternion myrot = new d.Quaternion();
416 myrot.X = _orientation.X;
417 myrot.Y = _orientation.Y;
418 myrot.Z = _orientation.Z;
419 myrot.W = _orientation.W;
420 d.BodySetQuaternion(Body, ref myrot);
421 d.GeomSetBody(prim_geom, Body);
422
423 if (m_assetFailed)
424 {
425 d.GeomSetCategoryBits(prim_geom, 0);
426 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
427 }
428 else
429 {
430 m_collisionCategories |= CollisionCategories.Body;
431 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
432 }
433
434 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
435 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
436
437 d.BodySetAutoDisableFlag(Body, true);
438 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
439
440 // disconnect from world gravity so we can apply buoyancy
441 d.BodySetGravityMode (Body, false);
442
443 m_interpenetrationcount = 0;
444 m_collisionscore = 0;
445 m_disabled = false;
446
447 // The body doesn't already have a finite rotation mode set here
448 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
449 {
450 createAMotor(m_angularlock);
451 }
452 if (m_vehicle.Type != Vehicle.TYPE_NONE)
453 {
454 m_vehicle.Enable(Body, _parent_scene);
455 }
456
457 _parent_scene.ActivatePrim(this);
458 }
459 }
460
461 #region Mass Calculation
462
463 private float CalculateMass()
464 {
465 float volume = _size.X * _size.Y * _size.Z; // default
466 float tmp;
467
468 float returnMass = 0;
469 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
470 float hollowVolume = hollowAmount * hollowAmount;
471
472 switch (_pbs.ProfileShape)
473 {
474 case ProfileShape.Square:
475 // default box
476
477 if (_pbs.PathCurve == (byte)Extrusion.Straight)
478 {
479 if (hollowAmount > 0.0)
480 {
481 switch (_pbs.HollowShape)
482 {
483 case HollowShape.Square:
484 case HollowShape.Same:
485 break;
486
487 case HollowShape.Circle:
488
489 hollowVolume *= 0.78539816339f;
490 break;
491
492 case HollowShape.Triangle:
493
494 hollowVolume *= (0.5f * .5f);
495 break;
496
497 default:
498 hollowVolume = 0;
499 break;
500 }
501 volume *= (1.0f - hollowVolume);
502 }
503 }
504
505 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
506 {
507 //a tube
508
509 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
510 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
511 volume -= volume*tmp*tmp;
512
513 if (hollowAmount > 0.0)
514 {
515 hollowVolume *= hollowAmount;
516
517 switch (_pbs.HollowShape)
518 {
519 case HollowShape.Square:
520 case HollowShape.Same:
521 break;
522
523 case HollowShape.Circle:
524 hollowVolume *= 0.78539816339f;;
525 break;
526
527 case HollowShape.Triangle:
528 hollowVolume *= 0.5f * 0.5f;
529 break;
530 default:
531 hollowVolume = 0;
532 break;
533 }
534 volume *= (1.0f - hollowVolume);
535 }
536 }
537
538 break;
539
540 case ProfileShape.Circle:
541
542 if (_pbs.PathCurve == (byte)Extrusion.Straight)
543 {
544 volume *= 0.78539816339f; // elipse base
545
546 if (hollowAmount > 0.0)
547 {
548 switch (_pbs.HollowShape)
549 {
550 case HollowShape.Same:
551 case HollowShape.Circle:
552 break;
553
554 case HollowShape.Square:
555 hollowVolume *= 0.5f * 2.5984480504799f;
556 break;
557
558 case HollowShape.Triangle:
559 hollowVolume *= .5f * 1.27323954473516f;
560 break;
561
562 default:
563 hollowVolume = 0;
564 break;
565 }
566 volume *= (1.0f - hollowVolume);
567 }
568 }
569
570 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
571 {
572 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
573 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
574 volume *= (1.0f - tmp * tmp);
575
576 if (hollowAmount > 0.0)
577 {
578
579 // calculate the hollow volume by it's shape compared to the prim shape
580 hollowVolume *= hollowAmount;
581
582 switch (_pbs.HollowShape)
583 {
584 case HollowShape.Same:
585 case HollowShape.Circle:
586 break;
587
588 case HollowShape.Square:
589 hollowVolume *= 0.5f * 2.5984480504799f;
590 break;
591
592 case HollowShape.Triangle:
593 hollowVolume *= .5f * 1.27323954473516f;
594 break;
595
596 default:
597 hollowVolume = 0;
598 break;
599 }
600 volume *= (1.0f - hollowVolume);
601 }
602 }
603 break;
604
605 case ProfileShape.HalfCircle:
606 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
607 {
608 volume *= 0.52359877559829887307710723054658f;
609 }
610 break;
611
612 case ProfileShape.EquilateralTriangle:
613
614 if (_pbs.PathCurve == (byte)Extrusion.Straight)
615 {
616 volume *= 0.32475953f;
617
618 if (hollowAmount > 0.0)
619 {
620
621 // calculate the hollow volume by it's shape compared to the prim shape
622 switch (_pbs.HollowShape)
623 {
624 case HollowShape.Same:
625 case HollowShape.Triangle:
626 hollowVolume *= .25f;
627 break;
628
629 case HollowShape.Square:
630 hollowVolume *= 0.499849f * 3.07920140172638f;
631 break;
632
633 case HollowShape.Circle:
634 // Hollow shape is a perfect cyllinder in respect to the cube's scale
635 // Cyllinder hollow volume calculation
636
637 hollowVolume *= 0.1963495f * 3.07920140172638f;
638 break;
639
640 default:
641 hollowVolume = 0;
642 break;
643 }
644 volume *= (1.0f - hollowVolume);
645 }
646 }
647 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
648 {
649 volume *= 0.32475953f;
650 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
651 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
652 volume *= (1.0f - tmp * tmp);
653
654 if (hollowAmount > 0.0)
655 {
656
657 hollowVolume *= hollowAmount;
658
659 switch (_pbs.HollowShape)
660 {
661 case HollowShape.Same:
662 case HollowShape.Triangle:
663 hollowVolume *= .25f;
664 break;
665
666 case HollowShape.Square:
667 hollowVolume *= 0.499849f * 3.07920140172638f;
668 break;
669
670 case HollowShape.Circle:
671
672 hollowVolume *= 0.1963495f * 3.07920140172638f;
673 break;
674
675 default:
676 hollowVolume = 0;
677 break;
678 }
679 volume *= (1.0f - hollowVolume);
680 }
681 }
682 break;
683
684 default:
685 break;
686 }
687
688 float taperX1;
689 float taperY1;
690 float taperX;
691 float taperY;
692 float pathBegin;
693 float pathEnd;
694 float profileBegin;
695 float profileEnd;
696
697 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
698 {
699 taperX1 = _pbs.PathScaleX * 0.01f;
700 if (taperX1 > 1.0f)
701 taperX1 = 2.0f - taperX1;
702 taperX = 1.0f - taperX1;
703
704 taperY1 = _pbs.PathScaleY * 0.01f;
705 if (taperY1 > 1.0f)
706 taperY1 = 2.0f - taperY1;
707 taperY = 1.0f - taperY1;
708 }
709 else
710 {
711 taperX = _pbs.PathTaperX * 0.01f;
712 if (taperX < 0.0f)
713 taperX = -taperX;
714 taperX1 = 1.0f - taperX;
715
716 taperY = _pbs.PathTaperY * 0.01f;
717 if (taperY < 0.0f)
718 taperY = -taperY;
719 taperY1 = 1.0f - taperY;
720 }
721
722 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
723
724 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
725 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
726 volume *= (pathEnd - pathBegin);
727
728// this is crude aproximation
729 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
730 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
731 volume *= (profileEnd - profileBegin);
732
733 returnMass = m_density * volume;
734
735 if (returnMass <= 0)
736 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
737// else if (returnMass > _parent_scene.maximumMassObject)
738// returnMass = _parent_scene.maximumMassObject;
739
740 // Recursively calculate mass
741 bool HasChildPrim = false;
742 lock (childrenPrim)
743 {
744 if (childrenPrim.Count > 0)
745 {
746 HasChildPrim = true;
747 }
748 }
749
750 if (HasChildPrim)
751 {
752 OdePrim[] childPrimArr = new OdePrim[0];
753
754 lock (childrenPrim)
755 childPrimArr = childrenPrim.ToArray();
756
757 for (int i = 0; i < childPrimArr.Length; i++)
758 {
759 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
760 returnMass += childPrimArr[i].CalculateMass();
761 // failsafe, this shouldn't happen but with OpenSim, you never know :)
762 if (i > 256)
763 break;
764 }
765 }
766
767 if (returnMass > _parent_scene.maximumMassObject)
768 returnMass = _parent_scene.maximumMassObject;
769
770 return returnMass;
771 }
772
773 #endregion
774
775 private void setMass()
776 {
777 if (Body != (IntPtr) 0)
778 {
779 float newmass = CalculateMass();
780
781 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
782
783 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
784 d.BodySetMass(Body, ref pMass);
785 }
786 }
787
788 private void setAngularVelocity(float x, float y, float z)
789 {
790 if (Body != (IntPtr)0)
791 {
792 d.BodySetAngularVel(Body, x, y, z);
793 }
794 }
795
796 /// <summary>
797 /// Stop a prim from being subject to physics.
798 /// </summary>
799 internal void disableBody()
800 {
801 //this kills the body so things like 'mesh' can re-create it.
802 lock (this)
803 {
804 if (!childPrim)
805 {
806 if (Body != IntPtr.Zero)
807 {
808 _parent_scene.DeactivatePrim(this);
809 m_collisionCategories &= ~CollisionCategories.Body;
810 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
811
812 if (m_assetFailed)
813 {
814 d.GeomSetCategoryBits(prim_geom, 0);
815 d.GeomSetCollideBits(prim_geom, 0);
816 }
817 else
818 {
819 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
820 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
821 }
822
823 d.BodyDestroy(Body);
824 lock (childrenPrim)
825 {
826 if (childrenPrim.Count > 0)
827 {
828 foreach (OdePrim prm in childrenPrim)
829 {
830 _parent_scene.DeactivatePrim(prm);
831 prm.Body = IntPtr.Zero;
832 }
833 }
834 }
835 Body = IntPtr.Zero;
836 }
837 }
838 else
839 {
840 _parent_scene.DeactivatePrim(this);
841
842 m_collisionCategories &= ~CollisionCategories.Body;
843 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
844
845 if (m_assetFailed)
846 {
847 d.GeomSetCategoryBits(prim_geom, 0);
848 d.GeomSetCollideBits(prim_geom, 0);
849 }
850 else
851 {
852
853 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
854 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
855 }
856
857 Body = IntPtr.Zero;
858 }
859 }
860
861 m_disabled = true;
862 m_collisionscore = 0;
863 }
864
865 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
866
867 private void setMesh(OdeScene parent_scene, IMesh mesh)
868 {
869// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
870
871 // This sleeper is there to moderate how long it takes between
872 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
873
874 //Thread.Sleep(10);
875
876 //Kill Body so that mesh can re-make the geom
877 if (IsPhysical && Body != IntPtr.Zero)
878 {
879 if (childPrim)
880 {
881 if (_parent != null)
882 {
883 OdePrim parent = (OdePrim)_parent;
884 parent.ChildDelink(this);
885 }
886 }
887 else
888 {
889 disableBody();
890 }
891 }
892
893 IntPtr vertices, indices;
894 int vertexCount, indexCount;
895 int vertexStride, triStride;
896 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
897 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
898 m_expectedCollisionContacts = indexCount;
899 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
900
901 // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
902 // the same time.
903 lock (m_MeshToTriMeshMap)
904 {
905 if (m_MeshToTriMeshMap.ContainsKey(mesh))
906 {
907 _triMeshData = m_MeshToTriMeshMap[mesh];
908 }
909 else
910 {
911 _triMeshData = d.GeomTriMeshDataCreate();
912
913 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
914 d.GeomTriMeshDataPreprocess(_triMeshData);
915 m_MeshToTriMeshMap[mesh] = _triMeshData;
916 }
917 }
918
919// _parent_scene.waitForSpaceUnlock(m_targetSpace);
920 try
921 {
922 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
923 }
924 catch (AccessViolationException)
925 {
926 m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
927 return;
928 }
929
930 // if (IsPhysical && Body == (IntPtr) 0)
931 // {
932 // Recreate the body
933 // m_interpenetrationcount = 0;
934 // m_collisionscore = 0;
935
936 // enableBody();
937 // }
938 }
939
940 internal void ProcessTaints()
941 {
942#if SPAM
943Console.WriteLine("ZProcessTaints for " + Name);
944#endif
945
946 // This must be processed as the very first taint so that later operations have a prim_geom to work with
947 // if this is a new prim.
948 if (m_taintadd)
949 changeadd();
950
951 if (!_position.ApproxEquals(m_taintposition, 0f))
952 changemove();
953
954 if (m_taintrot != _orientation)
955 {
956 if (childPrim && IsPhysical) // For physical child prim...
957 {
958 rotate();
959 // KF: ODE will also rotate the parent prim!
960 // so rotate the root back to where it was
961 OdePrim parent = (OdePrim)_parent;
962 parent.rotate();
963 }
964 else
965 {
966 //Just rotate the prim
967 rotate();
968 }
969 }
970
971 if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
972 changePhysicsStatus();
973
974 if (!_size.ApproxEquals(m_taintsize, 0f))
975 changesize();
976
977 if (m_taintshape)
978 changeshape();
979
980 if (m_taintforce)
981 changeAddForce();
982
983 if (m_taintaddangularforce)
984 changeAddAngularForce();
985
986 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
987 changeSetTorque();
988
989 if (m_taintdisable)
990 changedisable();
991
992 if (m_taintselected != m_isSelected)
993 changeSelectedStatus();
994
995 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
996 changevelocity();
997
998 if (m_taintparent != _parent)
999 changelink();
1000
1001 if (m_taintCollidesWater != m_collidesWater)
1002 changefloatonwater();
1003
1004 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
1005 changeAngularLock();
1006 }
1007
1008 /// <summary>
1009 /// Change prim in response to an angular lock taint.
1010 /// </summary>
1011 private void changeAngularLock()
1012 {
1013 // do we have a Physical object?
1014 if (Body != IntPtr.Zero)
1015 {
1016 //Check that we have a Parent
1017 //If we have a parent then we're not authorative here
1018 if (_parent == null)
1019 {
1020 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
1021 {
1022 //d.BodySetFiniteRotationMode(Body, 0);
1023 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
1024 createAMotor(m_taintAngularLock);
1025 }
1026 else
1027 {
1028 if (Amotor != IntPtr.Zero)
1029 {
1030 d.JointDestroy(Amotor);
1031 Amotor = IntPtr.Zero;
1032 }
1033 }
1034 }
1035 }
1036
1037 // Store this for later in case we get turned into a separate body
1038 m_angularlock = m_taintAngularLock;
1039 }
1040
1041 /// <summary>
1042 /// Change prim in response to a link taint.
1043 /// </summary>
1044 private void changelink()
1045 {
1046 // If the newly set parent is not null
1047 // create link
1048 if (_parent == null && m_taintparent != null)
1049 {
1050 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1051 {
1052 OdePrim obj = (OdePrim)m_taintparent;
1053 //obj.disableBody();
1054//Console.WriteLine("changelink calls ParentPrim");
1055 obj.AddChildPrim(this);
1056
1057 /*
1058 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1059 {
1060 _linkJointGroup = d.JointGroupCreate(0);
1061 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1062 d.JointAttach(m_linkJoint, obj.Body, Body);
1063 d.JointSetFixed(m_linkJoint);
1064 }
1065 */
1066 }
1067 }
1068 // If the newly set parent is null
1069 // destroy link
1070 else if (_parent != null && m_taintparent == null)
1071 {
1072//Console.WriteLine(" changelink B");
1073
1074 if (_parent is OdePrim)
1075 {
1076 OdePrim obj = (OdePrim)_parent;
1077 obj.ChildDelink(this);
1078 childPrim = false;
1079 //_parent = null;
1080 }
1081
1082 /*
1083 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1084 d.JointGroupDestroy(_linkJointGroup);
1085
1086 _linkJointGroup = (IntPtr)0;
1087 m_linkJoint = (IntPtr)0;
1088 */
1089 }
1090
1091 _parent = m_taintparent;
1092 m_taintPhysics = IsPhysical;
1093 }
1094
1095 /// <summary>
1096 /// Add a child prim to this parent prim.
1097 /// </summary>
1098 /// <param name="prim">Child prim</param>
1099 private void AddChildPrim(OdePrim prim)
1100 {
1101 if (LocalID == prim.LocalID)
1102 return;
1103
1104 if (Body == IntPtr.Zero)
1105 {
1106 Body = d.BodyCreate(_parent_scene.world);
1107 setMass();
1108 }
1109
1110 lock (childrenPrim)
1111 {
1112 if (childrenPrim.Contains(prim))
1113 return;
1114
1115// m_log.DebugFormat(
1116// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
1117
1118 childrenPrim.Add(prim);
1119
1120 foreach (OdePrim prm in childrenPrim)
1121 {
1122 d.Mass m2;
1123 d.MassSetZero(out m2);
1124 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1125
1126 d.Quaternion quat = new d.Quaternion();
1127 quat.W = prm._orientation.W;
1128 quat.X = prm._orientation.X;
1129 quat.Y = prm._orientation.Y;
1130 quat.Z = prm._orientation.Z;
1131
1132 d.Matrix3 mat = new d.Matrix3();
1133 d.RfromQ(out mat, ref quat);
1134 d.MassRotate(ref m2, ref mat);
1135 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1136 d.MassAdd(ref pMass, ref m2);
1137 }
1138
1139 foreach (OdePrim prm in childrenPrim)
1140 {
1141 prm.m_collisionCategories |= CollisionCategories.Body;
1142 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1143
1144//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
1145 if (prm.m_assetFailed)
1146 {
1147 d.GeomSetCategoryBits(prm.prim_geom, 0);
1148 d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
1149 }
1150 else
1151 {
1152 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1153 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1154 }
1155
1156 d.Quaternion quat = new d.Quaternion();
1157 quat.W = prm._orientation.W;
1158 quat.X = prm._orientation.X;
1159 quat.Y = prm._orientation.Y;
1160 quat.Z = prm._orientation.Z;
1161
1162 d.Matrix3 mat = new d.Matrix3();
1163 d.RfromQ(out mat, ref quat);
1164 if (Body != IntPtr.Zero)
1165 {
1166 d.GeomSetBody(prm.prim_geom, Body);
1167 prm.childPrim = true;
1168 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1169 //d.GeomSetOffsetPosition(prim.prim_geom,
1170 // (Position.X - prm.Position.X) - pMass.c.X,
1171 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1172 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1173 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1174 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1175 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1176 d.BodySetMass(Body, ref pMass);
1177 }
1178 else
1179 {
1180 m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
1181 }
1182
1183 prm.m_interpenetrationcount = 0;
1184 prm.m_collisionscore = 0;
1185 prm.m_disabled = false;
1186
1187 // The body doesn't already have a finite rotation mode set here
1188 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1189 {
1190 prm.createAMotor(m_angularlock);
1191 }
1192 prm.Body = Body;
1193 _parent_scene.ActivatePrim(prm);
1194 }
1195
1196 m_collisionCategories |= CollisionCategories.Body;
1197 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1198
1199 if (m_assetFailed)
1200 {
1201 d.GeomSetCategoryBits(prim_geom, 0);
1202 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
1203 }
1204 else
1205 {
1206 //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
1207 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1208 //Console.WriteLine(" Post GeomSetCategoryBits 2");
1209 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1210 }
1211
1212 d.Quaternion quat2 = new d.Quaternion();
1213 quat2.W = _orientation.W;
1214 quat2.X = _orientation.X;
1215 quat2.Y = _orientation.Y;
1216 quat2.Z = _orientation.Z;
1217
1218 d.Matrix3 mat2 = new d.Matrix3();
1219 d.RfromQ(out mat2, ref quat2);
1220 d.GeomSetBody(prim_geom, Body);
1221 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1222 //d.GeomSetOffsetPosition(prim.prim_geom,
1223 // (Position.X - prm.Position.X) - pMass.c.X,
1224 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1225 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1226 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1227 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1228 d.BodySetMass(Body, ref pMass);
1229
1230 d.BodySetAutoDisableFlag(Body, true);
1231 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1232
1233 m_interpenetrationcount = 0;
1234 m_collisionscore = 0;
1235 m_disabled = false;
1236
1237 // The body doesn't already have a finite rotation mode set here
1238 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1239 {
1240 createAMotor(m_angularlock);
1241 }
1242
1243 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1244
1245 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1246 m_vehicle.Enable(Body, _parent_scene);
1247
1248 _parent_scene.ActivatePrim(this);
1249 }
1250 }
1251
1252 private void ChildSetGeom(OdePrim odePrim)
1253 {
1254// m_log.DebugFormat(
1255// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1256
1257 //if (IsPhysical && Body != IntPtr.Zero)
1258 lock (childrenPrim)
1259 {
1260 foreach (OdePrim prm in childrenPrim)
1261 {
1262 //prm.childPrim = true;
1263 prm.disableBody();
1264 //prm.m_taintparent = null;
1265 //prm._parent = null;
1266 //prm.m_taintPhysics = false;
1267 //prm.m_disabled = true;
1268 //prm.childPrim = false;
1269 }
1270 }
1271
1272 disableBody();
1273
1274 // Spurious - Body == IntPtr.Zero after disableBody()
1275// if (Body != IntPtr.Zero)
1276// {
1277// _parent_scene.DeactivatePrim(this);
1278// }
1279
1280 lock (childrenPrim)
1281 {
1282 foreach (OdePrim prm in childrenPrim)
1283 {
1284//Console.WriteLine("ChildSetGeom calls ParentPrim");
1285 AddChildPrim(prm);
1286 }
1287 }
1288 }
1289
1290 private void ChildDelink(OdePrim odePrim)
1291 {
1292// m_log.DebugFormat(
1293// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1294
1295 // Okay, we have a delinked child.. need to rebuild the body.
1296 lock (childrenPrim)
1297 {
1298 foreach (OdePrim prm in childrenPrim)
1299 {
1300 prm.childPrim = true;
1301 prm.disableBody();
1302 //prm.m_taintparent = null;
1303 //prm._parent = null;
1304 //prm.m_taintPhysics = false;
1305 //prm.m_disabled = true;
1306 //prm.childPrim = false;
1307 }
1308 }
1309
1310 disableBody();
1311
1312 lock (childrenPrim)
1313 {
1314 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1315 childrenPrim.Remove(odePrim);
1316 }
1317
1318 // Spurious - Body == IntPtr.Zero after disableBody()
1319// if (Body != IntPtr.Zero)
1320// {
1321// _parent_scene.DeactivatePrim(this);
1322// }
1323
1324 lock (childrenPrim)
1325 {
1326 foreach (OdePrim prm in childrenPrim)
1327 {
1328//Console.WriteLine("ChildDelink calls ParentPrim");
1329 AddChildPrim(prm);
1330 }
1331 }
1332 }
1333
1334 /// <summary>
1335 /// Change prim in response to a selection taint.
1336 /// </summary>
1337 private void changeSelectedStatus()
1338 {
1339 if (m_taintselected)
1340 {
1341 m_collisionCategories = CollisionCategories.Selected;
1342 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1343
1344 // We do the body disable soft twice because 'in theory' a collision could have happened
1345 // in between the disabling and the collision properties setting
1346 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1347 // through the ground.
1348
1349 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1350 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1351 // so that causes the selected part to wake up and continue moving.
1352
1353 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1354 // assembly will stop simulating during the selection, because of the lack of atomicity
1355 // of select operations (their processing could be interrupted by a thread switch, causing
1356 // simulation to continue before all of the selected object notifications trickle down to
1357 // the physics engine).
1358
1359 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1360 // selected and disabled. then, due to a thread switch, the selection processing is
1361 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1362 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1363 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1364 // up, start simulating again, which in turn wakes up the last 50.
1365
1366 if (IsPhysical)
1367 {
1368 disableBodySoft();
1369 }
1370
1371 if (m_assetFailed)
1372 {
1373 d.GeomSetCategoryBits(prim_geom, 0);
1374 d.GeomSetCollideBits(prim_geom, 0);
1375 }
1376 else
1377 {
1378 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1379 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1380 }
1381
1382 if (IsPhysical)
1383 {
1384 disableBodySoft();
1385 }
1386 }
1387 else
1388 {
1389 m_collisionCategories = CollisionCategories.Geom;
1390
1391 if (IsPhysical)
1392 m_collisionCategories |= CollisionCategories.Body;
1393
1394 m_collisionFlags = m_default_collisionFlags;
1395
1396 if (m_collidesLand)
1397 m_collisionFlags |= CollisionCategories.Land;
1398 if (m_collidesWater)
1399 m_collisionFlags |= CollisionCategories.Water;
1400
1401 if (m_assetFailed)
1402 {
1403 d.GeomSetCategoryBits(prim_geom, 0);
1404 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
1405 }
1406 else
1407 {
1408 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1409 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1410 }
1411
1412 if (IsPhysical)
1413 {
1414 if (Body != IntPtr.Zero)
1415 {
1416 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1417 d.BodySetForce(Body, 0, 0, 0);
1418 enableBodySoft();
1419 }
1420 }
1421 }
1422
1423 resetCollisionAccounting();
1424 m_isSelected = m_taintselected;
1425 }//end changeSelectedStatus
1426
1427 internal void ResetTaints()
1428 {
1429 m_taintposition = _position;
1430 m_taintrot = _orientation;
1431 m_taintPhysics = IsPhysical;
1432 m_taintselected = m_isSelected;
1433 m_taintsize = _size;
1434 m_taintshape = false;
1435 m_taintforce = false;
1436 m_taintdisable = false;
1437 m_taintVelocity = Vector3.Zero;
1438 }
1439
1440 /// <summary>
1441 /// Create a geometry for the given mesh in the given target space.
1442 /// </summary>
1443 /// <param name="m_targetSpace"></param>
1444 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1445 private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
1446 {
1447#if SPAM
1448Console.WriteLine("CreateGeom:");
1449#endif
1450 if (mesh != null)
1451 {
1452 setMesh(_parent_scene, mesh);
1453 }
1454 else
1455 {
1456 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1457 {
1458 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1459 {
1460 if (((_size.X / 2f) > 0f))
1461 {
1462// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1463 try
1464 {
1465//Console.WriteLine(" CreateGeom 1");
1466 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1467 m_expectedCollisionContacts = 3;
1468 }
1469 catch (AccessViolationException)
1470 {
1471 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1472 return;
1473 }
1474 }
1475 else
1476 {
1477// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1478 try
1479 {
1480//Console.WriteLine(" CreateGeom 2");
1481 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1482 m_expectedCollisionContacts = 4;
1483 }
1484 catch (AccessViolationException)
1485 {
1486 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1487 return;
1488 }
1489 }
1490 }
1491 else
1492 {
1493// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1494 try
1495 {
1496//Console.WriteLine(" CreateGeom 3");
1497 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1498 m_expectedCollisionContacts = 4;
1499 }
1500 catch (AccessViolationException)
1501 {
1502 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1503 return;
1504 }
1505 }
1506 }
1507 else
1508 {
1509// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1510 try
1511 {
1512//Console.WriteLine(" CreateGeom 4");
1513 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1514 m_expectedCollisionContacts = 4;
1515 }
1516 catch (AccessViolationException)
1517 {
1518 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1519 return;
1520 }
1521 }
1522 }
1523 }
1524
1525 /// <summary>
1526 /// Remove the existing geom from this prim.
1527 /// </summary>
1528 /// <param name="m_targetSpace"></param>
1529 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1530 /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
1531 internal bool RemoveGeom()
1532 {
1533 if (prim_geom != IntPtr.Zero)
1534 {
1535 try
1536 {
1537 _parent_scene.geom_name_map.Remove(prim_geom);
1538 _parent_scene.actor_name_map.Remove(prim_geom);
1539 d.GeomDestroy(prim_geom);
1540 m_expectedCollisionContacts = 0;
1541 prim_geom = IntPtr.Zero;
1542 }
1543 catch (System.AccessViolationException)
1544 {
1545 prim_geom = IntPtr.Zero;
1546 m_expectedCollisionContacts = 0;
1547 m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
1548
1549 return false;
1550 }
1551
1552 return true;
1553 }
1554 else
1555 {
1556 m_log.WarnFormat(
1557 "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
1558
1559 return false;
1560 }
1561 }
1562 /// <summary>
1563 /// Add prim in response to an add taint.
1564 /// </summary>
1565 private void changeadd()
1566 {
1567// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
1568
1569 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1570 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1571
1572 if (targetspace == IntPtr.Zero)
1573 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1574
1575 m_targetSpace = targetspace;
1576
1577 IMesh mesh = null;
1578
1579 if (_parent_scene.needsMeshing(_pbs))
1580 {
1581 // Don't need to re-enable body.. it's done in SetMesh
1582 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1583 // createmesh returns null when it's a shape that isn't a cube.
1584 // m_log.Debug(m_localID);
1585 if (mesh == null)
1586 CheckMeshAsset();
1587 else
1588 m_assetFailed = false;
1589 }
1590
1591#if SPAM
1592Console.WriteLine("changeadd 1");
1593#endif
1594 CreateGeom(m_targetSpace, mesh);
1595
1596 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1597 d.Quaternion myrot = new d.Quaternion();
1598 myrot.X = _orientation.X;
1599 myrot.Y = _orientation.Y;
1600 myrot.Z = _orientation.Z;
1601 myrot.W = _orientation.W;
1602 d.GeomSetQuaternion(prim_geom, ref myrot);
1603
1604 if (IsPhysical && Body == IntPtr.Zero)
1605 enableBody();
1606
1607 changeSelectedStatus();
1608
1609 m_taintadd = false;
1610 }
1611
1612 /// <summary>
1613 /// Move prim in response to a move taint.
1614 /// </summary>
1615 private void changemove()
1616 {
1617 if (IsPhysical)
1618 {
1619 if (!m_disabled && !m_taintremove && !childPrim)
1620 {
1621 if (Body == IntPtr.Zero)
1622 enableBody();
1623
1624 //Prim auto disable after 20 frames,
1625 //if you move it, re-enable the prim manually.
1626 if (_parent != null)
1627 {
1628 if (m_linkJoint != IntPtr.Zero)
1629 {
1630 d.JointDestroy(m_linkJoint);
1631 m_linkJoint = IntPtr.Zero;
1632 }
1633 }
1634
1635 if (Body != IntPtr.Zero)
1636 {
1637 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1638
1639 if (_parent != null)
1640 {
1641 OdePrim odParent = (OdePrim)_parent;
1642 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1643 {
1644// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1645Console.WriteLine(" JointCreateFixed");
1646 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1647 d.JointAttach(m_linkJoint, Body, odParent.Body);
1648 d.JointSetFixed(m_linkJoint);
1649 }
1650 }
1651 d.BodyEnable(Body);
1652 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1653 {
1654 m_vehicle.Enable(Body, _parent_scene);
1655 }
1656 }
1657 else
1658 {
1659 m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
1660 }
1661 }
1662 //else
1663 // {
1664 //m_log.Debug("[BUG]: race!");
1665 //}
1666 }
1667
1668 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1669 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1670// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1671
1672 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1673 m_targetSpace = tempspace;
1674
1675// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1676
1677 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1678
1679// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1680 d.SpaceAdd(m_targetSpace, prim_geom);
1681
1682 changeSelectedStatus();
1683
1684 resetCollisionAccounting();
1685 m_taintposition = _position;
1686 }
1687
1688 internal void Move(float timestep)
1689 {
1690 float fx = 0;
1691 float fy = 0;
1692 float fz = 0;
1693
1694 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1695 {
1696 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1697 {
1698 // 'VEHICLES' are dealt with in ODEDynamics.cs
1699 m_vehicle.Step(timestep, _parent_scene);
1700 }
1701 else
1702 {
1703//Console.WriteLine("Move " + Name);
1704 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1705 // NON-'VEHICLES' are dealt with here
1706// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1707// {
1708// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1709// /*
1710// if (m_angularlock.X == 1)
1711// avel2.X = 0;
1712// if (m_angularlock.Y == 1)
1713// avel2.Y = 0;
1714// if (m_angularlock.Z == 1)
1715// avel2.Z = 0;
1716// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1717// */
1718// }
1719 //float PID_P = 900.0f;
1720
1721 float m_mass = CalculateMass();
1722
1723// fz = 0f;
1724 //m_log.Info(m_collisionFlags.ToString());
1725
1726
1727 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1728 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1729 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1730 // gravityz multiplier = 1 - m_buoyancy
1731 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1732
1733 if (PIDActive)
1734 {
1735//Console.WriteLine("PID " + Name);
1736 // KF - this is for object move? eg. llSetPos() ?
1737 //if (!d.BodyIsEnabled(Body))
1738 //d.BodySetForce(Body, 0f, 0f, 0f);
1739 // If we're using the PID controller, then we have no gravity
1740 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1741 fz = 0f;
1742
1743 // no lock; for now it's only called from within Simulate()
1744
1745 // If the PID Controller isn't active then we set our force
1746 // calculating base velocity to the current position
1747
1748 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1749 {
1750 //PID_G = PID_G / m_PIDTau;
1751 m_PIDTau = 1;
1752 }
1753
1754 if ((PID_G - m_PIDTau) <= 0)
1755 {
1756 PID_G = m_PIDTau + 1;
1757 }
1758 //PidStatus = true;
1759
1760 // PhysicsVector vec = new PhysicsVector();
1761 d.Vector3 vel = d.BodyGetLinearVel(Body);
1762
1763 d.Vector3 pos = d.BodyGetPosition(Body);
1764 _target_velocity =
1765 new Vector3(
1766 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1767 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1768 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1769 );
1770
1771 // if velocity is zero, use position control; otherwise, velocity control
1772
1773 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1774 {
1775 // keep track of where we stopped. No more slippin' & slidin'
1776
1777 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1778 // react to the physics scene by moving it's position.
1779 // Avatar to Avatar collisions
1780 // Prim to avatar collisions
1781
1782 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1783 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1784 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1785 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1786 d.BodySetLinearVel(Body, 0, 0, 0);
1787 d.BodyAddForce(Body, 0, 0, fz);
1788 return;
1789 }
1790 else
1791 {
1792 _zeroFlag = false;
1793
1794 // We're flying and colliding with something
1795 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1796 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1797
1798 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1799
1800 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1801 }
1802 } // end if (PIDActive)
1803
1804 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1805 if (m_useHoverPID && !PIDActive)
1806 {
1807//Console.WriteLine("Hover " + Name);
1808
1809 // If we're using the PID controller, then we have no gravity
1810 fz = (-1 * _parent_scene.gravityz) * m_mass;
1811
1812 // no lock; for now it's only called from within Simulate()
1813
1814 // If the PID Controller isn't active then we set our force
1815 // calculating base velocity to the current position
1816
1817 if ((m_PIDTau < 1))
1818 {
1819 PID_G = PID_G / m_PIDTau;
1820 }
1821
1822 if ((PID_G - m_PIDTau) <= 0)
1823 {
1824 PID_G = m_PIDTau + 1;
1825 }
1826
1827 // Where are we, and where are we headed?
1828 d.Vector3 pos = d.BodyGetPosition(Body);
1829 d.Vector3 vel = d.BodyGetLinearVel(Body);
1830
1831 // Non-Vehicles have a limited set of Hover options.
1832 // determine what our target height really is based on HoverType
1833 switch (m_PIDHoverType)
1834 {
1835 case PIDHoverType.Ground:
1836 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1837 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1838 break;
1839 case PIDHoverType.GroundAndWater:
1840 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1841 m_waterHeight = _parent_scene.GetWaterLevel();
1842 if (m_groundHeight > m_waterHeight)
1843 {
1844 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1845 }
1846 else
1847 {
1848 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1849 }
1850 break;
1851
1852 } // end switch (m_PIDHoverType)
1853
1854
1855 _target_velocity =
1856 new Vector3(0.0f, 0.0f,
1857 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1858 );
1859
1860 // if velocity is zero, use position control; otherwise, velocity control
1861
1862 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1863 {
1864 // keep track of where we stopped. No more slippin' & slidin'
1865
1866 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1867 // react to the physics scene by moving it's position.
1868 // Avatar to Avatar collisions
1869 // Prim to avatar collisions
1870
1871 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1872 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1873 d.BodyAddForce(Body, 0, 0, fz);
1874 return;
1875 }
1876 else
1877 {
1878 _zeroFlag = false;
1879
1880 // We're flying and colliding with something
1881 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1882 }
1883 }
1884
1885 fx *= m_mass;
1886 fy *= m_mass;
1887 //fz *= m_mass;
1888
1889 fx += m_force.X;
1890 fy += m_force.Y;
1891 fz += m_force.Z;
1892
1893 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1894 if (fx != 0 || fy != 0 || fz != 0)
1895 {
1896 //m_taintdisable = true;
1897 //base.RaiseOutOfBounds(Position);
1898 //d.BodySetLinearVel(Body, fx, fy, 0f);
1899 if (!d.BodyIsEnabled(Body))
1900 {
1901 // A physical body at rest on a surface will auto-disable after a while,
1902 // this appears to re-enable it incase the surface it is upon vanishes,
1903 // and the body should fall again.
1904 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1905 d.BodySetForce(Body, 0, 0, 0);
1906 enableBodySoft();
1907 }
1908
1909 // 35x10 = 350n times the mass per second applied maximum.
1910 float nmax = 35f * m_mass;
1911 float nmin = -35f * m_mass;
1912
1913 if (fx > nmax)
1914 fx = nmax;
1915 if (fx < nmin)
1916 fx = nmin;
1917 if (fy > nmax)
1918 fy = nmax;
1919 if (fy < nmin)
1920 fy = nmin;
1921 d.BodyAddForce(Body, fx, fy, fz);
1922//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1923 }
1924 }
1925 }
1926 else
1927 { // is not physical, or is not a body or is selected
1928 // _zeroPosition = d.BodyGetPosition(Body);
1929 return;
1930//Console.WriteLine("Nothing " + Name);
1931
1932 }
1933 }
1934
1935 private void rotate()
1936 {
1937 d.Quaternion myrot = new d.Quaternion();
1938 myrot.X = _orientation.X;
1939 myrot.Y = _orientation.Y;
1940 myrot.Z = _orientation.Z;
1941 myrot.W = _orientation.W;
1942 if (Body != IntPtr.Zero)
1943 {
1944 // KF: If this is a root prim do BodySet
1945 d.BodySetQuaternion(Body, ref myrot);
1946 if (IsPhysical)
1947 {
1948 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1949 createAMotor(m_angularlock);
1950 }
1951 }
1952 else
1953 {
1954 // daughter prim, do Geom set
1955 d.GeomSetQuaternion(prim_geom, ref myrot);
1956 }
1957
1958 resetCollisionAccounting();
1959 m_taintrot = _orientation;
1960 }
1961
1962 private void resetCollisionAccounting()
1963 {
1964 m_collisionscore = 0;
1965 m_interpenetrationcount = 0;
1966 m_disabled = false;
1967 }
1968
1969 /// <summary>
1970 /// Change prim in response to a disable taint.
1971 /// </summary>
1972 private void changedisable()
1973 {
1974 m_disabled = true;
1975 if (Body != IntPtr.Zero)
1976 {
1977 d.BodyDisable(Body);
1978 Body = IntPtr.Zero;
1979 }
1980
1981 m_taintdisable = false;
1982 }
1983
1984 /// <summary>
1985 /// Change prim in response to a physics status taint
1986 /// </summary>
1987 private void changePhysicsStatus()
1988 {
1989 if (IsPhysical)
1990 {
1991 if (Body == IntPtr.Zero)
1992 {
1993 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
1994 {
1995 changeshape();
1996 }
1997 else
1998 {
1999 enableBody();
2000 }
2001 }
2002 }
2003 else
2004 {
2005 if (Body != IntPtr.Zero)
2006 {
2007 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2008 {
2009 RemoveGeom();
2010
2011//Console.WriteLine("changePhysicsStatus for " + Name);
2012 changeadd();
2013 }
2014
2015 if (childPrim)
2016 {
2017 if (_parent != null)
2018 {
2019 OdePrim parent = (OdePrim)_parent;
2020 parent.ChildDelink(this);
2021 }
2022 }
2023 else
2024 {
2025 disableBody();
2026 }
2027 }
2028 }
2029
2030 changeSelectedStatus();
2031
2032 resetCollisionAccounting();
2033 m_taintPhysics = IsPhysical;
2034 }
2035
2036 /// <summary>
2037 /// Change prim in response to a size taint.
2038 /// </summary>
2039 private void changesize()
2040 {
2041#if SPAM
2042 m_log.DebugFormat("[ODE PRIM]: Called changesize");
2043#endif
2044
2045 if (_size.X <= 0) _size.X = 0.01f;
2046 if (_size.Y <= 0) _size.Y = 0.01f;
2047 if (_size.Z <= 0) _size.Z = 0.01f;
2048
2049 //kill body to rebuild
2050 if (IsPhysical && Body != IntPtr.Zero)
2051 {
2052 if (childPrim)
2053 {
2054 if (_parent != null)
2055 {
2056 OdePrim parent = (OdePrim)_parent;
2057 parent.ChildDelink(this);
2058 }
2059 }
2060 else
2061 {
2062 disableBody();
2063 }
2064 }
2065
2066 if (d.SpaceQuery(m_targetSpace, prim_geom))
2067 {
2068// _parent_scene.waitForSpaceUnlock(m_targetSpace);
2069 d.SpaceRemove(m_targetSpace, prim_geom);
2070 }
2071
2072 RemoveGeom();
2073
2074 // we don't need to do space calculation because the client sends a position update also.
2075
2076 IMesh mesh = null;
2077
2078 // Construction of new prim
2079 if (_parent_scene.needsMeshing(_pbs))
2080 {
2081 float meshlod = _parent_scene.meshSculptLOD;
2082
2083 if (IsPhysical)
2084 meshlod = _parent_scene.MeshSculptphysicalLOD;
2085 // Don't need to re-enable body.. it's done in SetMesh
2086
2087 if (_parent_scene.needsMeshing(_pbs))
2088 {
2089 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2090 if (mesh == null)
2091 CheckMeshAsset();
2092 else
2093 m_assetFailed = false;
2094 }
2095
2096 }
2097
2098 CreateGeom(m_targetSpace, mesh);
2099 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2100 d.Quaternion myrot = new d.Quaternion();
2101 myrot.X = _orientation.X;
2102 myrot.Y = _orientation.Y;
2103 myrot.Z = _orientation.Z;
2104 myrot.W = _orientation.W;
2105 d.GeomSetQuaternion(prim_geom, ref myrot);
2106
2107 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2108 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2109 {
2110 // Re creates body on size.
2111 // EnableBody also does setMass()
2112 enableBody();
2113 d.BodyEnable(Body);
2114 }
2115
2116 changeSelectedStatus();
2117
2118 if (childPrim)
2119 {
2120 if (_parent is OdePrim)
2121 {
2122 OdePrim parent = (OdePrim)_parent;
2123 parent.ChildSetGeom(this);
2124 }
2125 }
2126 resetCollisionAccounting();
2127 m_taintsize = _size;
2128 }
2129
2130 /// <summary>
2131 /// Change prim in response to a float on water taint.
2132 /// </summary>
2133 /// <param name="timestep"></param>
2134 private void changefloatonwater()
2135 {
2136 m_collidesWater = m_taintCollidesWater;
2137
2138 if (m_collidesWater)
2139 {
2140 m_collisionFlags |= CollisionCategories.Water;
2141 }
2142 else
2143 {
2144 m_collisionFlags &= ~CollisionCategories.Water;
2145 }
2146
2147 if (m_assetFailed)
2148 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
2149 else
2150
2151 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2152 }
2153 /// <summary>
2154 /// Change prim in response to a shape taint.
2155 /// </summary>
2156 private void changeshape()
2157 {
2158 m_taintshape = false;
2159
2160 // Cleanup of old prim geometry and Bodies
2161 if (IsPhysical && Body != IntPtr.Zero)
2162 {
2163 if (childPrim)
2164 {
2165 if (_parent != null)
2166 {
2167 OdePrim parent = (OdePrim)_parent;
2168 parent.ChildDelink(this);
2169 }
2170 }
2171 else
2172 {
2173 disableBody();
2174 }
2175 }
2176
2177 RemoveGeom();
2178
2179 // we don't need to do space calculation because the client sends a position update also.
2180 if (_size.X <= 0) _size.X = 0.01f;
2181 if (_size.Y <= 0) _size.Y = 0.01f;
2182 if (_size.Z <= 0) _size.Z = 0.01f;
2183 // Construction of new prim
2184
2185 IMesh mesh = null;
2186
2187
2188 if (_parent_scene.needsMeshing(_pbs))
2189 {
2190 // Don't need to re-enable body.. it's done in CreateMesh
2191 float meshlod = _parent_scene.meshSculptLOD;
2192
2193 if (IsPhysical)
2194 meshlod = _parent_scene.MeshSculptphysicalLOD;
2195
2196 // createmesh returns null when it doesn't mesh.
2197 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2198 if (mesh == null)
2199 CheckMeshAsset();
2200 else
2201 m_assetFailed = false;
2202 }
2203
2204 CreateGeom(m_targetSpace, mesh);
2205 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2206 d.Quaternion myrot = new d.Quaternion();
2207 //myrot.W = _orientation.w;
2208 myrot.W = _orientation.W;
2209 myrot.X = _orientation.X;
2210 myrot.Y = _orientation.Y;
2211 myrot.Z = _orientation.Z;
2212 d.GeomSetQuaternion(prim_geom, ref myrot);
2213
2214 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2215 if (IsPhysical && Body == IntPtr.Zero)
2216 {
2217 // Re creates body on size.
2218 // EnableBody also does setMass()
2219 enableBody();
2220 if (Body != IntPtr.Zero)
2221 {
2222 d.BodyEnable(Body);
2223 }
2224 }
2225
2226 changeSelectedStatus();
2227
2228 if (childPrim)
2229 {
2230 if (_parent is OdePrim)
2231 {
2232 OdePrim parent = (OdePrim)_parent;
2233 parent.ChildSetGeom(this);
2234 }
2235 }
2236
2237 resetCollisionAccounting();
2238// m_taintshape = false;
2239 }
2240
2241 /// <summary>
2242 /// Change prim in response to an add force taint.
2243 /// </summary>
2244 private void changeAddForce()
2245 {
2246 if (!m_isSelected)
2247 {
2248 lock (m_forcelist)
2249 {
2250 //m_log.Info("[PHYSICS]: dequeing forcelist");
2251 if (IsPhysical)
2252 {
2253 Vector3 iforce = Vector3.Zero;
2254 int i = 0;
2255 try
2256 {
2257 for (i = 0; i < m_forcelist.Count; i++)
2258 {
2259
2260 iforce = iforce + (m_forcelist[i] * 100);
2261 }
2262 }
2263 catch (IndexOutOfRangeException)
2264 {
2265 m_forcelist = new List<Vector3>();
2266 m_collisionscore = 0;
2267 m_interpenetrationcount = 0;
2268 m_taintforce = false;
2269 return;
2270 }
2271 catch (ArgumentOutOfRangeException)
2272 {
2273 m_forcelist = new List<Vector3>();
2274 m_collisionscore = 0;
2275 m_interpenetrationcount = 0;
2276 m_taintforce = false;
2277 return;
2278 }
2279 d.BodyEnable(Body);
2280 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2281 }
2282 m_forcelist.Clear();
2283 }
2284
2285 m_collisionscore = 0;
2286 m_interpenetrationcount = 0;
2287 }
2288
2289 m_taintforce = false;
2290 }
2291
2292 /// <summary>
2293 /// Change prim in response to a torque taint.
2294 /// </summary>
2295 private void changeSetTorque()
2296 {
2297 if (!m_isSelected)
2298 {
2299 if (IsPhysical && Body != IntPtr.Zero)
2300 {
2301 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2302 }
2303 }
2304
2305 m_taintTorque = Vector3.Zero;
2306 }
2307
2308 /// <summary>
2309 /// Change prim in response to an angular force taint.
2310 /// </summary>
2311 private void changeAddAngularForce()
2312 {
2313 if (!m_isSelected)
2314 {
2315 lock (m_angularforcelist)
2316 {
2317 //m_log.Info("[PHYSICS]: dequeing forcelist");
2318 if (IsPhysical)
2319 {
2320 Vector3 iforce = Vector3.Zero;
2321 for (int i = 0; i < m_angularforcelist.Count; i++)
2322 {
2323 iforce = iforce + (m_angularforcelist[i] * 100);
2324 }
2325 d.BodyEnable(Body);
2326 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2327
2328 }
2329 m_angularforcelist.Clear();
2330 }
2331
2332 m_collisionscore = 0;
2333 m_interpenetrationcount = 0;
2334 }
2335
2336 m_taintaddangularforce = false;
2337 }
2338
2339 /// <summary>
2340 /// Change prim in response to a velocity taint.
2341 /// </summary>
2342 private void changevelocity()
2343 {
2344 if (!m_isSelected)
2345 {
2346 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
2347 // walking through a default rez size prim if it keeps kicking it around - justincc.
2348 Thread.Sleep(20);
2349
2350 if (IsPhysical)
2351 {
2352 if (Body != IntPtr.Zero)
2353 {
2354 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2355 }
2356 }
2357
2358 //resetCollisionAccounting();
2359 }
2360
2361 m_taintVelocity = Vector3.Zero;
2362 }
2363
2364 internal void setPrimForRemoval()
2365 {
2366 m_taintremove = true;
2367 }
2368
2369 public override bool Flying
2370 {
2371 // no flying prims for you
2372 get { return false; }
2373 set { }
2374 }
2375
2376 public override bool IsColliding
2377 {
2378 get { return iscolliding; }
2379 set { iscolliding = value; }
2380 }
2381
2382 public override bool CollidingGround
2383 {
2384 get { return false; }
2385 set { return; }
2386 }
2387
2388 public override bool CollidingObj
2389 {
2390 get { return false; }
2391 set { return; }
2392 }
2393
2394 public override bool ThrottleUpdates
2395 {
2396 get { return m_throttleUpdates; }
2397 set { m_throttleUpdates = value; }
2398 }
2399
2400 public override bool Stopped
2401 {
2402 get { return _zeroFlag; }
2403 }
2404
2405 public override Vector3 Position
2406 {
2407 get { return _position; }
2408
2409 set { _position = value;
2410 //m_log.Info("[PHYSICS]: " + _position.ToString());
2411 }
2412 }
2413
2414 public override Vector3 Size
2415 {
2416 get { return _size; }
2417 set
2418 {
2419 if (value.IsFinite())
2420 {
2421 _size = value;
2422// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
2423 }
2424 else
2425 {
2426 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
2427 }
2428 }
2429 }
2430
2431 public override float Mass
2432 {
2433 get { return CalculateMass(); }
2434 }
2435
2436 public override Vector3 Force
2437 {
2438 //get { return Vector3.Zero; }
2439 get { return m_force; }
2440 set
2441 {
2442 if (value.IsFinite())
2443 {
2444 m_force = value;
2445 }
2446 else
2447 {
2448 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
2449 }
2450 }
2451 }
2452
2453 public override int VehicleType
2454 {
2455 get { return (int)m_vehicle.Type; }
2456 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2457 }
2458
2459 public override void VehicleFloatParam(int param, float value)
2460 {
2461 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2462 }
2463
2464 public override void VehicleVectorParam(int param, Vector3 value)
2465 {
2466 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2467 }
2468
2469 public override void VehicleRotationParam(int param, Quaternion rotation)
2470 {
2471 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2472 }
2473
2474 public override void VehicleFlags(int param, bool remove)
2475 {
2476 m_vehicle.ProcessVehicleFlags(param, remove);
2477 }
2478
2479 public override void SetVolumeDetect(int param)
2480 {
2481 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
2482 // possible collisions with this prim or for none of them.
2483 lock (_parent_scene.OdeLock)
2484 {
2485 m_isVolumeDetect = (param != 0);
2486 }
2487 }
2488
2489 public override Vector3 CenterOfMass
2490 {
2491 get { return Vector3.Zero; }
2492 }
2493
2494 public override Vector3 GeometricCenter
2495 {
2496 get { return Vector3.Zero; }
2497 }
2498
2499 public override PrimitiveBaseShape Shape
2500 {
2501 set
2502 {
2503 _pbs = value;
2504 m_assetFailed = false;
2505 m_taintshape = true;
2506 }
2507 }
2508
2509 public override Vector3 Velocity
2510 {
2511 get
2512 {
2513 // Average previous velocity with the new one so
2514 // client object interpolation works a 'little' better
2515 if (_zeroFlag)
2516 return Vector3.Zero;
2517
2518 Vector3 returnVelocity = Vector3.Zero;
2519 returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
2520 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
2521 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
2522 return returnVelocity;
2523 }
2524 set
2525 {
2526 if (value.IsFinite())
2527 {
2528 _velocity = value;
2529
2530 m_taintVelocity = value;
2531 _parent_scene.AddPhysicsActorTaint(this);
2532 }
2533 else
2534 {
2535 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
2536 }
2537
2538 }
2539 }
2540
2541 public override Vector3 Torque
2542 {
2543 get
2544 {
2545 if (!IsPhysical || Body == IntPtr.Zero)
2546 return Vector3.Zero;
2547
2548 return _torque;
2549 }
2550
2551 set
2552 {
2553 if (value.IsFinite())
2554 {
2555 m_taintTorque = value;
2556 _parent_scene.AddPhysicsActorTaint(this);
2557 }
2558 else
2559 {
2560 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
2561 }
2562 }
2563 }
2564
2565 public override float CollisionScore
2566 {
2567 get { return m_collisionscore; }
2568 set { m_collisionscore = value; }
2569 }
2570
2571 public override bool Kinematic
2572 {
2573 get { return false; }
2574 set { }
2575 }
2576
2577 public override Quaternion Orientation
2578 {
2579 get { return _orientation; }
2580 set
2581 {
2582 if (QuaternionIsFinite(value))
2583 _orientation = value;
2584 else
2585 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
2586 }
2587 }
2588
2589 private static bool QuaternionIsFinite(Quaternion q)
2590 {
2591 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2592 return false;
2593 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2594 return false;
2595 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2596 return false;
2597 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2598 return false;
2599 return true;
2600 }
2601
2602 public override Vector3 Acceleration
2603 {
2604 get { return _acceleration; }
2605 set { _acceleration = value; }
2606 }
2607
2608 public override void AddForce(Vector3 force, bool pushforce)
2609 {
2610 if (force.IsFinite())
2611 {
2612 lock (m_forcelist)
2613 m_forcelist.Add(force);
2614
2615 m_taintforce = true;
2616 }
2617 else
2618 {
2619 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
2620 }
2621 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2622 }
2623
2624 public override void AddAngularForce(Vector3 force, bool pushforce)
2625 {
2626 if (force.IsFinite())
2627 {
2628 m_angularforcelist.Add(force);
2629 m_taintaddangularforce = true;
2630 }
2631 else
2632 {
2633 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2634 }
2635 }
2636
2637 public override Vector3 RotationalVelocity
2638 {
2639 get
2640 {
2641 Vector3 pv = Vector3.Zero;
2642 if (_zeroFlag)
2643 return pv;
2644 m_lastUpdateSent = false;
2645
2646 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2647 return pv;
2648
2649 return m_rotationalVelocity;
2650 }
2651 set
2652 {
2653 if (value.IsFinite())
2654 {
2655 m_rotationalVelocity = value;
2656 setAngularVelocity(value.X, value.Y, value.Z);
2657 }
2658 else
2659 {
2660 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2661 }
2662 }
2663 }
2664
2665 public override void CrossingFailure()
2666 {
2667 m_crossingfailures++;
2668 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2669 {
2670 base.RaiseOutOfBounds(_position);
2671 return;
2672 }
2673 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2674 {
2675 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2676 }
2677 }
2678
2679 public override float Buoyancy
2680 {
2681 get { return m_buoyancy; }
2682 set { m_buoyancy = value; }
2683 }
2684
2685 public override void link(PhysicsActor obj)
2686 {
2687 m_taintparent = obj;
2688 }
2689
2690 public override void delink()
2691 {
2692 m_taintparent = null;
2693 }
2694
2695 public override void LockAngularMotion(Vector3 axis)
2696 {
2697 // reverse the zero/non zero values for ODE.
2698 if (axis.IsFinite())
2699 {
2700 axis.X = (axis.X > 0) ? 1f : 0f;
2701 axis.Y = (axis.Y > 0) ? 1f : 0f;
2702 axis.Z = (axis.Z > 0) ? 1f : 0f;
2703 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2704 m_taintAngularLock = axis;
2705 }
2706 else
2707 {
2708 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
2709 }
2710 }
2711
2712 internal void UpdatePositionAndVelocity()
2713 {
2714 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2715 if (_parent == null)
2716 {
2717 Vector3 pv = Vector3.Zero;
2718 bool lastZeroFlag = _zeroFlag;
2719 float m_minvelocity = 0;
2720 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2721 {
2722 d.Vector3 vec = d.BodyGetPosition(Body);
2723 d.Quaternion ori = d.BodyGetQuaternion(Body);
2724 d.Vector3 vel = d.BodyGetLinearVel(Body);
2725 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2726 d.Vector3 torque = d.BodyGetTorque(Body);
2727 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2728 Vector3 l_position = Vector3.Zero;
2729 Quaternion l_orientation = Quaternion.Identity;
2730
2731 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2732 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2733 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2734 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2735 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2736
2737 m_lastposition = _position;
2738 m_lastorientation = _orientation;
2739
2740 l_position.X = vec.X;
2741 l_position.Y = vec.Y;
2742 l_position.Z = vec.Z;
2743 l_orientation.X = ori.X;
2744 l_orientation.Y = ori.Y;
2745 l_orientation.Z = ori.Z;
2746 l_orientation.W = ori.W;
2747
2748 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
2749 {
2750 //base.RaiseOutOfBounds(l_position);
2751
2752 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2753 {
2754 _position = l_position;
2755 //_parent_scene.remActivePrim(this);
2756 if (_parent == null)
2757 base.RequestPhysicsterseUpdate();
2758 return;
2759 }
2760 else
2761 {
2762 if (_parent == null)
2763 base.RaiseOutOfBounds(l_position);
2764 return;
2765 }
2766 }
2767
2768 if (l_position.Z < 0)
2769 {
2770 // This is so prim that get lost underground don't fall forever and suck up
2771 //
2772 // Sim resources and memory.
2773 // Disables the prim's movement physics....
2774 // It's a hack and will generate a console message if it fails.
2775
2776 //IsPhysical = false;
2777 if (_parent == null)
2778 base.RaiseOutOfBounds(_position);
2779
2780 _acceleration.X = 0;
2781 _acceleration.Y = 0;
2782 _acceleration.Z = 0;
2783
2784 _velocity.X = 0;
2785 _velocity.Y = 0;
2786 _velocity.Z = 0;
2787 m_rotationalVelocity.X = 0;
2788 m_rotationalVelocity.Y = 0;
2789 m_rotationalVelocity.Z = 0;
2790
2791 if (_parent == null)
2792 base.RequestPhysicsterseUpdate();
2793
2794 m_throttleUpdates = false;
2795 throttleCounter = 0;
2796 _zeroFlag = true;
2797 //outofBounds = true;
2798 }
2799
2800 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2801//Console.WriteLine("Adiff " + Name + " = " + Adiff);
2802 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2803 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2804 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2805// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2806 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2807 {
2808 _zeroFlag = true;
2809//Console.WriteLine("ZFT 2");
2810 m_throttleUpdates = false;
2811 }
2812 else
2813 {
2814 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2815 _zeroFlag = false;
2816 m_lastUpdateSent = false;
2817 //m_throttleUpdates = false;
2818 }
2819
2820 if (_zeroFlag)
2821 {
2822 _velocity.X = 0.0f;
2823 _velocity.Y = 0.0f;
2824 _velocity.Z = 0.0f;
2825
2826 _acceleration.X = 0;
2827 _acceleration.Y = 0;
2828 _acceleration.Z = 0;
2829
2830 //_orientation.w = 0f;
2831 //_orientation.X = 0f;
2832 //_orientation.Y = 0f;
2833 //_orientation.Z = 0f;
2834 m_rotationalVelocity.X = 0;
2835 m_rotationalVelocity.Y = 0;
2836 m_rotationalVelocity.Z = 0;
2837 if (!m_lastUpdateSent)
2838 {
2839 m_throttleUpdates = false;
2840 throttleCounter = 0;
2841 m_rotationalVelocity = pv;
2842
2843 if (_parent == null)
2844 {
2845 base.RequestPhysicsterseUpdate();
2846 }
2847
2848 m_lastUpdateSent = true;
2849 }
2850 }
2851 else
2852 {
2853 if (lastZeroFlag != _zeroFlag)
2854 {
2855 if (_parent == null)
2856 {
2857 base.RequestPhysicsterseUpdate();
2858 }
2859 }
2860
2861 m_lastVelocity = _velocity;
2862
2863 _position = l_position;
2864
2865 _velocity.X = vel.X;
2866 _velocity.Y = vel.Y;
2867 _velocity.Z = vel.Z;
2868
2869 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2870 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2871 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2872
2873 // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
2874 // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
2875 // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
2876 // adding these logical exclusion situations to maintain this where I think it was intended to be.
2877 if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
2878 {
2879 m_minvelocity = 0.5f;
2880 }
2881 else
2882 {
2883 m_minvelocity = 0.02f;
2884 }
2885
2886 if (_velocity.ApproxEquals(pv, m_minvelocity))
2887 {
2888 m_rotationalVelocity = pv;
2889 }
2890 else
2891 {
2892 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2893 }
2894
2895 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2896 _orientation.X = ori.X;
2897 _orientation.Y = ori.Y;
2898 _orientation.Z = ori.Z;
2899 _orientation.W = ori.W;
2900 m_lastUpdateSent = false;
2901 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2902 {
2903 if (_parent == null)
2904 {
2905 base.RequestPhysicsterseUpdate();
2906 }
2907 }
2908 else
2909 {
2910 throttleCounter++;
2911 }
2912 }
2913 m_lastposition = l_position;
2914 }
2915 else
2916 {
2917 // Not a body.. so Make sure the client isn't interpolating
2918 _velocity.X = 0;
2919 _velocity.Y = 0;
2920 _velocity.Z = 0;
2921
2922 _acceleration.X = 0;
2923 _acceleration.Y = 0;
2924 _acceleration.Z = 0;
2925
2926 m_rotationalVelocity.X = 0;
2927 m_rotationalVelocity.Y = 0;
2928 m_rotationalVelocity.Z = 0;
2929 _zeroFlag = true;
2930 }
2931 }
2932 }
2933
2934 public override bool FloatOnWater
2935 {
2936 set {
2937 m_taintCollidesWater = value;
2938 _parent_scene.AddPhysicsActorTaint(this);
2939 }
2940 }
2941
2942 public override void SetMomentum(Vector3 momentum)
2943 {
2944 }
2945
2946 public override Vector3 PIDTarget
2947 {
2948 set
2949 {
2950 if (value.IsFinite())
2951 {
2952 m_PIDTarget = value;
2953 }
2954 else
2955 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
2956 }
2957 }
2958 public override bool PIDActive { get; set; }
2959 public override float PIDTau { set { m_PIDTau = value; } }
2960
2961 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2962 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2963 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2964 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2965
2966 public override Quaternion APIDTarget{ set { return; } }
2967
2968 public override bool APIDActive{ set { return; } }
2969
2970 public override float APIDStrength{ set { return; } }
2971
2972 public override float APIDDamping{ set { return; } }
2973
2974 private void createAMotor(Vector3 axis)
2975 {
2976 if (Body == IntPtr.Zero)
2977 return;
2978
2979 if (Amotor != IntPtr.Zero)
2980 {
2981 d.JointDestroy(Amotor);
2982 Amotor = IntPtr.Zero;
2983 }
2984
2985 float axisnum = 3;
2986
2987 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2988
2989 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2990
2991
2992 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2993 d.Mass objMass;
2994 d.MassSetZero(out objMass);
2995 DMassCopy(ref pMass, ref objMass);
2996
2997 //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2998
2999 Matrix4 dMassMat = FromDMass(objMass);
3000
3001 Matrix4 mathmat = Inverse(dMassMat);
3002
3003 /*
3004 //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
3005
3006 mathmat = Inverse(mathmat);
3007
3008
3009 objMass = FromMatrix4(mathmat, ref objMass);
3010 //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
3011
3012 mathmat = Inverse(mathmat);
3013 */
3014 if (axis.X == 0)
3015 {
3016 mathmat.M33 = 50.0000001f;
3017 //objMass.I.M22 = 0;
3018 }
3019 if (axis.Y == 0)
3020 {
3021 mathmat.M22 = 50.0000001f;
3022 //objMass.I.M11 = 0;
3023 }
3024 if (axis.Z == 0)
3025 {
3026 mathmat.M11 = 50.0000001f;
3027 //objMass.I.M00 = 0;
3028 }
3029
3030
3031
3032 mathmat = Inverse(mathmat);
3033 objMass = FromMatrix4(mathmat, ref objMass);
3034 //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
3035
3036 //return;
3037 if (d.MassCheck(ref objMass))
3038 {
3039 d.BodySetMass(Body, ref objMass);
3040 }
3041 else
3042 {
3043 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
3044 }
3045
3046 if (axisnum <= 0)
3047 return;
3048 // int dAMotorEuler = 1;
3049
3050 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3051 d.JointAttach(Amotor, Body, IntPtr.Zero);
3052 d.JointSetAMotorMode(Amotor, 0);
3053
3054 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3055 int i = 0;
3056
3057 if (axis.X == 0)
3058 {
3059 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3060 i++;
3061 }
3062
3063 if (axis.Y == 0)
3064 {
3065 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3066 i++;
3067 }
3068
3069 if (axis.Z == 0)
3070 {
3071 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3072 i++;
3073 }
3074
3075 for (int j = 0; j < (int)axisnum; j++)
3076 {
3077 //d.JointSetAMotorAngle(Amotor, j, 0);
3078 }
3079
3080 //d.JointSetAMotorAngle(Amotor, 1, 0);
3081 //d.JointSetAMotorAngle(Amotor, 2, 0);
3082
3083 // These lowstops and high stops are effectively (no wiggle room)
3084 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3085 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3086 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3087 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3088 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3089 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3090 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3091 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3092 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3093 }
3094
3095 private Matrix4 FromDMass(d.Mass pMass)
3096 {
3097 Matrix4 obj;
3098 obj.M11 = pMass.I.M00;
3099 obj.M12 = pMass.I.M01;
3100 obj.M13 = pMass.I.M02;
3101 obj.M14 = 0;
3102 obj.M21 = pMass.I.M10;
3103 obj.M22 = pMass.I.M11;
3104 obj.M23 = pMass.I.M12;
3105 obj.M24 = 0;
3106 obj.M31 = pMass.I.M20;
3107 obj.M32 = pMass.I.M21;
3108 obj.M33 = pMass.I.M22;
3109 obj.M34 = 0;
3110 obj.M41 = 0;
3111 obj.M42 = 0;
3112 obj.M43 = 0;
3113 obj.M44 = 1;
3114 return obj;
3115 }
3116
3117 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3118 {
3119 obj.I.M00 = pMat[0, 0];
3120 obj.I.M01 = pMat[0, 1];
3121 obj.I.M02 = pMat[0, 2];
3122 obj.I.M10 = pMat[1, 0];
3123 obj.I.M11 = pMat[1, 1];
3124 obj.I.M12 = pMat[1, 2];
3125 obj.I.M20 = pMat[2, 0];
3126 obj.I.M21 = pMat[2, 1];
3127 obj.I.M22 = pMat[2, 2];
3128 return obj;
3129 }
3130
3131 public override void SubscribeEvents(int ms)
3132 {
3133 m_eventsubscription = ms;
3134 _parent_scene.AddCollisionEventReporting(this);
3135 }
3136
3137 public override void UnSubscribeEvents()
3138 {
3139 _parent_scene.RemoveCollisionEventReporting(this);
3140 m_eventsubscription = 0;
3141 }
3142
3143 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3144 {
3145 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
3146 }
3147
3148 public void SendCollisions()
3149 {
3150 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
3151 {
3152 base.SendCollisionUpdate(CollisionEventsThisFrame);
3153
3154 if (CollisionEventsThisFrame.Count > 0)
3155 {
3156 m_collisionsOnPreviousFrame = true;
3157 CollisionEventsThisFrame.Clear();
3158 }
3159 else
3160 {
3161 m_collisionsOnPreviousFrame = false;
3162 }
3163 }
3164 }
3165
3166 public override bool SubscribedEvents()
3167 {
3168 if (m_eventsubscription > 0)
3169 return true;
3170 return false;
3171 }
3172
3173 public static Matrix4 Inverse(Matrix4 pMat)
3174 {
3175 if (determinant3x3(pMat) == 0)
3176 {
3177 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3178 }
3179
3180 return (Adjoint(pMat) / determinant3x3(pMat));
3181 }
3182
3183 public static Matrix4 Adjoint(Matrix4 pMat)
3184 {
3185 Matrix4 adjointMatrix = new Matrix4();
3186 for (int i=0; i<4; i++)
3187 {
3188 for (int j=0; j<4; j++)
3189 {
3190 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3191 }
3192 }
3193
3194 adjointMatrix = Transpose(adjointMatrix);
3195 return adjointMatrix;
3196 }
3197
3198 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3199 {
3200 Matrix4 minor = new Matrix4();
3201 int m = 0, n = 0;
3202 for (int i = 0; i < 4; i++)
3203 {
3204 if (i == iRow)
3205 continue;
3206 n = 0;
3207 for (int j = 0; j < 4; j++)
3208 {
3209 if (j == iCol)
3210 continue;
3211 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3212 n++;
3213 }
3214 m++;
3215 }
3216
3217 return minor;
3218 }
3219
3220 public static Matrix4 Transpose(Matrix4 pMat)
3221 {
3222 Matrix4 transposeMatrix = new Matrix4();
3223 for (int i = 0; i < 4; i++)
3224 for (int j = 0; j < 4; j++)
3225 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3226 return transposeMatrix;
3227 }
3228
3229 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3230 {
3231 switch (r)
3232 {
3233 case 0:
3234 switch (c)
3235 {
3236 case 0:
3237 pMat.M11 = val;
3238 break;
3239 case 1:
3240 pMat.M12 = val;
3241 break;
3242 case 2:
3243 pMat.M13 = val;
3244 break;
3245 case 3:
3246 pMat.M14 = val;
3247 break;
3248 }
3249
3250 break;
3251 case 1:
3252 switch (c)
3253 {
3254 case 0:
3255 pMat.M21 = val;
3256 break;
3257 case 1:
3258 pMat.M22 = val;
3259 break;
3260 case 2:
3261 pMat.M23 = val;
3262 break;
3263 case 3:
3264 pMat.M24 = val;
3265 break;
3266 }
3267
3268 break;
3269 case 2:
3270 switch (c)
3271 {
3272 case 0:
3273 pMat.M31 = val;
3274 break;
3275 case 1:
3276 pMat.M32 = val;
3277 break;
3278 case 2:
3279 pMat.M33 = val;
3280 break;
3281 case 3:
3282 pMat.M34 = val;
3283 break;
3284 }
3285
3286 break;
3287 case 3:
3288 switch (c)
3289 {
3290 case 0:
3291 pMat.M41 = val;
3292 break;
3293 case 1:
3294 pMat.M42 = val;
3295 break;
3296 case 2:
3297 pMat.M43 = val;
3298 break;
3299 case 3:
3300 pMat.M44 = val;
3301 break;
3302 }
3303
3304 break;
3305 }
3306 }
3307
3308 private static float determinant3x3(Matrix4 pMat)
3309 {
3310 float det = 0;
3311 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3312 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3313 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3314 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3315 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3316 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3317
3318 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3319 return det;
3320 }
3321
3322 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3323 {
3324 dst.c.W = src.c.W;
3325 dst.c.X = src.c.X;
3326 dst.c.Y = src.c.Y;
3327 dst.c.Z = src.c.Z;
3328 dst.mass = src.mass;
3329 dst.I.M00 = src.I.M00;
3330 dst.I.M01 = src.I.M01;
3331 dst.I.M02 = src.I.M02;
3332 dst.I.M10 = src.I.M10;
3333 dst.I.M11 = src.I.M11;
3334 dst.I.M12 = src.I.M12;
3335 dst.I.M20 = src.I.M20;
3336 dst.I.M21 = src.I.M21;
3337 dst.I.M22 = src.I.M22;
3338 }
3339
3340 public override void SetMaterial(int pMaterial)
3341 {
3342 m_material = pMaterial;
3343 }
3344
3345 private void CheckMeshAsset()
3346 {
3347 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
3348 {
3349 m_assetFailed = true;
3350 Util.FireAndForget(delegate
3351 {
3352 RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
3353 if (assetProvider != null)
3354 assetProvider(_pbs.SculptTexture, MeshAssetReceived);
3355 }, null, "ODEPrim.CheckMeshAsset");
3356 }
3357 }
3358
3359 private void MeshAssetReceived(AssetBase asset)
3360 {
3361 if (asset != null && asset.Data != null && asset.Data.Length > 0)
3362 {
3363 if (!_pbs.SculptEntry)
3364 return;
3365 if (_pbs.SculptTexture.ToString() != asset.ID)
3366 return;
3367
3368 _pbs.SculptData = new byte[asset.Data.Length];
3369 asset.Data.CopyTo(_pbs.SculptData, 0);
3370// m_assetFailed = false;
3371
3372// m_log.DebugFormat(
3373// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
3374// _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
3375
3376 m_taintshape = true;
3377 _parent_scene.AddPhysicsActorTaint(this);
3378 }
3379 else
3380 {
3381 m_log.WarnFormat(
3382 "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
3383 _pbs.SculptTexture, Name, _position, _parent_scene.Name);
3384 }
3385 }
3386 }
3387} \ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..8d7d3b3
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs
@@ -0,0 +1,434 @@
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 /// Pending ray requests
54 /// </summary>
55 protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>();
56
57 /// <summary>
58 /// Scene that created this object.
59 /// </summary>
60 private OdeScene m_scene;
61
62 /// <summary>
63 /// ODE contact array to be filled by the collision testing
64 /// </summary>
65 d.ContactGeom[] contacts = new d.ContactGeom[5];
66
67 /// <summary>
68 /// ODE near callback delegate
69 /// </summary>
70 private d.NearCallback nearCallback;
71 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
72 private List<ContactResult> m_contactResults = new List<ContactResult>();
73
74
75 public ODERayCastRequestManager(OdeScene pScene)
76 {
77 m_scene = pScene;
78 nearCallback = near;
79
80 }
81
82 /// <summary>
83 /// Queues a raycast
84 /// </summary>
85 /// <param name="position">Origin of Ray</param>
86 /// <param name="direction">Ray normal</param>
87 /// <param name="length">Ray length</param>
88 /// <param name="retMethod">Return method to send the results</param>
89 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
90 {
91 lock (m_PendingRequests)
92 {
93 ODERayCastRequest req = new ODERayCastRequest();
94 req.callbackMethod = retMethod;
95 req.length = length;
96 req.Normal = direction;
97 req.Origin = position;
98
99 m_PendingRequests.Add(req);
100 }
101 }
102
103 /// <summary>
104 /// Queues a raycast
105 /// </summary>
106 /// <param name="position">Origin of Ray</param>
107 /// <param name="direction">Ray normal</param>
108 /// <param name="length">Ray length</param>
109 /// <param name="count"></param>
110 /// <param name="retMethod">Return method to send the results</param>
111 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
112 {
113 lock (m_PendingRequests)
114 {
115 ODERayRequest req = new ODERayRequest();
116 req.callbackMethod = retMethod;
117 req.length = length;
118 req.Normal = direction;
119 req.Origin = position;
120 req.Count = count;
121
122 m_PendingRayRequests.Add(req);
123 }
124 }
125
126 /// <summary>
127 /// Process all queued raycast requests
128 /// </summary>
129 /// <returns>Time in MS the raycasts took to process.</returns>
130 public int ProcessQueuedRequests()
131 {
132 int time = System.Environment.TickCount;
133 lock (m_PendingRequests)
134 {
135 if (m_PendingRequests.Count > 0)
136 {
137 ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
138 for (int i = 0; i < reqs.Length; i++)
139 {
140 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
141 RayCast(reqs[i]); // if there isn't anyone to send results
142 }
143
144 m_PendingRequests.Clear();
145 }
146 }
147
148 lock (m_PendingRayRequests)
149 {
150 if (m_PendingRayRequests.Count > 0)
151 {
152 ODERayRequest[] reqs = m_PendingRayRequests.ToArray();
153 for (int i = 0; i < reqs.Length; i++)
154 {
155 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
156 RayCast(reqs[i]); // if there isn't anyone to send results
157 }
158
159 m_PendingRayRequests.Clear();
160 }
161 }
162
163 lock (m_contactResults)
164 m_contactResults.Clear();
165
166 return System.Environment.TickCount - time;
167 }
168
169 /// <summary>
170 /// Method that actually initiates the raycast
171 /// </summary>
172 /// <param name="req"></param>
173 private void RayCast(ODERayCastRequest req)
174 {
175 // Create the ray
176 IntPtr ray = d.CreateRay(m_scene.space, req.length);
177 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
178
179 // Collide test
180 d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
181
182 // Remove Ray
183 d.GeomDestroy(ray);
184
185 // Define default results
186 bool hitYN = false;
187 uint hitConsumerID = 0;
188 float distance = 999999999999f;
189 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
190 Vector3 snormal = Vector3.Zero;
191
192 // Find closest contact and object.
193 lock (m_contactResults)
194 {
195 foreach (ContactResult cResult in m_contactResults)
196 {
197 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
198 {
199 closestcontact = cResult.Pos;
200 hitConsumerID = cResult.ConsumerID;
201 distance = cResult.Depth;
202 hitYN = true;
203 snormal = cResult.Normal;
204 }
205 }
206
207 m_contactResults.Clear();
208 }
209
210 // Return results
211 if (req.callbackMethod != null)
212 req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
213 }
214
215 /// <summary>
216 /// Method that actually initiates the raycast
217 /// </summary>
218 /// <param name="req"></param>
219 private void RayCast(ODERayRequest req)
220 {
221 // Create the ray
222 IntPtr ray = d.CreateRay(m_scene.space, req.length);
223 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
224
225 // Collide test
226 d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
227
228 // Remove Ray
229 d.GeomDestroy(ray);
230
231 // Find closest contact and object.
232 lock (m_contactResults)
233 {
234 // Return results
235 if (req.callbackMethod != null)
236 req.callbackMethod(m_contactResults);
237 }
238 }
239
240 // This is the standard Near. Uses space AABBs to speed up detection.
241 private void near(IntPtr space, IntPtr g1, IntPtr g2)
242 {
243
244 //Don't test against heightfield Geom, or you'll be sorry!
245
246 /*
247 terminate called after throwing an instance of 'std::bad_alloc'
248 what(): std::bad_alloc
249 Stacktrace:
250
251 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
252 at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
253 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
254 at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
255 fffff>
256 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
257 at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
258 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
259 0x00114>
260 at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
261 at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
262 at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
263 at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
264 at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
265 at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
266
267 Native stacktrace:
268
269 mono [0x80d2a42]
270 [0xb7f5840c]
271 /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
272 /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
273 /usr/lib/libstdc++.so.6 [0xb45fa865]
274 /usr/lib/libstdc++.so.6 [0xb45fa8a2]
275 /usr/lib/libstdc++.so.6 [0xb45fa9da]
276 /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
277 /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
278 libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
279 libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
280 libode.so(dCollide+0x102) [0xb46571b2]
281 [0x95cfdec9]
282 [0x8ea07fe1]
283 [0xab260146]
284 libode.so [0xb465a5c4]
285 libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
286 libode.so(dSpaceCollide2+0x177) [0xb465ac67]
287 [0x95cf978e]
288 [0x8ea07945]
289 [0x95cf2bbc]
290 [0xab2787e7]
291 [0xab419fb3]
292 [0xab416657]
293 [0xab415bda]
294 [0xb609b08e]
295 mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
296 mono [0x81a2f0f]
297 mono [0x81d28b6]
298 mono [0x81ea2c6]
299 /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
300 /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
301 */
302
303 // Exclude heightfield geom
304
305 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
306 return;
307 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
308 return;
309
310 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
311 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
312 {
313 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
314 return;
315
316 // Separating static prim geometry spaces.
317 // We'll be calling near recursivly if one
318 // of them is a space to find all of the
319 // contact points in the space
320 try
321 {
322 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
323 }
324 catch (AccessViolationException)
325 {
326 m_log.Warn("[PHYSICS]: Unable to collide test a space");
327 return;
328 }
329 //Colliding a space or a geom with a space or a geom. so drill down
330
331 //Collide all geoms in each space..
332 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
333 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
334 return;
335 }
336
337 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
338 return;
339
340 int count = 0;
341 try
342 {
343
344 if (g1 == g2)
345 return; // Can't collide with yourself
346
347 lock (contacts)
348 {
349 count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
350 }
351 }
352 catch (SEHException)
353 {
354 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.");
355 }
356 catch (Exception e)
357 {
358 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
359 return;
360 }
361
362 PhysicsActor p1 = null;
363 PhysicsActor p2 = null;
364
365 if (g1 != IntPtr.Zero)
366 m_scene.actor_name_map.TryGetValue(g1, out p1);
367
368 if (g2 != IntPtr.Zero)
369 m_scene.actor_name_map.TryGetValue(g1, out p2);
370
371 // Loop over contacts, build results.
372 for (int i = 0; i < count; i++)
373 {
374 if (p1 != null)
375 {
376 if (p1 is OdePrim)
377 {
378 ContactResult collisionresult = new ContactResult();
379
380 collisionresult.ConsumerID = p1.LocalID;
381 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
382 collisionresult.Depth = contacts[i].depth;
383 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
384 contacts[i].normal.Z);
385 lock (m_contactResults)
386 m_contactResults.Add(collisionresult);
387 }
388 }
389
390 if (p2 != null)
391 {
392 if (p2 is OdePrim)
393 {
394 ContactResult collisionresult = new ContactResult();
395
396 collisionresult.ConsumerID = p2.LocalID;
397 collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
398 collisionresult.Depth = contacts[i].depth;
399 collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
400 contacts[i].normal.Z);
401
402 lock (m_contactResults)
403 m_contactResults.Add(collisionresult);
404 }
405 }
406 }
407 }
408
409 /// <summary>
410 /// Dereference the creator scene so that it can be garbage collected if needed.
411 /// </summary>
412 internal void Dispose()
413 {
414 m_scene = null;
415 }
416 }
417
418 public struct ODERayCastRequest
419 {
420 public Vector3 Origin;
421 public Vector3 Normal;
422 public float length;
423 public RaycastCallback callbackMethod;
424 }
425
426 public struct ODERayRequest
427 {
428 public Vector3 Origin;
429 public Vector3 Normal;
430 public int Count;
431 public float length;
432 public RayCallback callbackMethod;
433 }
434} \ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs
new file mode 100644
index 0000000..b4a3c48
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/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/PhysicsModules/Ode/OdePlugin.cs b/OpenSim/Region/PhysicsModules/Ode/OdePlugin.cs
new file mode 100644
index 0000000..7e652fc
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/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 Ode.NET;
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 return true;
56 }
57
58 public PhysicsScene GetScene(String sceneIdentifier)
59 {
60 if (m_scene == null)
61 {
62 // We do this so that OpenSimulator on Windows loads the correct native ODE library depending on whether
63 // it's running as a 32-bit process or a 64-bit one. By invoking LoadLibary here, later DLLImports
64 // will find it already loaded later on.
65 //
66 // This isn't necessary for other platforms (e.g. Mac OSX and Linux) since the DLL used can be
67 // controlled in Ode.NET.dll.config
68 if (Util.IsWindows())
69 Util.LoadArchSpecificWindowsDll("ode.dll");
70
71 // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
72 // http://opensimulator.org/mantis/view.php?id=2750).
73 d.InitODE();
74
75 m_scene = new OdeScene(GetName(), sceneIdentifier);
76 }
77
78 return m_scene;
79 }
80
81 public string GetName()
82 {
83 return ("OpenDynamicsEngine");
84 }
85
86 public void Dispose()
87 {
88 }
89 }
90} \ No newline at end of file
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
new file mode 100644
index 0000000..5953557
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
@@ -0,0 +1,4319 @@
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//#define SPAM
30
31using System;
32using System.Collections.Generic;
33using System.Diagnostics;
34using System.IO;
35using System.Linq;
36using System.Reflection;
37using System.Runtime.InteropServices;
38using System.Threading;
39using log4net;
40using Nini.Config;
41using Ode.NET;
42using OpenMetaverse;
43#if USE_DRAWSTUFF
44using Drawstuff.NET;
45#endif
46using OpenSim.Framework;
47using OpenSim.Region.Physics.Manager;
48
49namespace OpenSim.Region.Physics.OdePlugin
50{
51 public enum StatusIndicators : int
52 {
53 Generic = 0,
54 Start = 1,
55 End = 2
56 }
57
58// public struct sCollisionData
59// {
60// public uint ColliderLocalId;
61// public uint CollidedWithLocalId;
62// public int NumberOfCollisions;
63// public int CollisionType;
64// public int StatusIndicator;
65// public int lastframe;
66// }
67
68 [Flags]
69 public enum CollisionCategories : int
70 {
71 Disabled = 0,
72 Geom = 0x00000001,
73 Body = 0x00000002,
74 Space = 0x00000004,
75 Character = 0x00000008,
76 Land = 0x00000010,
77 Water = 0x00000020,
78 Wind = 0x00000040,
79 Sensor = 0x00000080,
80 Selected = 0x00000100
81 }
82
83 /// <summary>
84 /// Material type for a primitive
85 /// </summary>
86 public enum Material : int
87 {
88 /// <summary></summary>
89 Stone = 0,
90 /// <summary></summary>
91 Metal = 1,
92 /// <summary></summary>
93 Glass = 2,
94 /// <summary></summary>
95 Wood = 3,
96 /// <summary></summary>
97 Flesh = 4,
98 /// <summary></summary>
99 Plastic = 5,
100 /// <summary></summary>
101 Rubber = 6
102 }
103
104 public class OdeScene : PhysicsScene
105 {
106 private readonly ILog m_log;
107 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
108
109 /// <summary>
110 /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances.
111 /// </summary>
112 /// <remarks>
113 /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
114 /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts
115 /// uses a static cache at the ODE level.
116 ///
117 /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
118 /// to
119 ///
120 /// mono() [0x489171]
121 /// mono() [0x4d154f]
122 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
123 /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
124 ///
125 /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
126 /// causes OpenSimulator to immediately crash with a native stack trace similar to
127 ///
128 /// mono() [0x489171]
129 /// mono() [0x4d154f]
130 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
131 /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
132 /// </remarks>
133 internal static Object UniversalColliderSyncObject = new Object();
134
135 /// <summary>
136 /// Is stats collecting enabled for this ODE scene?
137 /// </summary>
138 public bool CollectStats { get; set; }
139
140 /// <summary>
141 /// Statistics for this scene.
142 /// </summary>
143 private Dictionary<string, float> m_stats = new Dictionary<string, float>();
144
145 /// <summary>
146 /// Stat name for total number of avatars in this ODE scene.
147 /// </summary>
148 public const string ODETotalAvatarsStatName = "ODETotalAvatars";
149
150 /// <summary>
151 /// Stat name for total number of prims in this ODE scene.
152 /// </summary>
153 public const string ODETotalPrimsStatName = "ODETotalPrims";
154
155 /// <summary>
156 /// Stat name for total number of prims with active physics in this ODE scene.
157 /// </summary>
158 public const string ODEActivePrimsStatName = "ODEActivePrims";
159
160 /// <summary>
161 /// Stat name for the total time spent in ODE frame processing.
162 /// </summary>
163 /// <remarks>
164 /// A sanity check for the main scene loop physics time.
165 /// </remarks>
166 public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
167
168 /// <summary>
169 /// Stat name for time spent processing avatar taints per frame
170 /// </summary>
171 public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
172
173 /// <summary>
174 /// Stat name for time spent processing prim taints per frame
175 /// </summary>
176 public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
177
178 /// <summary>
179 /// Stat name for time spent calculating avatar forces per frame.
180 /// </summary>
181 public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
182
183 /// <summary>
184 /// Stat name for time spent calculating prim forces per frame
185 /// </summary>
186 public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
187
188 /// <summary>
189 /// Stat name for time spent fulfilling raycasting requests per frame
190 /// </summary>
191 public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
192
193 /// <summary>
194 /// Stat name for time spent in native code that actually steps through the simulation.
195 /// </summary>
196 public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
197
198 /// <summary>
199 /// Stat name for the number of milliseconds that ODE spends in native space collision code.
200 /// </summary>
201 public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
202
203 /// <summary>
204 /// Stat name for milliseconds that ODE spends in native geom collision code.
205 /// </summary>
206 public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
207
208 /// <summary>
209 /// Time spent in collision processing that is not spent in native space or geom collision code.
210 /// </summary>
211 public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
212
213 /// <summary>
214 /// Stat name for time spent notifying listeners of collisions
215 /// </summary>
216 public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
217
218 /// <summary>
219 /// Stat name for milliseconds spent updating avatar position and velocity
220 /// </summary>
221 public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
222
223 /// <summary>
224 /// Stat name for the milliseconds spent updating prim position and velocity
225 /// </summary>
226 public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
227
228 /// <summary>
229 /// Stat name for avatar collisions with another entity.
230 /// </summary>
231 public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
232
233 /// <summary>
234 /// Stat name for prim collisions with another entity.
235 /// </summary>
236 public const string ODEPrimContactsStatName = "ODEPrimContacts";
237
238 /// <summary>
239 /// Used to hold tick numbers for stat collection purposes.
240 /// </summary>
241 private int m_nativeCollisionStartTick;
242
243 /// <summary>
244 /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
245 /// </summary>
246 private bool m_inCollisionTiming;
247
248 /// <summary>
249 /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
250 /// collisions occured using the _perloopcontact if stats collection is enabled.
251 /// </summary>
252 private int m_tempAvatarCollisionsThisFrame;
253
254 /// <summary>
255 /// Used in calculating physics frame time dilation
256 /// </summary>
257 private int tickCountFrameRun;
258
259 /// <summary>
260 /// Used in calculating physics frame time dilation
261 /// </summary>
262 private int latertickcount;
263
264 private Random fluidRandomizer = new Random(Environment.TickCount);
265
266 private const uint m_regionWidth = Constants.RegionSize;
267 private const uint m_regionHeight = Constants.RegionSize;
268
269 private float ODE_STEPSIZE = 0.0178f;
270 private float metersInSpace = 29.9f;
271 private float m_timeDilation = 1.0f;
272
273 public float gravityx = 0f;
274 public float gravityy = 0f;
275 public float gravityz = -9.8f;
276
277 public float AvatarTerminalVelocity { get; set; }
278
279 private float contactsurfacelayer = 0.001f;
280
281 private int worldHashspaceLow = -4;
282 private int worldHashspaceHigh = 128;
283
284 private int smallHashspaceLow = -4;
285 private int smallHashspaceHigh = 66;
286
287 private float waterlevel = 0f;
288 private int framecount = 0;
289 //private int m_returncollisions = 10;
290
291 private readonly IntPtr contactgroup;
292
293 internal IntPtr WaterGeom;
294
295 private float nmTerrainContactFriction = 255.0f;
296 private float nmTerrainContactBounce = 0.1f;
297 private float nmTerrainContactERP = 0.1025f;
298
299 private float mTerrainContactFriction = 75f;
300 private float mTerrainContactBounce = 0.1f;
301 private float mTerrainContactERP = 0.05025f;
302
303 private float nmAvatarObjectContactFriction = 250f;
304 private float nmAvatarObjectContactBounce = 0.1f;
305
306 private float mAvatarObjectContactFriction = 75f;
307 private float mAvatarObjectContactBounce = 0.1f;
308
309 private float avPIDD = 3200f;
310 private float avPIDP = 1400f;
311 private float avCapRadius = 0.37f;
312 private float avStandupTensor = 2000000f;
313
314 /// <summary>
315 /// true = old compatibility mode with leaning capsule; false = new corrected mode
316 /// </summary>
317 /// <remarks>
318 /// Even when set to false, the capsule still tilts but this is done in a different way.
319 /// </remarks>
320 public bool IsAvCapsuleTilted { get; private set; }
321
322 private float avDensity = 80f;
323// private float avHeightFudgeFactor = 0.52f;
324 private float avMovementDivisorWalk = 1.3f;
325 private float avMovementDivisorRun = 0.8f;
326 private float minimumGroundFlightOffset = 3f;
327 public float maximumMassObject = 10000.01f;
328
329 public bool meshSculptedPrim = true;
330 public bool forceSimplePrimMeshing = false;
331
332 public float meshSculptLOD = 32;
333 public float MeshSculptphysicalLOD = 16;
334
335 public float geomDefaultDensity = 10.000006836f;
336
337 public int geomContactPointsStartthrottle = 3;
338 public int geomUpdatesPerThrottledUpdate = 15;
339 private const int avatarExpectedContacts = 3;
340
341 public float bodyPIDD = 35f;
342 public float bodyPIDG = 25;
343
344 public int geomCrossingFailuresBeforeOutofbounds = 5;
345
346 public float bodyMotorJointMaxforceTensor = 2;
347
348 public int bodyFramesAutoDisable = 20;
349
350 private float[] _watermap;
351 private bool m_filterCollisions = true;
352
353 private d.NearCallback nearCallback;
354 public d.TriCallback triCallback;
355 public d.TriArrayCallback triArrayCallback;
356
357 /// <summary>
358 /// Avatars in the physics scene.
359 /// </summary>
360 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
361
362 /// <summary>
363 /// Prims in the physics scene.
364 /// </summary>
365 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
366
367 /// <summary>
368 /// Prims in the physics scene that are subject to physics, not just collisions.
369 /// </summary>
370 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
371
372 /// <summary>
373 /// Prims that the simulator has created/deleted/updated and so need updating in ODE.
374 /// </summary>
375 private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>();
376
377 /// <summary>
378 /// Record a character that has taints to be processed.
379 /// </summary>
380 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
381
382 /// <summary>
383 /// Keep record of contacts in the physics loop so that we can remove duplicates.
384 /// </summary>
385 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
386
387 /// <summary>
388 /// A dictionary of actors that should receive collision events.
389 /// </summary>
390 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
391
392 /// <summary>
393 /// A dictionary of collision event changes that are waiting to be processed.
394 /// </summary>
395 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
396
397 /// <summary>
398 /// Maps a unique geometry id (a memory location) to a physics actor name.
399 /// </summary>
400 /// <remarks>
401 /// Only actors participating in collisions have geometries. This has to be maintained separately from
402 /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
403 /// apart from the singleton PANull
404 /// </remarks>
405 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
406
407 /// <summary>
408 /// Maps a unique geometry id (a memory location) to a physics actor.
409 /// </summary>
410 /// <remarks>
411 /// Only actors participating in collisions have geometries.
412 /// </remarks>
413 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
414
415 /// <summary>
416 /// Defects list to remove characters that no longer have finite positions due to some other bug.
417 /// </summary>
418 /// <remarks>
419 /// Used repeatedly in Simulate() but initialized once here.
420 /// </remarks>
421 private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
422
423 private bool m_NINJA_physics_joints_enabled = false;
424 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
425 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
426 private d.ContactGeom[] contacts;
427
428 /// <summary>
429 /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
430 /// </summary>
431 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
432
433 /// <summary>
434 /// can lock for longer. accessed only by OdeScene.
435 /// </summary>
436 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>();
437
438 /// <summary>
439 /// can lock for longer. accessed only by OdeScene.
440 /// </summary>
441 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>();
442
443 /// <summary>
444 /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
445 /// </summary>
446 private readonly List<string> requestedJointsToBeDeleted = new List<string>();
447
448 private Object externalJointRequestsLock = new Object();
449 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
450 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
451 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
452 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
453
454 private d.Contact contact;
455 private d.Contact TerrainContact;
456 private d.Contact AvatarMovementprimContact;
457 private d.Contact AvatarMovementTerrainContact;
458 private d.Contact WaterContact;
459 private d.Contact[,] m_materialContacts;
460
461//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
462//Ckrinke private int m_randomizeWater = 200;
463 private int m_physicsiterations = 10;
464 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
465 private readonly PhysicsActor PANull = new NullPhysicsActor();
466// private float step_time = 0.0f;
467//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
468//Ckrinke private int ms = 0;
469 public IntPtr world;
470 //private bool returncollisions = false;
471 // private uint obj1LocalID = 0;
472 private uint obj2LocalID = 0;
473 //private int ctype = 0;
474 private OdeCharacter cc1;
475 private OdePrim cp1;
476 private OdeCharacter cc2;
477 private OdePrim cp2;
478 private int p1ExpectedPoints = 0;
479 private int p2ExpectedPoints = 0;
480 //private int cStartStop = 0;
481 //private string cDictKey = "";
482
483 public IntPtr space;
484
485 //private IntPtr tmpSpace;
486 // split static geometry collision handling into spaces of 30 meters
487 public IntPtr[,] staticPrimspace;
488
489 /// <summary>
490 /// Used to lock the entire physics scene. Locked during the main part of Simulate()
491 /// </summary>
492 internal Object OdeLock = new Object();
493
494 private bool _worldInitialized = false;
495
496 public IMesher mesher;
497
498 private IConfigSource m_config;
499
500 public bool physics_logging = false;
501 public int physics_logging_interval = 0;
502 public bool physics_logging_append_existing_logfile = false;
503
504 private bool avplanted = false;
505 private bool av_av_collisions_off = false;
506
507 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
508 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
509
510 // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
511 // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
512 // TODO: unused: private uint heightmapWidthSamples;
513 // TODO: unused: private uint heightmapHeightSamples;
514
515 private volatile int m_global_contactcount = 0;
516
517 private Vector3 m_worldOffset = Vector3.Zero;
518 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
519 private PhysicsScene m_parentScene = null;
520
521 private ODERayCastRequestManager m_rayCastManager;
522
523 /// <summary>
524 /// Initiailizes the scene
525 /// Sets many properties that ODE requires to be stable
526 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
527 /// </summary>
528 /// <param value="name">Name of the scene. Useful in debug messages.</param>
529 public OdeScene(string engineType, string name)
530 {
531 m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name);
532
533 Name = name;
534 EngineType = engineType;
535
536 nearCallback = near;
537 triCallback = TriCallback;
538 triArrayCallback = TriArrayCallback;
539 m_rayCastManager = new ODERayCastRequestManager(this);
540
541 // Create the world and the first space
542 world = d.WorldCreate();
543 space = d.HashSpaceCreate(IntPtr.Zero);
544
545 contactgroup = d.JointGroupCreate(0);
546
547 d.WorldSetAutoDisableFlag(world, false);
548
549 #if USE_DRAWSTUFF
550 Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
551 viewthread.Start();
552 #endif
553
554 _watermap = new float[258 * 258];
555
556 // Zero out the prim spaces array (we split our space into smaller spaces so
557 // we can hit test less.
558 }
559
560#if USE_DRAWSTUFF
561 public void startvisualization(object o)
562 {
563 ds.Functions fn;
564 fn.version = ds.VERSION;
565 fn.start = new ds.CallbackFunction(start);
566 fn.step = new ds.CallbackFunction(step);
567 fn.command = new ds.CallbackFunction(command);
568 fn.stop = null;
569 fn.path_to_textures = "./textures";
570 string[] args = new string[0];
571 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
572 }
573#endif
574
575 // Initialize the mesh plugin
576 public override void Initialise(IMesher meshmerizer, IConfigSource config)
577 {
578 InitializeExtraStats();
579
580 mesher = meshmerizer;
581 m_config = config;
582 // Defaults
583
584 if (Environment.OSVersion.Platform == PlatformID.Unix)
585 {
586 avPIDD = 3200.0f;
587 avPIDP = 1400.0f;
588 avStandupTensor = 2000000f;
589 }
590 else
591 {
592 avPIDD = 2200.0f;
593 avPIDP = 900.0f;
594 avStandupTensor = 550000f;
595 }
596
597 int contactsPerCollision = 80;
598
599 if (m_config != null)
600 {
601 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
602 if (physicsconfig != null)
603 {
604 CollectStats = physicsconfig.GetBoolean("collect_stats", false);
605
606 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
607 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
608 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
609
610 float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
611 AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
612 if (AvatarTerminalVelocity != avatarTerminalVelocity)
613 {
614 m_log.WarnFormat(
615 "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}",
616 avatarTerminalVelocity, AvatarTerminalVelocity);
617 }
618
619 worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
620 worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
621
622 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
623 smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
624 smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
625
626 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
627
628 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
629 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
630 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
631
632 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
633 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
634 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
635
636 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
637 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
638
639 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
640 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
641
642 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
643 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
644
645 avDensity = physicsconfig.GetFloat("av_density", 80f);
646// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
647 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
648 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
649 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
650 avplanted = physicsconfig.GetBoolean("av_planted", false);
651 av_av_collisions_off = physicsconfig.GetBoolean("av_av_collisions_off", false);
652
653 IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
654
655 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
656
657 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
658 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
659 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
660
661 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
662 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
663
664 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
665 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
666
667 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
668 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
669 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
670 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
671 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
672
673
674
675 if (Environment.OSVersion.Platform == PlatformID.Unix)
676 {
677 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
678 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
679 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
680 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
681 }
682 else
683 {
684 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
685 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
686 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
687 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
688 }
689
690 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
691 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
692 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
693
694 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
695 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
696 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
697 }
698 }
699
700 contacts = new d.ContactGeom[contactsPerCollision];
701
702 staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
703
704 // Centeral contact friction and bounce
705 // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
706 // an avatar falls through in Z but not in X or Y when walking on a prim.
707 contact.surface.mode |= d.ContactFlags.SoftERP;
708 contact.surface.mu = nmAvatarObjectContactFriction;
709 contact.surface.bounce = nmAvatarObjectContactBounce;
710 contact.surface.soft_cfm = 0.010f;
711 contact.surface.soft_erp = 0.010f;
712
713 // Terrain contact friction and Bounce
714 // This is the *non* moving version. Use this when an avatar
715 // isn't moving to keep it in place better
716 TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
717 TerrainContact.surface.mu = nmTerrainContactFriction;
718 TerrainContact.surface.bounce = nmTerrainContactBounce;
719 TerrainContact.surface.soft_erp = nmTerrainContactERP;
720
721 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
722 WaterContact.surface.mu = 0f; // No friction
723 WaterContact.surface.bounce = 0.0f; // No bounce
724 WaterContact.surface.soft_cfm = 0.010f;
725 WaterContact.surface.soft_erp = 0.010f;
726
727 // Prim contact friction and bounce
728 // THis is the *non* moving version of friction and bounce
729 // Use this when an avatar comes in contact with a prim
730 // and is moving
731 AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
732 AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
733
734 // Terrain contact friction bounce and various error correcting calculations
735 // Use this when an avatar is in contact with the terrain and moving.
736 AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
737 AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
738 AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
739 AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
740
741 /*
742 <summary></summary>
743 Stone = 0,
744 /// <summary></summary>
745 Metal = 1,
746 /// <summary></summary>
747 Glass = 2,
748 /// <summary></summary>
749 Wood = 3,
750 /// <summary></summary>
751 Flesh = 4,
752 /// <summary></summary>
753 Plastic = 5,
754 /// <summary></summary>
755 Rubber = 6
756 */
757
758 m_materialContacts = new d.Contact[7,2];
759
760 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
761 m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
762 m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
763 m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
764 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
765 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
766
767 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
768 m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
769 m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
770 m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
771 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
772 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
773
774 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
775 m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
776 m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
777 m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
778 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
779 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
780
781 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
782 m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
783 m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
784 m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
785 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
786 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
787
788 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
789 m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
790 m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
791 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
792 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
793 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
794
795 /*
796 private float nmAvatarObjectContactFriction = 250f;
797 private float nmAvatarObjectContactBounce = 0.1f;
798
799 private float mAvatarObjectContactFriction = 75f;
800 private float mAvatarObjectContactBounce = 0.1f;
801 */
802 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
803 m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
804 m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
805 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
806 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
807 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
808
809 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
810 m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
811 m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
812 m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
813 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
814 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
815
816 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
817 m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
818 m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
819 m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
820 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
821 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
822
823 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
824 m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
825 m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
826 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
827 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
828 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
829
830 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
831 m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
832 m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
833 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
834 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
835 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
836
837 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
838 m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
839 m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
840 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
841 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
842 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
843
844 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
845 m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
846 m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
847 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
848 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
849 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
850
851 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
852 m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
853 m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
854 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
855 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
856 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
857
858 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
859 m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
860 m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
861 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
862 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
863 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
864
865 d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
866
867 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
868
869 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
870 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
871
872 d.WorldSetLinearDamping(world, 256f);
873 d.WorldSetAngularDamping(world, 256f);
874 d.WorldSetAngularDampingThreshold(world, 256f);
875 d.WorldSetLinearDampingThreshold(world, 256f);
876 d.WorldSetMaxAngularSpeed(world, 256f);
877
878 // Set how many steps we go without running collision testing
879 // This is in addition to the step size.
880 // Essentially Steps * m_physicsiterations
881 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
882 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
883
884 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
885 {
886 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
887 {
888 staticPrimspace[i, j] = IntPtr.Zero;
889 }
890 }
891
892 _worldInitialized = true;
893 }
894
895// internal void waitForSpaceUnlock(IntPtr space)
896// {
897// //if (space != IntPtr.Zero)
898// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
899// }
900
901// /// <summary>
902// /// Debug space message for printing the space that a prim/avatar is in.
903// /// </summary>
904// /// <param name="pos"></param>
905// /// <returns>Returns which split up space the given position is in.</returns>
906// public string whichspaceamIin(Vector3 pos)
907// {
908// return calculateSpaceForGeom(pos).ToString();
909// }
910
911 #region Collision Detection
912
913 /// <summary>
914 /// Collides two geometries.
915 /// </summary>
916 /// <returns></returns>
917 /// <param name='geom1'></param>
918 /// <param name='geom2'>/param>
919 /// <param name='maxContacts'></param>
920 /// <param name='contactsArray'></param>
921 /// <param name='contactGeomSize'></param>
922 private int CollideGeoms(
923 IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
924 {
925 int count;
926
927 lock (OdeScene.UniversalColliderSyncObject)
928 {
929 // We do this inside the lock so that we don't count any delay in acquiring it
930 if (CollectStats)
931 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
932
933 count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
934 }
935
936 // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
937 // negligable
938 if (CollectStats)
939 m_stats[ODENativeGeomCollisionFrameMsStatName]
940 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
941
942 return count;
943 }
944
945 /// <summary>
946 /// Collide two spaces or a space and a geometry.
947 /// </summary>
948 /// <param name='space1'></param>
949 /// <param name='space2'>/param>
950 /// <param name='data'></param>
951 private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
952 {
953 if (CollectStats)
954 {
955 m_inCollisionTiming = true;
956 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
957 }
958
959 d.SpaceCollide2(space1, space2, data, nearCallback);
960
961 if (CollectStats && m_inCollisionTiming)
962 {
963 m_stats[ODENativeSpaceCollisionFrameMsStatName]
964 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
965 m_inCollisionTiming = false;
966 }
967 }
968
969 /// <summary>
970 /// This is our near callback. A geometry is near a body
971 /// </summary>
972 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
973 /// <param name="g1">a geometry or space</param>
974 /// <param name="g2">another geometry or space</param>
975 private void near(IntPtr space, IntPtr g1, IntPtr g2)
976 {
977 if (CollectStats && m_inCollisionTiming)
978 {
979 m_stats[ODENativeSpaceCollisionFrameMsStatName]
980 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
981 m_inCollisionTiming = false;
982 }
983
984// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
985 // no lock here! It's invoked from within Simulate(), which is thread-locked
986
987 // Test if we're colliding a geom with a space.
988 // If so we have to drill down into the space recursively
989
990 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
991 {
992 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
993 return;
994
995 // Separating static prim geometry spaces.
996 // We'll be calling near recursivly if one
997 // of them is a space to find all of the
998 // contact points in the space
999 try
1000 {
1001 CollideSpaces(g1, g2, IntPtr.Zero);
1002 }
1003 catch (AccessViolationException)
1004 {
1005 m_log.Error("[ODE SCENE]: Unable to collide test a space");
1006 return;
1007 }
1008 //Colliding a space or a geom with a space or a geom. so drill down
1009
1010 //Collide all geoms in each space..
1011 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
1012 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
1013 return;
1014 }
1015
1016 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
1017 return;
1018
1019 IntPtr b1 = d.GeomGetBody(g1);
1020 IntPtr b2 = d.GeomGetBody(g2);
1021
1022 // d.GeomClassID id = d.GeomGetClass(g1);
1023
1024 String name1 = null;
1025 String name2 = null;
1026
1027 if (!geom_name_map.TryGetValue(g1, out name1))
1028 {
1029 name1 = "null";
1030 }
1031 if (!geom_name_map.TryGetValue(g2, out name2))
1032 {
1033 name2 = "null";
1034 }
1035
1036 //if (id == d.GeomClassId.TriMeshClass)
1037 //{
1038 // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
1039 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1040 //}
1041
1042 // Figure out how many contact points we have
1043 int count = 0;
1044
1045 try
1046 {
1047 // Colliding Geom To Geom
1048 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
1049
1050 if (g1 == g2)
1051 return; // Can't collide with yourself
1052
1053 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
1054 return;
1055
1056 count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf);
1057
1058 // All code after this is only relevant if we have any collisions
1059 if (count <= 0)
1060 return;
1061
1062 if (count > contacts.Length)
1063 m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
1064 }
1065 catch (SEHException)
1066 {
1067 m_log.Error(
1068 "[ODE SCENE]: 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.");
1069 base.TriggerPhysicsBasedRestart();
1070 }
1071 catch (Exception e)
1072 {
1073 m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message);
1074 return;
1075 }
1076
1077 PhysicsActor p1;
1078 PhysicsActor p2;
1079
1080 p1ExpectedPoints = 0;
1081 p2ExpectedPoints = 0;
1082
1083 if (!actor_name_map.TryGetValue(g1, out p1))
1084 {
1085 p1 = PANull;
1086 }
1087
1088 if (!actor_name_map.TryGetValue(g2, out p2))
1089 {
1090 p2 = PANull;
1091 }
1092
1093 ContactPoint maxDepthContact = new ContactPoint();
1094 if (p1.CollisionScore + count >= float.MaxValue)
1095 p1.CollisionScore = 0;
1096 p1.CollisionScore += count;
1097
1098 if (p2.CollisionScore + count >= float.MaxValue)
1099 p2.CollisionScore = 0;
1100 p2.CollisionScore += count;
1101
1102 for (int i = 0; i < count; i++)
1103 {
1104 d.ContactGeom curContact = contacts[i];
1105
1106 if (curContact.depth > maxDepthContact.PenetrationDepth)
1107 {
1108 maxDepthContact = new ContactPoint(
1109 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
1110 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
1111 curContact.depth
1112 );
1113 }
1114
1115 //m_log.Warn("[CCOUNT]: " + count);
1116 IntPtr joint;
1117 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
1118 // allows us to have different settings
1119
1120 // We only need to test p2 for 'jump crouch purposes'
1121 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
1122 {
1123 // Testing if the collision is at the feet of the avatar
1124
1125 //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));
1126 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
1127 p2.IsColliding = true;
1128 }
1129 else
1130 {
1131 p2.IsColliding = true;
1132 }
1133
1134 //if ((framecount % m_returncollisions) == 0)
1135
1136 switch (p1.PhysicsActorType)
1137 {
1138 case (int)ActorTypes.Agent:
1139 p1ExpectedPoints = avatarExpectedContacts;
1140 p2.CollidingObj = true;
1141 break;
1142 case (int)ActorTypes.Prim:
1143 if (p1 != null && p1 is OdePrim)
1144 p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
1145
1146 if (p2.Velocity.LengthSquared() > 0.0f)
1147 p2.CollidingObj = true;
1148 break;
1149 case (int)ActorTypes.Unknown:
1150 p2.CollidingGround = true;
1151 break;
1152 default:
1153 p2.CollidingGround = true;
1154 break;
1155 }
1156
1157 // we don't want prim or avatar to explode
1158
1159 #region InterPenetration Handling - Unintended physics explosions
1160# region disabled code1
1161
1162 if (curContact.depth >= 0.08f)
1163 {
1164 //This is disabled at the moment only because it needs more tweaking
1165 //It will eventually be uncommented
1166 /*
1167 if (contact.depth >= 1.00f)
1168 {
1169 //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
1170 }
1171
1172 //If you interpenetrate a prim with an agent
1173 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1174 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
1175 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1176 p2.PhysicsActorType == (int) ActorTypes.Prim))
1177 {
1178
1179 //contact.depth = contact.depth * 4.15f;
1180 /*
1181 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
1182 {
1183 p2.CollidingObj = true;
1184 contact.depth = 0.003f;
1185 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
1186 OdeCharacter character = (OdeCharacter) p2;
1187 character.SetPidStatus(true);
1188 contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
1189
1190 }
1191 else
1192 {
1193
1194 //contact.depth = 0.0000000f;
1195 }
1196 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
1197 {
1198
1199 p1.CollidingObj = true;
1200 contact.depth = 0.003f;
1201 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
1202 contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
1203 OdeCharacter character = (OdeCharacter)p1;
1204 character.SetPidStatus(true);
1205 }
1206 else
1207 {
1208
1209 //contact.depth = 0.0000000f;
1210 }
1211
1212
1213
1214 }
1215*/
1216 // If you interpenetrate a prim with another prim
1217 /*
1218 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
1219 {
1220 #region disabledcode2
1221 //OdePrim op1 = (OdePrim)p1;
1222 //OdePrim op2 = (OdePrim)p2;
1223 //op1.m_collisionscore++;
1224 //op2.m_collisionscore++;
1225
1226 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
1227 //{
1228 //op1.m_taintdisable = true;
1229 //AddPhysicsActorTaint(p1);
1230 //op2.m_taintdisable = true;
1231 //AddPhysicsActorTaint(p2);
1232 //}
1233
1234 //if (contact.depth >= 0.25f)
1235 //{
1236 // Don't collide, one or both prim will expld.
1237
1238 //op1.m_interpenetrationcount++;
1239 //op2.m_interpenetrationcount++;
1240 //interpenetrations_before_disable = 200;
1241 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
1242 //{
1243 //op1.m_taintdisable = true;
1244 //AddPhysicsActorTaint(p1);
1245 //}
1246 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
1247 //{
1248 // op2.m_taintdisable = true;
1249 //AddPhysicsActorTaint(p2);
1250 //}
1251
1252 //contact.depth = contact.depth / 8f;
1253 //contact.normal = new d.Vector3(0, 0, 1);
1254 //}
1255 //if (op1.m_disabled || op2.m_disabled)
1256 //{
1257 //Manually disabled objects stay disabled
1258 //contact.depth = 0f;
1259 //}
1260 #endregion
1261 }
1262 */
1263#endregion
1264 if (curContact.depth >= 1.00f)
1265 {
1266 //m_log.Info("[P]: " + contact.depth.ToString());
1267 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1268 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
1269 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1270 p2.PhysicsActorType == (int) ActorTypes.Unknown))
1271 {
1272 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
1273 {
1274 if (p2 is OdeCharacter)
1275 {
1276 OdeCharacter character = (OdeCharacter) p2;
1277
1278 //p2.CollidingObj = true;
1279 curContact.depth = 0.00000003f;
1280 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
1281 curContact.pos =
1282 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1283 curContact.pos.Y + (p1.Size.Y/2),
1284 curContact.pos.Z + (p1.Size.Z/2));
1285 character.SetPidStatus(true);
1286 }
1287 }
1288
1289 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
1290 {
1291 if (p1 is OdeCharacter)
1292 {
1293 OdeCharacter character = (OdeCharacter) p1;
1294
1295 //p2.CollidingObj = true;
1296 curContact.depth = 0.00000003f;
1297 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1298 curContact.pos =
1299 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1300 curContact.pos.Y + (p1.Size.Y/2),
1301 curContact.pos.Z + (p1.Size.Z/2));
1302 character.SetPidStatus(true);
1303 }
1304 }
1305 }
1306 }
1307 }
1308
1309 #endregion
1310
1311 // Logic for collision handling
1312 // Note, that if *all* contacts are skipped (VolumeDetect)
1313 // The prim still detects (and forwards) collision events but
1314 // appears to be phantom for the world
1315 Boolean skipThisContact = false;
1316
1317 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1318 skipThisContact = true; // No collision on volume detect prims
1319
1320 if (av_av_collisions_off)
1321 if ((p1 is OdeCharacter) && (p2 is OdeCharacter))
1322 skipThisContact = true;
1323
1324 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1325 skipThisContact = true; // No collision on volume detect prims
1326
1327 if (!skipThisContact && curContact.depth < 0f)
1328 skipThisContact = true;
1329
1330 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1331 skipThisContact = true;
1332
1333 const int maxContactsbeforedeath = 4000;
1334 joint = IntPtr.Zero;
1335
1336 if (!skipThisContact)
1337 {
1338 _perloopContact.Add(curContact);
1339
1340 if (name1 == "Terrain" || name2 == "Terrain")
1341 {
1342 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
1343 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1344 {
1345 p2ExpectedPoints = avatarExpectedContacts;
1346 // Avatar is moving on terrain, use the movement terrain contact
1347 AvatarMovementTerrainContact.geom = curContact;
1348
1349 if (m_global_contactcount < maxContactsbeforedeath)
1350 {
1351 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1352 m_global_contactcount++;
1353 }
1354 }
1355 else
1356 {
1357 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1358 {
1359 p2ExpectedPoints = avatarExpectedContacts;
1360 // Avatar is standing on terrain, use the non moving terrain contact
1361 TerrainContact.geom = curContact;
1362
1363 if (m_global_contactcount < maxContactsbeforedeath)
1364 {
1365 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1366 m_global_contactcount++;
1367 }
1368 }
1369 else
1370 {
1371 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1372 {
1373 // prim prim contact
1374 // int pj294950 = 0;
1375 int movintYN = 0;
1376 int material = (int) Material.Wood;
1377 // prim terrain contact
1378 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1379 {
1380 movintYN = 1;
1381 }
1382
1383 if (p2 is OdePrim)
1384 {
1385 material = ((OdePrim) p2).m_material;
1386 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1387 }
1388
1389 // Unnessesary because p1 is defined above
1390 //if (p1 is OdePrim)
1391 // {
1392 // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
1393 // }
1394 //m_log.DebugFormat("Material: {0}", material);
1395
1396 m_materialContacts[material, movintYN].geom = curContact;
1397
1398 if (m_global_contactcount < maxContactsbeforedeath)
1399 {
1400 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1401 m_global_contactcount++;
1402 }
1403 }
1404 else
1405 {
1406 int movintYN = 0;
1407 // prim terrain contact
1408 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1409 {
1410 movintYN = 1;
1411 }
1412
1413 int material = (int)Material.Wood;
1414
1415 if (p2 is OdePrim)
1416 {
1417 material = ((OdePrim)p2).m_material;
1418 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1419 }
1420
1421 //m_log.DebugFormat("Material: {0}", material);
1422 m_materialContacts[material, movintYN].geom = curContact;
1423
1424 if (m_global_contactcount < maxContactsbeforedeath)
1425 {
1426 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1427 m_global_contactcount++;
1428 }
1429 }
1430 }
1431 }
1432 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1433 //{
1434 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1435 //}
1436 }
1437 else if (name1 == "Water" || name2 == "Water")
1438 {
1439 /*
1440 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1441 {
1442 }
1443 else
1444 {
1445 }
1446 */
1447 //WaterContact.surface.soft_cfm = 0.0000f;
1448 //WaterContact.surface.soft_erp = 0.00000f;
1449 if (curContact.depth > 0.1f)
1450 {
1451 curContact.depth *= 52;
1452 //contact.normal = new d.Vector3(0, 0, 1);
1453 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1454 }
1455
1456 WaterContact.geom = curContact;
1457
1458 if (m_global_contactcount < maxContactsbeforedeath)
1459 {
1460 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1461 m_global_contactcount++;
1462 }
1463 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
1464 }
1465 else
1466 {
1467 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1468 {
1469 p2ExpectedPoints = avatarExpectedContacts;
1470 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1471 {
1472 // Avatar is moving on a prim, use the Movement prim contact
1473 AvatarMovementprimContact.geom = curContact;
1474
1475 if (m_global_contactcount < maxContactsbeforedeath)
1476 {
1477 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1478 m_global_contactcount++;
1479 }
1480 }
1481 else
1482 {
1483 // Avatar is standing still on a prim, use the non movement contact
1484 contact.geom = curContact;
1485
1486 if (m_global_contactcount < maxContactsbeforedeath)
1487 {
1488 joint = d.JointCreateContact(world, contactgroup, ref contact);
1489 m_global_contactcount++;
1490 }
1491 }
1492 }
1493 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1494 {
1495 //p1.PhysicsActorType
1496 int material = (int)Material.Wood;
1497
1498 if (p2 is OdePrim)
1499 {
1500 material = ((OdePrim)p2).m_material;
1501 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1502 }
1503
1504 //m_log.DebugFormat("Material: {0}", material);
1505 m_materialContacts[material, 0].geom = curContact;
1506
1507 if (m_global_contactcount < maxContactsbeforedeath)
1508 {
1509 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1510 m_global_contactcount++;
1511 }
1512 }
1513 }
1514
1515 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1516 {
1517 d.JointAttach(joint, b1, b2);
1518 m_global_contactcount++;
1519 }
1520 }
1521
1522 collision_accounting_events(p1, p2, maxDepthContact);
1523
1524 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
1525 {
1526 // If there are more then 3 contact points, it's likely
1527 // that we've got a pile of objects, so ...
1528 // We don't want to send out hundreds of terse updates over and over again
1529 // so lets throttle them and send them again after it's somewhat sorted out.
1530 p2.ThrottleUpdates = true;
1531 }
1532 //m_log.Debug(count.ToString());
1533 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1534 }
1535 }
1536
1537 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1538 {
1539 if (!m_filterCollisions)
1540 return false;
1541
1542 bool result = false;
1543
1544 ActorTypes at = (ActorTypes)atype;
1545
1546 foreach (d.ContactGeom contact in _perloopContact)
1547 {
1548 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1549 //{
1550 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1551 if (at == ActorTypes.Agent)
1552 {
1553 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
1554 && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
1555 && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1556 {
1557 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1558 {
1559 //contactGeom.depth *= .00005f;
1560 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1561 // 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));
1562 result = true;
1563 break;
1564 }
1565// else
1566// {
1567// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1568// }
1569 }
1570// else
1571// {
1572// //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));
1573// //int i = 0;
1574// }
1575 }
1576 else if (at == ActorTypes.Prim)
1577 {
1578 //d.AABB aabb1 = new d.AABB();
1579 //d.AABB aabb2 = new d.AABB();
1580
1581 //d.GeomGetAABB(contactGeom.g2, out aabb2);
1582 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1583 //aabb1.
1584 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)))
1585 {
1586 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1587 {
1588 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1589 {
1590 result = true;
1591 break;
1592 }
1593 }
1594 //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1595 //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));
1596 }
1597 }
1598 }
1599
1600 return result;
1601 }
1602
1603 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1604 {
1605 // obj1LocalID = 0;
1606 //returncollisions = false;
1607 obj2LocalID = 0;
1608 //ctype = 0;
1609 //cStartStop = 0;
1610 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1611 return;
1612
1613 switch ((ActorTypes)p2.PhysicsActorType)
1614 {
1615 case ActorTypes.Agent:
1616 cc2 = (OdeCharacter)p2;
1617
1618 // obj1LocalID = cc2.m_localID;
1619 switch ((ActorTypes)p1.PhysicsActorType)
1620 {
1621 case ActorTypes.Agent:
1622 cc1 = (OdeCharacter)p1;
1623 obj2LocalID = cc1.LocalID;
1624 cc1.AddCollisionEvent(cc2.LocalID, contact);
1625 //ctype = (int)CollisionCategories.Character;
1626
1627 //if (cc1.CollidingObj)
1628 //cStartStop = (int)StatusIndicators.Generic;
1629 //else
1630 //cStartStop = (int)StatusIndicators.Start;
1631
1632 //returncollisions = true;
1633 break;
1634
1635 case ActorTypes.Prim:
1636 if (p1 is OdePrim)
1637 {
1638 cp1 = (OdePrim) p1;
1639 obj2LocalID = cp1.LocalID;
1640 cp1.AddCollisionEvent(cc2.LocalID, contact);
1641 }
1642 //ctype = (int)CollisionCategories.Geom;
1643
1644 //if (cp1.CollidingObj)
1645 //cStartStop = (int)StatusIndicators.Generic;
1646 //else
1647 //cStartStop = (int)StatusIndicators.Start;
1648
1649 //returncollisions = true;
1650 break;
1651
1652 case ActorTypes.Ground:
1653 case ActorTypes.Unknown:
1654 obj2LocalID = 0;
1655 //ctype = (int)CollisionCategories.Land;
1656 //returncollisions = true;
1657 break;
1658 }
1659
1660 cc2.AddCollisionEvent(obj2LocalID, contact);
1661 break;
1662
1663 case ActorTypes.Prim:
1664
1665 if (p2 is OdePrim)
1666 {
1667 cp2 = (OdePrim) p2;
1668
1669 // obj1LocalID = cp2.m_localID;
1670 switch ((ActorTypes) p1.PhysicsActorType)
1671 {
1672 case ActorTypes.Agent:
1673 if (p1 is OdeCharacter)
1674 {
1675 cc1 = (OdeCharacter) p1;
1676 obj2LocalID = cc1.LocalID;
1677 cc1.AddCollisionEvent(cp2.LocalID, contact);
1678 //ctype = (int)CollisionCategories.Character;
1679
1680 //if (cc1.CollidingObj)
1681 //cStartStop = (int)StatusIndicators.Generic;
1682 //else
1683 //cStartStop = (int)StatusIndicators.Start;
1684 //returncollisions = true;
1685 }
1686 break;
1687 case ActorTypes.Prim:
1688
1689 if (p1 is OdePrim)
1690 {
1691 cp1 = (OdePrim) p1;
1692 obj2LocalID = cp1.LocalID;
1693 cp1.AddCollisionEvent(cp2.LocalID, contact);
1694 //ctype = (int)CollisionCategories.Geom;
1695
1696 //if (cp1.CollidingObj)
1697 //cStartStop = (int)StatusIndicators.Generic;
1698 //else
1699 //cStartStop = (int)StatusIndicators.Start;
1700
1701 //returncollisions = true;
1702 }
1703 break;
1704
1705 case ActorTypes.Ground:
1706 case ActorTypes.Unknown:
1707 obj2LocalID = 0;
1708 //ctype = (int)CollisionCategories.Land;
1709
1710 //returncollisions = true;
1711 break;
1712 }
1713
1714 cp2.AddCollisionEvent(obj2LocalID, contact);
1715 }
1716 break;
1717 }
1718 //if (returncollisions)
1719 //{
1720
1721 //lock (m_storedCollisions)
1722 //{
1723 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1724 //if (m_storedCollisions.ContainsKey(cDictKey))
1725 //{
1726 //sCollisionData objd = m_storedCollisions[cDictKey];
1727 //objd.NumberOfCollisions += 1;
1728 //objd.lastframe = framecount;
1729 //m_storedCollisions[cDictKey] = objd;
1730 //}
1731 //else
1732 //{
1733 //sCollisionData objd = new sCollisionData();
1734 //objd.ColliderLocalId = obj1LocalID;
1735 //objd.CollidedWithLocalId = obj2LocalID;
1736 //objd.CollisionType = ctype;
1737 //objd.NumberOfCollisions = 1;
1738 //objd.lastframe = framecount;
1739 //objd.StatusIndicator = cStartStop;
1740 //m_storedCollisions.Add(cDictKey, objd);
1741 //}
1742 //}
1743 // }
1744 }
1745
1746 private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1747 {
1748 /* String name1 = null;
1749 String name2 = null;
1750
1751 if (!geom_name_map.TryGetValue(trimesh, out name1))
1752 {
1753 name1 = "null";
1754 }
1755 if (!geom_name_map.TryGetValue(refObject, out name2))
1756 {
1757 name2 = "null";
1758 }
1759
1760 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
1761 */
1762 return 1;
1763 }
1764
1765 private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1766 {
1767// String name1 = null;
1768// String name2 = null;
1769//
1770// if (!geom_name_map.TryGetValue(trimesh, out name1))
1771// {
1772// name1 = "null";
1773// }
1774//
1775// if (!geom_name_map.TryGetValue(refObject, out name2))
1776// {
1777// name2 = "null";
1778// }
1779
1780 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1781
1782 d.Vector3 v0 = new d.Vector3();
1783 d.Vector3 v1 = new d.Vector3();
1784 d.Vector3 v2 = new d.Vector3();
1785
1786 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1787 // 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);
1788
1789 return 1;
1790 }
1791
1792 /// <summary>
1793 /// This is our collision testing routine in ODE
1794 /// </summary>
1795 private void collision_optimized()
1796 {
1797 _perloopContact.Clear();
1798
1799 foreach (OdeCharacter chr in _characters)
1800 {
1801 // Reset the collision values to false
1802 // since we don't know if we're colliding yet
1803 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1804 continue;
1805
1806 chr.IsColliding = false;
1807 chr.CollidingGround = false;
1808 chr.CollidingObj = false;
1809
1810 // Test the avatar's geometry for collision with the space
1811 // This will return near and the space that they are the closest to
1812 // And we'll run this again against the avatar and the space segment
1813 // This will return with a bunch of possible objects in the space segment
1814 // and we'll run it again on all of them.
1815 try
1816 {
1817 CollideSpaces(space, chr.Shell, IntPtr.Zero);
1818 }
1819 catch (AccessViolationException)
1820 {
1821 m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name);
1822 }
1823
1824 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1825 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1826 //{
1827 //chr.Position.Z = terrainheight + 10.0f;
1828 //forcedZ = true;
1829 //}
1830 }
1831
1832 if (CollectStats)
1833 {
1834 m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
1835 m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
1836 }
1837
1838 List<OdePrim> removeprims = null;
1839 foreach (OdePrim chr in _activeprims)
1840 {
1841 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
1842 {
1843 try
1844 {
1845 lock (chr)
1846 {
1847 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1848 {
1849 CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
1850 }
1851 else
1852 {
1853 if (removeprims == null)
1854 {
1855 removeprims = new List<OdePrim>();
1856 }
1857 removeprims.Add(chr);
1858 m_log.Error(
1859 "[ODE SCENE]: 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!");
1860 }
1861 }
1862 }
1863 catch (AccessViolationException)
1864 {
1865 m_log.Error("[ODE SCENE]: Unable to space collide");
1866 }
1867 }
1868 }
1869
1870 if (CollectStats)
1871 m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
1872
1873 if (removeprims != null)
1874 {
1875 foreach (OdePrim chr in removeprims)
1876 {
1877 _activeprims.Remove(chr);
1878 }
1879 }
1880 }
1881
1882 #endregion
1883
1884 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1885 {
1886 m_worldOffset = offset;
1887 WorldExtents = new Vector2(extents.X, extents.Y);
1888 m_parentScene = pScene;
1889 }
1890
1891 // Recovered for use by fly height. Kitto Flora
1892 internal float GetTerrainHeightAtXY(float x, float y)
1893 {
1894 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1895 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1896
1897 IntPtr heightFieldGeom = IntPtr.Zero;
1898
1899 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1900 {
1901 if (heightFieldGeom != IntPtr.Zero)
1902 {
1903 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1904 {
1905
1906 int index;
1907
1908
1909 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1910 (int)x < 0.001f || (int)y < 0.001f)
1911 return 0;
1912
1913 x = x - offsetX;
1914 y = y - offsetY;
1915
1916 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1917
1918 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1919 {
1920 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1921 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1922 }
1923
1924 else
1925 return 0f;
1926 }
1927 else
1928 {
1929 return 0f;
1930 }
1931
1932 }
1933 else
1934 {
1935 return 0f;
1936 }
1937
1938 }
1939 else
1940 {
1941 return 0f;
1942 }
1943 }
1944// End recovered. Kitto Flora
1945
1946 /// <summary>
1947 /// Add actor to the list that should receive collision events in the simulate loop.
1948 /// </summary>
1949 /// <param name="obj"></param>
1950 internal void AddCollisionEventReporting(PhysicsActor obj)
1951 {
1952// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
1953
1954 lock (m_collisionEventActorsChanges)
1955 m_collisionEventActorsChanges[obj.LocalID] = obj;
1956 }
1957
1958 /// <summary>
1959 /// Remove actor from the list that should receive collision events in the simulate loop.
1960 /// </summary>
1961 /// <param name="obj"></param>
1962 internal void RemoveCollisionEventReporting(PhysicsActor obj)
1963 {
1964// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
1965
1966 lock (m_collisionEventActorsChanges)
1967 m_collisionEventActorsChanges[obj.LocalID] = null;
1968 }
1969
1970 #region Add/Remove Entities
1971
1972 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
1973 {
1974 OdeCharacter newAv
1975 = new OdeCharacter(
1976 avName, this, position, velocity, size, avPIDD, avPIDP,
1977 avCapRadius, avStandupTensor, avDensity,
1978 avMovementDivisorWalk, avMovementDivisorRun);
1979
1980 newAv.Flying = isFlying;
1981 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1982 newAv.m_avatarplanted = avplanted;
1983
1984 return newAv;
1985 }
1986
1987 public override void RemoveAvatar(PhysicsActor actor)
1988 {
1989// m_log.DebugFormat(
1990// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
1991// actor.Name, actor.LocalID, Name);
1992
1993 ((OdeCharacter) actor).Destroy();
1994 }
1995
1996 internal void AddCharacter(OdeCharacter chr)
1997 {
1998 chr.m_avatarplanted = avplanted;
1999 if (!_characters.Contains(chr))
2000 {
2001 _characters.Add(chr);
2002
2003// m_log.DebugFormat(
2004// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}",
2005// chr.Name, chr.LocalID, Name, _characters.Count);
2006
2007 if (chr.bad)
2008 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
2009 }
2010 else
2011 {
2012 m_log.ErrorFormat(
2013 "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!",
2014 chr.Name, chr.LocalID);
2015 }
2016 }
2017
2018 internal void RemoveCharacter(OdeCharacter chr)
2019 {
2020 if (_characters.Contains(chr))
2021 {
2022 _characters.Remove(chr);
2023
2024// m_log.DebugFormat(
2025// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}",
2026// chr.Name, chr.LocalID, Name, _characters.Count);
2027 }
2028 else
2029 {
2030 m_log.ErrorFormat(
2031 "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!",
2032 chr.Name, chr.LocalID);
2033 }
2034 }
2035
2036 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
2037 PrimitiveBaseShape pbs, bool isphysical, uint localID)
2038 {
2039 Vector3 pos = position;
2040 Vector3 siz = size;
2041 Quaternion rot = rotation;
2042
2043 OdePrim newPrim;
2044 lock (OdeLock)
2045 {
2046 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
2047
2048 lock (_prims)
2049 _prims.Add(newPrim);
2050 }
2051 newPrim.LocalID = localID;
2052 return newPrim;
2053 }
2054
2055 /// <summary>
2056 /// Make this prim subject to physics.
2057 /// </summary>
2058 /// <param name="prim"></param>
2059 internal void ActivatePrim(OdePrim prim)
2060 {
2061 // adds active prim.. (ones that should be iterated over in collisions_optimized
2062 if (!_activeprims.Contains(prim))
2063 _activeprims.Add(prim);
2064 //else
2065 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
2066 }
2067
2068 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
2069 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
2070 {
2071// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
2072
2073 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
2074 }
2075
2076 public override float TimeDilation
2077 {
2078 get { return m_timeDilation; }
2079 }
2080
2081 public override bool SupportsNINJAJoints
2082 {
2083 get { return m_NINJA_physics_joints_enabled; }
2084 }
2085
2086 // internal utility function: must be called within a lock (OdeLock)
2087 private void InternalAddActiveJoint(PhysicsJoint joint)
2088 {
2089 activeJoints.Add(joint);
2090 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
2091 }
2092
2093 // internal utility function: must be called within a lock (OdeLock)
2094 private void InternalAddPendingJoint(OdePhysicsJoint joint)
2095 {
2096 pendingJoints.Add(joint);
2097 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
2098 }
2099
2100 // internal utility function: must be called within a lock (OdeLock)
2101 private void InternalRemovePendingJoint(PhysicsJoint joint)
2102 {
2103 pendingJoints.Remove(joint);
2104 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
2105 }
2106
2107 // internal utility function: must be called within a lock (OdeLock)
2108 private void InternalRemoveActiveJoint(PhysicsJoint joint)
2109 {
2110 activeJoints.Remove(joint);
2111 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
2112 }
2113
2114 public override void DumpJointInfo()
2115 {
2116 string hdr = "[NINJA] JOINTINFO: ";
2117 foreach (PhysicsJoint j in pendingJoints)
2118 {
2119 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2120 }
2121 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
2122 foreach (string jointName in SOPName_to_pendingJoint.Keys)
2123 {
2124 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
2125 }
2126 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
2127 foreach (PhysicsJoint j in activeJoints)
2128 {
2129 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2130 }
2131 m_log.Debug(hdr + activeJoints.Count + " total active joints");
2132 foreach (string jointName in SOPName_to_activeJoint.Keys)
2133 {
2134 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
2135 }
2136 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
2137
2138 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
2139 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
2140 foreach (string actorName in joints_connecting_actor.Keys)
2141 {
2142 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
2143 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
2144 {
2145 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2146 }
2147 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
2148 }
2149 }
2150
2151 public override void RequestJointDeletion(string ObjectNameInScene)
2152 {
2153 lock (externalJointRequestsLock)
2154 {
2155 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
2156 {
2157 requestedJointsToBeDeleted.Add(ObjectNameInScene);
2158 }
2159 }
2160 }
2161
2162 private void DeleteRequestedJoints()
2163 {
2164 List<string> myRequestedJointsToBeDeleted;
2165 lock (externalJointRequestsLock)
2166 {
2167 // make a local copy of the shared list for processing (threading issues)
2168 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
2169 }
2170
2171 foreach (string jointName in myRequestedJointsToBeDeleted)
2172 {
2173 lock (OdeLock)
2174 {
2175 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
2176 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
2177 {
2178 OdePhysicsJoint joint = null;
2179 if (SOPName_to_activeJoint.ContainsKey(jointName))
2180 {
2181 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
2182 InternalRemoveActiveJoint(joint);
2183 }
2184 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
2185 {
2186 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
2187 InternalRemovePendingJoint(joint);
2188 }
2189
2190 if (joint != null)
2191 {
2192 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
2193 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2194 {
2195 string bodyName = joint.BodyNames[iBodyName];
2196 if (bodyName != "NULL")
2197 {
2198 joints_connecting_actor[bodyName].Remove(joint);
2199 if (joints_connecting_actor[bodyName].Count == 0)
2200 {
2201 joints_connecting_actor.Remove(bodyName);
2202 }
2203 }
2204 }
2205
2206 DoJointDeactivated(joint);
2207 if (joint.jointID != IntPtr.Zero)
2208 {
2209 d.JointDestroy(joint.jointID);
2210 joint.jointID = IntPtr.Zero;
2211 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
2212 }
2213 else
2214 {
2215 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
2216 }
2217 }
2218 else
2219 {
2220 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
2221 }
2222 }
2223 else
2224 {
2225 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
2226 }
2227 }
2228 }
2229
2230 // remove processed joints from the shared list
2231 lock (externalJointRequestsLock)
2232 {
2233 foreach (string jointName in myRequestedJointsToBeDeleted)
2234 {
2235 requestedJointsToBeDeleted.Remove(jointName);
2236 }
2237 }
2238 }
2239
2240 // for pending joints we don't know if their associated bodies exist yet or not.
2241 // the joint is actually created during processing of the taints
2242 private void CreateRequestedJoints()
2243 {
2244 List<PhysicsJoint> myRequestedJointsToBeCreated;
2245 lock (externalJointRequestsLock)
2246 {
2247 // make a local copy of the shared list for processing (threading issues)
2248 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
2249 }
2250
2251 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2252 {
2253 lock (OdeLock)
2254 {
2255 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
2256 {
2257 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);
2258 continue;
2259 }
2260 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
2261 {
2262 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);
2263 continue;
2264 }
2265
2266 InternalAddPendingJoint(joint as OdePhysicsJoint);
2267
2268 if (joint.BodyNames.Count >= 2)
2269 {
2270 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2271 {
2272 string bodyName = joint.BodyNames[iBodyName];
2273 if (bodyName != "NULL")
2274 {
2275 if (!joints_connecting_actor.ContainsKey(bodyName))
2276 {
2277 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
2278 }
2279 joints_connecting_actor[bodyName].Add(joint);
2280 }
2281 }
2282 }
2283 }
2284 }
2285
2286 // remove processed joints from shared list
2287 lock (externalJointRequestsLock)
2288 {
2289 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2290 {
2291 requestedJointsToBeCreated.Remove(joint);
2292 }
2293 }
2294 }
2295
2296 /// <summary>
2297 /// Add a request for joint creation.
2298 /// </summary>
2299 /// <remarks>
2300 /// this joint will just be added to a waiting list that is NOT processed during the main
2301 /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2302 /// </remarks>
2303 /// <param name="objectNameInScene"></param>
2304 /// <param name="jointType"></param>
2305 /// <param name="position"></param>
2306 /// <param name="rotation"></param>
2307 /// <param name="parms"></param>
2308 /// <param name="bodyNames"></param>
2309 /// <param name="trackedBodyName"></param>
2310 /// <param name="localRotation"></param>
2311 /// <returns></returns>
2312 public override PhysicsJoint RequestJointCreation(
2313 string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2314 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2315 {
2316 OdePhysicsJoint joint = new OdePhysicsJoint();
2317 joint.ObjectNameInScene = objectNameInScene;
2318 joint.Type = jointType;
2319 joint.Position = position;
2320 joint.Rotation = rotation;
2321 joint.RawParams = parms;
2322 joint.BodyNames = new List<string>(bodyNames);
2323 joint.TrackedBodyName = trackedBodyName;
2324 joint.LocalRotation = localRotation;
2325 joint.jointID = IntPtr.Zero;
2326 joint.ErrorMessageCount = 0;
2327
2328 lock (externalJointRequestsLock)
2329 {
2330 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2331 {
2332 requestedJointsToBeCreated.Add(joint);
2333 }
2334 }
2335
2336 return joint;
2337 }
2338
2339 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2340 {
2341 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2342 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2343 {
2344 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2345 //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)
2346 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2347 {
2348 jointsToRemove.Add(j);
2349 }
2350 foreach (PhysicsJoint j in jointsToRemove)
2351 {
2352 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2353 RequestJointDeletion(j.ObjectNameInScene);
2354 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2355 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)
2356 }
2357 }
2358 }
2359
2360 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2361 {
2362 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2363 lock (OdeLock)
2364 {
2365 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2366 RemoveAllJointsConnectedToActor(actor);
2367 }
2368 }
2369
2370 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2371 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2372 {
2373 Debug.Assert(joint.IsInPhysicsEngine);
2374 d.Vector3 pos = new d.Vector3();
2375
2376 if (!(joint is OdePhysicsJoint))
2377 {
2378 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2379 }
2380 else
2381 {
2382 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2383 switch (odeJoint.Type)
2384 {
2385 case PhysicsJointType.Ball:
2386 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2387 break;
2388 case PhysicsJointType.Hinge:
2389 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2390 break;
2391 }
2392 }
2393 return new Vector3(pos.X, pos.Y, pos.Z);
2394 }
2395
2396 /// <summary>
2397 /// Get joint axis.
2398 /// </summary>
2399 /// <remarks>
2400 /// normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2401 /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2402 /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2403 /// keeping track of the joint's original orientation relative to one of the involved bodies.
2404 /// </remarks>
2405 /// <param name="joint"></param>
2406 /// <returns></returns>
2407 public override Vector3 GetJointAxis(PhysicsJoint joint)
2408 {
2409 Debug.Assert(joint.IsInPhysicsEngine);
2410 d.Vector3 axis = new d.Vector3();
2411
2412 if (!(joint is OdePhysicsJoint))
2413 {
2414 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2415 }
2416 else
2417 {
2418 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2419 switch (odeJoint.Type)
2420 {
2421 case PhysicsJointType.Ball:
2422 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2423 break;
2424 case PhysicsJointType.Hinge:
2425 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2426 break;
2427 }
2428 }
2429 return new Vector3(axis.X, axis.Y, axis.Z);
2430 }
2431
2432 /// <summary>
2433 /// Stop this prim being subject to physics
2434 /// </summary>
2435 /// <param name="prim"></param>
2436 internal void DeactivatePrim(OdePrim prim)
2437 {
2438 _activeprims.Remove(prim);
2439 }
2440
2441 public override void RemovePrim(PhysicsActor prim)
2442 {
2443 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
2444 // removed in the next physics simulate pass.
2445 if (prim is OdePrim)
2446 {
2447 lock (OdeLock)
2448 {
2449 OdePrim p = (OdePrim) prim;
2450
2451 p.setPrimForRemoval();
2452 AddPhysicsActorTaint(prim);
2453 }
2454 }
2455 }
2456
2457 /// <summary>
2458 /// This is called from within simulate but outside the locked portion
2459 /// We need to do our own locking here
2460 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
2461 /// Simulate() -- justincc).
2462 ///
2463 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2464 ///
2465 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2466 /// that the space was using.
2467 /// </summary>
2468 /// <param name="prim"></param>
2469 internal void RemovePrimThreadLocked(OdePrim prim)
2470 {
2471// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
2472
2473 lock (prim)
2474 {
2475 RemoveCollisionEventReporting(prim);
2476
2477 if (prim.prim_geom != IntPtr.Zero)
2478 {
2479 prim.ResetTaints();
2480
2481 if (prim.IsPhysical)
2482 {
2483 prim.disableBody();
2484 if (prim.childPrim)
2485 {
2486 prim.childPrim = false;
2487 prim.Body = IntPtr.Zero;
2488 prim.m_disabled = true;
2489 prim.IsPhysical = false;
2490 }
2491
2492
2493 }
2494 // we don't want to remove the main space
2495
2496 // If the geometry is in the targetspace, remove it from the target space
2497 //m_log.Warn(prim.m_targetSpace);
2498
2499 //if (prim.m_targetSpace != IntPtr.Zero)
2500 //{
2501 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2502 //{
2503
2504 //if (d.GeomIsSpace(prim.m_targetSpace))
2505 //{
2506 //waitForSpaceUnlock(prim.m_targetSpace);
2507 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2508 prim.m_targetSpace = IntPtr.Zero;
2509 //}
2510 //else
2511 //{
2512 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2513 //((OdePrim)prim).m_targetSpace.ToString());
2514 //}
2515
2516 //}
2517 //}
2518 //m_log.Warn(prim.prim_geom);
2519
2520 if (!prim.RemoveGeom())
2521 m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
2522
2523 lock (_prims)
2524 _prims.Remove(prim);
2525
2526 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2527 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2528 //{
2529 //if (prim.m_targetSpace != null)
2530 //{
2531 //if (d.GeomIsSpace(prim.m_targetSpace))
2532 //{
2533 //waitForSpaceUnlock(prim.m_targetSpace);
2534 //d.SpaceRemove(space, prim.m_targetSpace);
2535 // free up memory used by the space.
2536 //d.SpaceDestroy(prim.m_targetSpace);
2537 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2538 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2539 //}
2540 //else
2541 //{
2542 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2543 //((OdePrim) prim).m_targetSpace.ToString());
2544 //}
2545 //}
2546 //}
2547
2548 if (SupportsNINJAJoints)
2549 RemoveAllJointsConnectedToActorThreadLocked(prim);
2550 }
2551 }
2552 }
2553
2554 #endregion
2555
2556 #region Space Separation Calculation
2557
2558 /// <summary>
2559 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2560 /// </summary>
2561 /// <param name="pSpace"></param>
2562 private void resetSpaceArrayItemToZero(IntPtr pSpace)
2563 {
2564 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2565 {
2566 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2567 {
2568 if (staticPrimspace[x, y] == pSpace)
2569 staticPrimspace[x, y] = IntPtr.Zero;
2570 }
2571 }
2572 }
2573
2574// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2575// {
2576// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2577// }
2578
2579 /// <summary>
2580 /// Called when a static prim moves. Allocates a space for the prim based on its position
2581 /// </summary>
2582 /// <param name="geom">the pointer to the geom that moved</param>
2583 /// <param name="pos">the position that the geom moved to</param>
2584 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2585 /// <returns>a pointer to the new space it's in</returns>
2586 internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2587 {
2588 // Called from setting the Position and Size of an ODEPrim so
2589 // it's already in locked space.
2590
2591 // we don't want to remove the main space
2592 // we don't need to test physical here because this function should
2593 // never be called if the prim is physical(active)
2594
2595 // All physical prim end up in the root space
2596 //Thread.Sleep(20);
2597 if (currentspace != space)
2598 {
2599 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2600 //if (currentspace == IntPtr.Zero)
2601 //{
2602 //int adfadf = 0;
2603 //}
2604 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2605 {
2606 if (d.GeomIsSpace(currentspace))
2607 {
2608// waitForSpaceUnlock(currentspace);
2609 d.SpaceRemove(currentspace, geom);
2610 }
2611 else
2612 {
2613 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2614 " Geom:" + geom);
2615 }
2616 }
2617 else
2618 {
2619 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2620 if (sGeomIsIn != IntPtr.Zero)
2621 {
2622 if (d.GeomIsSpace(currentspace))
2623 {
2624// waitForSpaceUnlock(sGeomIsIn);
2625 d.SpaceRemove(sGeomIsIn, geom);
2626 }
2627 else
2628 {
2629 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2630 sGeomIsIn + " Geom:" + geom);
2631 }
2632 }
2633 }
2634
2635 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2636 if (d.SpaceGetNumGeoms(currentspace) == 0)
2637 {
2638 if (currentspace != IntPtr.Zero)
2639 {
2640 if (d.GeomIsSpace(currentspace))
2641 {
2642// waitForSpaceUnlock(currentspace);
2643// waitForSpaceUnlock(space);
2644 d.SpaceRemove(space, currentspace);
2645 // free up memory used by the space.
2646
2647 //d.SpaceDestroy(currentspace);
2648 resetSpaceArrayItemToZero(currentspace);
2649 }
2650 else
2651 {
2652 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2653 currentspace + " Geom:" + geom);
2654 }
2655 }
2656 }
2657 }
2658 else
2659 {
2660 // this is a physical object that got disabled. ;.;
2661 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2662 {
2663 if (d.SpaceQuery(currentspace, geom))
2664 {
2665 if (d.GeomIsSpace(currentspace))
2666 {
2667// waitForSpaceUnlock(currentspace);
2668 d.SpaceRemove(currentspace, geom);
2669 }
2670 else
2671 {
2672 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2673 currentspace + " Geom:" + geom);
2674 }
2675 }
2676 else
2677 {
2678 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2679 if (sGeomIsIn != IntPtr.Zero)
2680 {
2681 if (d.GeomIsSpace(sGeomIsIn))
2682 {
2683// waitForSpaceUnlock(sGeomIsIn);
2684 d.SpaceRemove(sGeomIsIn, geom);
2685 }
2686 else
2687 {
2688 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2689 sGeomIsIn + " Geom:" + geom);
2690 }
2691 }
2692 }
2693 }
2694 }
2695
2696 // The routines in the Position and Size sections do the 'inserting' into the space,
2697 // so all we have to do is make sure that the space that we're putting the prim into
2698 // is in the 'main' space.
2699 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2700 IntPtr newspace = calculateSpaceForGeom(pos);
2701
2702 if (newspace == IntPtr.Zero)
2703 {
2704 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2705 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2706 }
2707
2708 return newspace;
2709 }
2710
2711 /// <summary>
2712 /// Creates a new space at X Y
2713 /// </summary>
2714 /// <param name="iprimspaceArrItemX"></param>
2715 /// <param name="iprimspaceArrItemY"></param>
2716 /// <returns>A pointer to the created space</returns>
2717 internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2718 {
2719 // creating a new space for prim and inserting it into main space.
2720 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2721 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2722// waitForSpaceUnlock(space);
2723 d.SpaceSetSublevel(space, 1);
2724 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2725
2726 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2727 }
2728
2729 /// <summary>
2730 /// Calculates the space the prim should be in by its position
2731 /// </summary>
2732 /// <param name="pos"></param>
2733 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2734 internal IntPtr calculateSpaceForGeom(Vector3 pos)
2735 {
2736 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2737 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2738 return staticPrimspace[xyspace[0], xyspace[1]];
2739 }
2740
2741 /// <summary>
2742 /// Holds the space allocation logic
2743 /// </summary>
2744 /// <param name="pos"></param>
2745 /// <returns>an array item based on the position</returns>
2746 internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2747 {
2748 int[] returnint = new int[2];
2749
2750 returnint[0] = (int) (pos.X/metersInSpace);
2751
2752 if (returnint[0] > ((int) (259f/metersInSpace)))
2753 returnint[0] = ((int) (259f/metersInSpace));
2754 if (returnint[0] < 0)
2755 returnint[0] = 0;
2756
2757 returnint[1] = (int) (pos.Y/metersInSpace);
2758 if (returnint[1] > ((int) (259f/metersInSpace)))
2759 returnint[1] = ((int) (259f/metersInSpace));
2760 if (returnint[1] < 0)
2761 returnint[1] = 0;
2762
2763 return returnint;
2764 }
2765
2766 #endregion
2767
2768 /// <summary>
2769 /// Routine to figure out if we need to mesh this prim with our mesher
2770 /// </summary>
2771 /// <param name="pbs"></param>
2772 /// <returns></returns>
2773 internal bool needsMeshing(PrimitiveBaseShape pbs)
2774 {
2775 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2776 // but we still need to check for sculptie meshing being enabled so this is the most
2777 // convenient place to do it for now...
2778
2779 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2780 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2781 int iPropertiesNotSupportedDefault = 0;
2782
2783 if (pbs.SculptEntry && !meshSculptedPrim)
2784 {
2785#if SPAM
2786 m_log.Warn("NonMesh");
2787#endif
2788 return false;
2789 }
2790
2791 // 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
2792 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2793 {
2794 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2795 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2796 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2797 {
2798
2799 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2800 && pbs.ProfileHollow == 0
2801 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2802 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2803 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2804 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2805 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2806 {
2807#if SPAM
2808 m_log.Warn("NonMesh");
2809#endif
2810 return false;
2811 }
2812 }
2813 }
2814
2815 if (pbs.ProfileHollow != 0)
2816 iPropertiesNotSupportedDefault++;
2817
2818 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
2819 iPropertiesNotSupportedDefault++;
2820
2821 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2822 iPropertiesNotSupportedDefault++;
2823
2824 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2825 iPropertiesNotSupportedDefault++;
2826
2827 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2828 iPropertiesNotSupportedDefault++;
2829
2830 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2831 iPropertiesNotSupportedDefault++;
2832
2833 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2834 iPropertiesNotSupportedDefault++;
2835
2836 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))
2837 iPropertiesNotSupportedDefault++;
2838
2839 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2840 iPropertiesNotSupportedDefault++;
2841
2842 // test for torus
2843 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2844 {
2845 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2846 {
2847 iPropertiesNotSupportedDefault++;
2848 }
2849 }
2850 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2851 {
2852 if (pbs.PathCurve == (byte)Extrusion.Straight)
2853 {
2854 iPropertiesNotSupportedDefault++;
2855 }
2856
2857 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2858 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2859 {
2860 iPropertiesNotSupportedDefault++;
2861 }
2862 }
2863 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2864 {
2865 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2866 {
2867 iPropertiesNotSupportedDefault++;
2868 }
2869 }
2870 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2871 {
2872 if (pbs.PathCurve == (byte)Extrusion.Straight)
2873 {
2874 iPropertiesNotSupportedDefault++;
2875 }
2876 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2877 {
2878 iPropertiesNotSupportedDefault++;
2879 }
2880 }
2881
2882 if (pbs.SculptEntry && meshSculptedPrim)
2883 iPropertiesNotSupportedDefault++;
2884
2885 if (iPropertiesNotSupportedDefault == 0)
2886 {
2887#if SPAM
2888 m_log.Warn("NonMesh");
2889#endif
2890 return false;
2891 }
2892#if SPAM
2893 m_log.Debug("Mesh");
2894#endif
2895 return true;
2896 }
2897
2898 /// <summary>
2899 /// Called after our prim properties are set Scale, position etc.
2900 /// </summary>
2901 /// <remarks>
2902 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2903 /// This assures us that we have no race conditions
2904 /// </remarks>
2905 /// <param name="actor"></param>
2906 public override void AddPhysicsActorTaint(PhysicsActor actor)
2907 {
2908 if (actor is OdePrim)
2909 {
2910 OdePrim taintedprim = ((OdePrim)actor);
2911 lock (_taintedPrims)
2912 _taintedPrims.Add(taintedprim);
2913 }
2914 else if (actor is OdeCharacter)
2915 {
2916 OdeCharacter taintedchar = ((OdeCharacter)actor);
2917 lock (_taintedActors)
2918 {
2919 _taintedActors.Add(taintedchar);
2920 if (taintedchar.bad)
2921 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2922 }
2923 }
2924 }
2925
2926 /// <summary>
2927 /// This is our main simulate loop
2928 /// </summary>
2929 /// <remarks>
2930 /// It's thread locked by a Mutex in the scene.
2931 /// It holds Collisions, it instructs ODE to step through the physical reactions
2932 /// It moves the objects around in memory
2933 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2934 /// </remarks>
2935 /// <param name="timeStep"></param>
2936 /// <returns>The number of frames simulated over that period.</returns>
2937 public override float Simulate(float timeStep)
2938 {
2939 if (!_worldInitialized) return 11f;
2940
2941 int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
2942 int tempTick = 0, tempTick2 = 0;
2943
2944 if (framecount >= int.MaxValue)
2945 framecount = 0;
2946
2947 framecount++;
2948
2949 float fps = 0;
2950
2951 float timeLeft = timeStep;
2952
2953 //m_log.Info(timeStep.ToString());
2954// step_time += timeSte
2955//
2956// // If We're loaded down by something else,
2957// // or debugging with the Visual Studio project on pause
2958// // skip a few frames to catch up gracefully.
2959// // without shooting the physicsactors all over the place
2960//
2961// if (step_time >= m_SkipFramesAtms)
2962// {
2963// // Instead of trying to catch up, it'll do 5 physics frames only
2964// step_time = ODE_STEPSIZE;
2965// m_physicsiterations = 5;
2966// }
2967// else
2968// {
2969// m_physicsiterations = 10;
2970// }
2971
2972 // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
2973 // deadlock if the collision event tries to lock something else later on which is already locked by a
2974 // caller that is adding or removing the collision event.
2975 lock (m_collisionEventActorsChanges)
2976 {
2977 foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
2978 {
2979 if (kvp.Value == null)
2980 m_collisionEventActors.Remove(kvp.Key);
2981 else
2982 m_collisionEventActors[kvp.Key] = kvp.Value;
2983 }
2984
2985 m_collisionEventActorsChanges.Clear();
2986 }
2987
2988 if (SupportsNINJAJoints)
2989 {
2990 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2991 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2992 }
2993
2994 lock (OdeLock)
2995 {
2996 // Process 10 frames if the sim is running normal..
2997 // process 5 frames if the sim is running slow
2998 //try
2999 //{
3000 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
3001 //}
3002 //catch (StackOverflowException)
3003 //{
3004 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
3005 // ode.drelease(world);
3006 //base.TriggerPhysicsBasedRestart();
3007 //}
3008
3009 // Figure out the Frames Per Second we're going at.
3010 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
3011
3012 fps = (timeStep / ODE_STEPSIZE) * 1000;
3013 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
3014 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
3015
3016 while (timeLeft > 0.0f)
3017 {
3018 try
3019 {
3020 if (CollectStats)
3021 tempTick = Util.EnvironmentTickCount();
3022
3023 lock (_taintedActors)
3024 {
3025 foreach (OdeCharacter character in _taintedActors)
3026 character.ProcessTaints();
3027
3028 _taintedActors.Clear();
3029 }
3030
3031 if (CollectStats)
3032 {
3033 tempTick2 = Util.EnvironmentTickCount();
3034 m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3035 tempTick = tempTick2;
3036 }
3037
3038 lock (_taintedPrims)
3039 {
3040 foreach (OdePrim prim in _taintedPrims)
3041 {
3042 if (prim.m_taintremove)
3043 {
3044// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name);
3045 RemovePrimThreadLocked(prim);
3046 }
3047 else
3048 {
3049// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name);
3050 prim.ProcessTaints();
3051 }
3052
3053 prim.m_collisionscore = 0;
3054
3055 // This loop can block up the Heartbeat for a very long time on large regions.
3056 // We need to let the Watchdog know that the Heartbeat is not dead
3057 // NOTE: This is currently commented out, but if things like OAR loading are
3058 // timing the heartbeat out we will need to uncomment it
3059 //Watchdog.UpdateThread();
3060 }
3061
3062 if (SupportsNINJAJoints)
3063 SimulatePendingNINJAJoints();
3064
3065 _taintedPrims.Clear();
3066 }
3067
3068 if (CollectStats)
3069 {
3070 tempTick2 = Util.EnvironmentTickCount();
3071 m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3072 tempTick = tempTick2;
3073 }
3074
3075 // Move characters
3076 foreach (OdeCharacter actor in _characters)
3077 actor.Move(defects);
3078
3079 if (defects.Count != 0)
3080 {
3081 foreach (OdeCharacter actor in defects)
3082 {
3083 m_log.ErrorFormat(
3084 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving",
3085 actor.Name, actor.LocalID, Name);
3086
3087 RemoveCharacter(actor);
3088 actor.DestroyOdeStructures();
3089 }
3090
3091 defects.Clear();
3092 }
3093
3094 if (CollectStats)
3095 {
3096 tempTick2 = Util.EnvironmentTickCount();
3097 m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3098 tempTick = tempTick2;
3099 }
3100
3101 // Move other active objects
3102 foreach (OdePrim prim in _activeprims)
3103 {
3104 prim.m_collisionscore = 0;
3105 prim.Move(timeStep);
3106 }
3107
3108 if (CollectStats)
3109 {
3110 tempTick2 = Util.EnvironmentTickCount();
3111 m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3112 tempTick = tempTick2;
3113 }
3114
3115 //if ((framecount % m_randomizeWater) == 0)
3116 // randomizeWater(waterlevel);
3117
3118 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
3119 m_rayCastManager.ProcessQueuedRequests();
3120
3121 if (CollectStats)
3122 {
3123 tempTick2 = Util.EnvironmentTickCount();
3124 m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3125 tempTick = tempTick2;
3126 }
3127
3128 collision_optimized();
3129
3130 if (CollectStats)
3131 {
3132 tempTick2 = Util.EnvironmentTickCount();
3133 m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3134 tempTick = tempTick2;
3135 }
3136
3137 foreach (PhysicsActor obj in m_collisionEventActors.Values)
3138 {
3139// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
3140
3141 switch ((ActorTypes)obj.PhysicsActorType)
3142 {
3143 case ActorTypes.Agent:
3144 OdeCharacter cobj = (OdeCharacter)obj;
3145 cobj.AddCollisionFrameTime(100);
3146 cobj.SendCollisions();
3147 break;
3148
3149 case ActorTypes.Prim:
3150 OdePrim pobj = (OdePrim)obj;
3151 pobj.SendCollisions();
3152 break;
3153 }
3154 }
3155
3156// if (m_global_contactcount > 0)
3157// m_log.DebugFormat(
3158// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
3159
3160 m_global_contactcount = 0;
3161
3162 if (CollectStats)
3163 {
3164 tempTick2 = Util.EnvironmentTickCount();
3165 m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3166 tempTick = tempTick2;
3167 }
3168
3169 d.WorldQuickStep(world, ODE_STEPSIZE);
3170
3171 if (CollectStats)
3172 m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
3173
3174 d.JointGroupEmpty(contactgroup);
3175 }
3176 catch (Exception e)
3177 {
3178 m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3179 }
3180
3181 timeLeft -= ODE_STEPSIZE;
3182 }
3183
3184 if (CollectStats)
3185 tempTick = Util.EnvironmentTickCount();
3186
3187 foreach (OdeCharacter actor in _characters)
3188 {
3189 if (actor.bad)
3190 m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3191
3192 actor.UpdatePositionAndVelocity(defects);
3193 }
3194
3195 if (defects.Count != 0)
3196 {
3197 foreach (OdeCharacter actor in defects)
3198 {
3199 m_log.ErrorFormat(
3200 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity",
3201 actor.Name, actor.LocalID, Name);
3202
3203 RemoveCharacter(actor);
3204 actor.DestroyOdeStructures();
3205 }
3206
3207 defects.Clear();
3208 }
3209
3210 if (CollectStats)
3211 {
3212 tempTick2 = Util.EnvironmentTickCount();
3213 m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3214 tempTick = tempTick2;
3215 }
3216
3217 //if (timeStep < 0.2f)
3218
3219 foreach (OdePrim prim in _activeprims)
3220 {
3221 if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag))
3222 {
3223 prim.UpdatePositionAndVelocity();
3224
3225 if (SupportsNINJAJoints)
3226 SimulateActorPendingJoints(prim);
3227 }
3228 }
3229
3230 if (CollectStats)
3231 m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
3232
3233 //DumpJointInfo();
3234
3235 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3236 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3237 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3238 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
3239 {
3240 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3241 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3242
3243 if (physics_logging_append_existing_logfile)
3244 {
3245 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3246 TextWriter fwriter = File.AppendText(fname);
3247 fwriter.WriteLine(header);
3248 fwriter.Close();
3249 }
3250
3251 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3252 }
3253
3254 latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
3255
3256 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics
3257 // has a max of 100 ms to run theoretically.
3258 // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
3259 // If Physics stalls, it takes longer which makes the tick count ms larger.
3260
3261 if (latertickcount < 100)
3262 {
3263 m_timeDilation = 1.0f;
3264 }
3265 else
3266 {
3267 m_timeDilation = 100f / latertickcount;
3268 //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f);
3269 }
3270
3271 tickCountFrameRun = Util.EnvironmentTickCount();
3272
3273 if (CollectStats)
3274 m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
3275 }
3276
3277 return fps;
3278 }
3279
3280 /// <summary>
3281 /// Simulate pending NINJA joints.
3282 /// </summary>
3283 /// <remarks>
3284 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
3285 /// </remarks>
3286 private void SimulatePendingNINJAJoints()
3287 {
3288 // Create pending joints, if possible
3289
3290 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
3291 // a joint requires specifying the body id of both involved bodies
3292 if (pendingJoints.Count > 0)
3293 {
3294 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
3295 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
3296 foreach (PhysicsJoint joint in pendingJoints)
3297 {
3298 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
3299 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
3300 List<IntPtr> jointBodies = new List<IntPtr>();
3301 bool allJointBodiesAreReady = true;
3302 foreach (string jointParam in jointParams)
3303 {
3304 if (jointParam == "NULL")
3305 {
3306 //DoJointErrorMessage(joint, "attaching NULL joint to world");
3307 jointBodies.Add(IntPtr.Zero);
3308 }
3309 else
3310 {
3311 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
3312 bool foundPrim = false;
3313 lock (_prims)
3314 {
3315 foreach (OdePrim prim in _prims) // FIXME: inefficient
3316 {
3317 if (prim.SOPName == jointParam)
3318 {
3319 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
3320 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
3321 {
3322 jointBodies.Add(prim.Body);
3323 foundPrim = true;
3324 break;
3325 }
3326 else
3327 {
3328 DoJointErrorMessage(joint, "prim name " + jointParam +
3329 " exists but is not (yet) physical; deferring joint creation. " +
3330 "IsPhysical property is " + prim.IsPhysical +
3331 " and body is " + prim.Body);
3332 foundPrim = false;
3333 break;
3334 }
3335 }
3336 }
3337 }
3338 if (foundPrim)
3339 {
3340 // all is fine
3341 }
3342 else
3343 {
3344 allJointBodiesAreReady = false;
3345 break;
3346 }
3347 }
3348 }
3349
3350 if (allJointBodiesAreReady)
3351 {
3352 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3353 if (jointBodies[0] == jointBodies[1])
3354 {
3355 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
3356 }
3357 else
3358 {
3359 switch (joint.Type)
3360 {
3361 case PhysicsJointType.Ball:
3362 {
3363 IntPtr odeJoint;
3364 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3365 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3366 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3367 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3368 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3369 d.JointSetBallAnchor(odeJoint,
3370 joint.Position.X,
3371 joint.Position.Y,
3372 joint.Position.Z);
3373 //DoJointErrorMessage(joint, "ODE joint setting OK");
3374 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3375 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3376 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3377 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3378
3379 if (joint is OdePhysicsJoint)
3380 {
3381 ((OdePhysicsJoint)joint).jointID = odeJoint;
3382 }
3383 else
3384 {
3385 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3386 }
3387 }
3388 break;
3389 case PhysicsJointType.Hinge:
3390 {
3391 IntPtr odeJoint;
3392 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3393 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3394 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3395 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3396 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3397 d.JointSetHingeAnchor(odeJoint,
3398 joint.Position.X,
3399 joint.Position.Y,
3400 joint.Position.Z);
3401 // We use the orientation of the x-axis of the joint's coordinate frame
3402 // as the axis for the hinge.
3403
3404 // Therefore, we must get the joint's coordinate frame based on the
3405 // joint.Rotation field, which originates from the orientation of the
3406 // joint's proxy object in the scene.
3407
3408 // The joint's coordinate frame is defined as the transformation matrix
3409 // that converts a vector from joint-local coordinates into world coordinates.
3410 // World coordinates are defined as the XYZ coordinate system of the sim,
3411 // as shown in the top status-bar of the viewer.
3412
3413 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3414 // and use that as the hinge axis.
3415
3416 //joint.Rotation.Normalize();
3417 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3418
3419 // Now extract the X axis of the joint's coordinate frame.
3420
3421 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3422 // tar pit of transposed, inverted, and generally messed-up orientations.
3423 // (In other words, Matrix4.AtAxis() is borked.)
3424 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3425
3426 // Instead, compute the X axis of the coordinate frame by transforming
3427 // the (1,0,0) vector. At least that works.
3428
3429 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3430 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3431 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3432 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3433 d.JointSetHingeAxis(odeJoint,
3434 jointAxis.X,
3435 jointAxis.Y,
3436 jointAxis.Z);
3437 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3438 if (joint is OdePhysicsJoint)
3439 {
3440 ((OdePhysicsJoint)joint).jointID = odeJoint;
3441 }
3442 else
3443 {
3444 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3445 }
3446 }
3447 break;
3448 }
3449 successfullyProcessedPendingJoints.Add(joint);
3450 }
3451 }
3452 else
3453 {
3454 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3455 }
3456 }
3457
3458 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
3459 {
3460 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
3461 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
3462 InternalRemovePendingJoint(successfullyProcessedJoint);
3463 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
3464 InternalAddActiveJoint(successfullyProcessedJoint);
3465 //DoJointErrorMessage(successfullyProcessedJoint, "done");
3466 }
3467 }
3468 }
3469
3470 /// <summary>
3471 /// Simulate the joint proxies of a NINJA actor.
3472 /// </summary>
3473 /// <remarks>
3474 /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
3475 /// </remarks>
3476 /// <param name="actor"></param>
3477 private void SimulateActorPendingJoints(OdePrim actor)
3478 {
3479 // If an actor moved, move its joint proxy objects as well.
3480 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3481 // for this purpose but it is never called! So we just do the joint
3482 // movement code here.
3483
3484 if (actor.SOPName != null &&
3485 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3486 joints_connecting_actor[actor.SOPName] != null &&
3487 joints_connecting_actor[actor.SOPName].Count > 0)
3488 {
3489 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3490 {
3491 if (affectedJoint.IsInPhysicsEngine)
3492 {
3493 DoJointMoved(affectedJoint);
3494 }
3495 else
3496 {
3497 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);
3498 }
3499 }
3500 }
3501 }
3502
3503 public override void GetResults()
3504 {
3505 }
3506
3507 public override bool IsThreaded
3508 {
3509 // for now we won't be multithreaded
3510 get { return false; }
3511 }
3512
3513 #region ODE Specific Terrain Fixes
3514 private float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3515 {
3516 float[] returnarr = new float[262144];
3517 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3518
3519 // Filling out the array into its multi-dimensional components
3520 for (int y = 0; y < WorldExtents.Y; y++)
3521 {
3522 for (int x = 0; x < WorldExtents.X; x++)
3523 {
3524 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3525 }
3526 }
3527
3528 // Resize using Nearest Neighbour
3529
3530 // This particular way is quick but it only works on a multiple of the original
3531
3532 // The idea behind this method can be described with the following diagrams
3533 // second pass and third pass happen in the same loop really.. just separated
3534 // them to show what this does.
3535
3536 // First Pass
3537 // ResultArr:
3538 // 1,1,1,1,1,1
3539 // 1,1,1,1,1,1
3540 // 1,1,1,1,1,1
3541 // 1,1,1,1,1,1
3542 // 1,1,1,1,1,1
3543 // 1,1,1,1,1,1
3544
3545 // Second Pass
3546 // ResultArr2:
3547 // 1,,1,,1,,1,,1,,1,
3548 // ,,,,,,,,,,
3549 // 1,,1,,1,,1,,1,,1,
3550 // ,,,,,,,,,,
3551 // 1,,1,,1,,1,,1,,1,
3552 // ,,,,,,,,,,
3553 // 1,,1,,1,,1,,1,,1,
3554 // ,,,,,,,,,,
3555 // 1,,1,,1,,1,,1,,1,
3556 // ,,,,,,,,,,
3557 // 1,,1,,1,,1,,1,,1,
3558
3559 // Third pass fills in the blanks
3560 // ResultArr2:
3561 // 1,1,1,1,1,1,1,1,1,1,1,1
3562 // 1,1,1,1,1,1,1,1,1,1,1,1
3563 // 1,1,1,1,1,1,1,1,1,1,1,1
3564 // 1,1,1,1,1,1,1,1,1,1,1,1
3565 // 1,1,1,1,1,1,1,1,1,1,1,1
3566 // 1,1,1,1,1,1,1,1,1,1,1,1
3567 // 1,1,1,1,1,1,1,1,1,1,1,1
3568 // 1,1,1,1,1,1,1,1,1,1,1,1
3569 // 1,1,1,1,1,1,1,1,1,1,1,1
3570 // 1,1,1,1,1,1,1,1,1,1,1,1
3571 // 1,1,1,1,1,1,1,1,1,1,1,1
3572
3573 // X,Y = .
3574 // X+1,y = ^
3575 // X,Y+1 = *
3576 // X+1,Y+1 = #
3577
3578 // Filling in like this;
3579 // .*
3580 // ^#
3581 // 1st .
3582 // 2nd *
3583 // 3rd ^
3584 // 4th #
3585 // on single loop.
3586
3587 float[,] resultarr2 = new float[512, 512];
3588 for (int y = 0; y < WorldExtents.Y; y++)
3589 {
3590 for (int x = 0; x < WorldExtents.X; x++)
3591 {
3592 resultarr2[y * 2, x * 2] = resultarr[y, x];
3593
3594 if (y < WorldExtents.Y)
3595 {
3596 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3597 }
3598 if (x < WorldExtents.X)
3599 {
3600 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3601 }
3602 if (x < WorldExtents.X && y < WorldExtents.Y)
3603 {
3604 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3605 }
3606 }
3607 }
3608
3609 //Flatten out the array
3610 int i = 0;
3611 for (int y = 0; y < 512; y++)
3612 {
3613 for (int x = 0; x < 512; x++)
3614 {
3615 if (resultarr2[y, x] <= 0)
3616 returnarr[i] = 0.0000001f;
3617 else
3618 returnarr[i] = resultarr2[y, x];
3619
3620 i++;
3621 }
3622 }
3623
3624 return returnarr;
3625 }
3626
3627 private float[] ResizeTerrain512Interpolation(float[] heightMap)
3628 {
3629 float[] returnarr = new float[262144];
3630 float[,] resultarr = new float[512,512];
3631
3632 // Filling out the array into its multi-dimensional components
3633 for (int y = 0; y < 256; y++)
3634 {
3635 for (int x = 0; x < 256; x++)
3636 {
3637 resultarr[y, x] = heightMap[y * 256 + x];
3638 }
3639 }
3640
3641 // Resize using interpolation
3642
3643 // This particular way is quick but it only works on a multiple of the original
3644
3645 // The idea behind this method can be described with the following diagrams
3646 // second pass and third pass happen in the same loop really.. just separated
3647 // them to show what this does.
3648
3649 // First Pass
3650 // ResultArr:
3651 // 1,1,1,1,1,1
3652 // 1,1,1,1,1,1
3653 // 1,1,1,1,1,1
3654 // 1,1,1,1,1,1
3655 // 1,1,1,1,1,1
3656 // 1,1,1,1,1,1
3657
3658 // Second Pass
3659 // ResultArr2:
3660 // 1,,1,,1,,1,,1,,1,
3661 // ,,,,,,,,,,
3662 // 1,,1,,1,,1,,1,,1,
3663 // ,,,,,,,,,,
3664 // 1,,1,,1,,1,,1,,1,
3665 // ,,,,,,,,,,
3666 // 1,,1,,1,,1,,1,,1,
3667 // ,,,,,,,,,,
3668 // 1,,1,,1,,1,,1,,1,
3669 // ,,,,,,,,,,
3670 // 1,,1,,1,,1,,1,,1,
3671
3672 // Third pass fills in the blanks
3673 // ResultArr2:
3674 // 1,1,1,1,1,1,1,1,1,1,1,1
3675 // 1,1,1,1,1,1,1,1,1,1,1,1
3676 // 1,1,1,1,1,1,1,1,1,1,1,1
3677 // 1,1,1,1,1,1,1,1,1,1,1,1
3678 // 1,1,1,1,1,1,1,1,1,1,1,1
3679 // 1,1,1,1,1,1,1,1,1,1,1,1
3680 // 1,1,1,1,1,1,1,1,1,1,1,1
3681 // 1,1,1,1,1,1,1,1,1,1,1,1
3682 // 1,1,1,1,1,1,1,1,1,1,1,1
3683 // 1,1,1,1,1,1,1,1,1,1,1,1
3684 // 1,1,1,1,1,1,1,1,1,1,1,1
3685
3686 // X,Y = .
3687 // X+1,y = ^
3688 // X,Y+1 = *
3689 // X+1,Y+1 = #
3690
3691 // Filling in like this;
3692 // .*
3693 // ^#
3694 // 1st .
3695 // 2nd *
3696 // 3rd ^
3697 // 4th #
3698 // on single loop.
3699
3700 float[,] resultarr2 = new float[512,512];
3701 for (int y = 0; y < (int)Constants.RegionSize; y++)
3702 {
3703 for (int x = 0; x < (int)Constants.RegionSize; x++)
3704 {
3705 resultarr2[y*2, x*2] = resultarr[y, x];
3706
3707 if (y < (int)Constants.RegionSize)
3708 {
3709 if (y + 1 < (int)Constants.RegionSize)
3710 {
3711 if (x + 1 < (int)Constants.RegionSize)
3712 {
3713 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3714 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3715 }
3716 else
3717 {
3718 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3719 }
3720 }
3721 else
3722 {
3723 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3724 }
3725 }
3726 if (x < (int)Constants.RegionSize)
3727 {
3728 if (x + 1 < (int)Constants.RegionSize)
3729 {
3730 if (y + 1 < (int)Constants.RegionSize)
3731 {
3732 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3733 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3734 }
3735 else
3736 {
3737 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3738 }
3739 }
3740 else
3741 {
3742 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3743 }
3744 }
3745 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3746 {
3747 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3748 {
3749 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3750 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3751 }
3752 else
3753 {
3754 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3755 }
3756 }
3757 }
3758 }
3759 //Flatten out the array
3760 int i = 0;
3761 for (int y = 0; y < 512; y++)
3762 {
3763 for (int x = 0; x < 512; x++)
3764 {
3765 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3766 {
3767 m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0");
3768 resultarr2[y, x] = 0;
3769 }
3770 returnarr[i] = resultarr2[y, x];
3771 i++;
3772 }
3773 }
3774
3775 return returnarr;
3776 }
3777
3778 #endregion
3779
3780 public override void SetTerrain(float[] heightMap)
3781 {
3782 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3783 {
3784 if (m_parentScene is OdeScene)
3785 {
3786 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3787 }
3788 }
3789 else
3790 {
3791 SetTerrain(heightMap, m_worldOffset);
3792 }
3793 }
3794
3795 private void SetTerrain(float[] heightMap, Vector3 pOffset)
3796 {
3797 int startTime = Util.EnvironmentTickCount();
3798 m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset);
3799
3800 // this._heightmap[i] = (double)heightMap[i];
3801 // dbm (danx0r) -- creating a buffer zone of one extra sample all around
3802 //_origheightmap = heightMap;
3803
3804 float[] _heightmap;
3805
3806 // zero out a heightmap array float array (single dimension [flattened]))
3807 //if ((int)Constants.RegionSize == 256)
3808 // _heightmap = new float[514 * 514];
3809 //else
3810
3811 _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
3812
3813 uint heightmapWidth = Constants.RegionSize + 1;
3814 uint heightmapHeight = Constants.RegionSize + 1;
3815
3816 uint heightmapWidthSamples;
3817
3818 uint heightmapHeightSamples;
3819
3820 //if (((int)Constants.RegionSize) == 256)
3821 //{
3822 // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
3823 // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
3824 // heightmapWidth++;
3825 // heightmapHeight++;
3826 //}
3827 //else
3828 //{
3829
3830 heightmapWidthSamples = (uint)Constants.RegionSize + 1;
3831 heightmapHeightSamples = (uint)Constants.RegionSize + 1;
3832 //}
3833
3834 const float scale = 1.0f;
3835 const float offset = 0.0f;
3836 const float thickness = 0.2f;
3837 const int wrap = 0;
3838
3839 int regionsize = (int) Constants.RegionSize + 2;
3840 //Double resolution
3841 //if (((int)Constants.RegionSize) == 256)
3842 // heightMap = ResizeTerrain512Interpolation(heightMap);
3843
3844
3845 // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256)
3846 // regionsize = 512;
3847
3848 float hfmin = 2000;
3849 float hfmax = -2000;
3850
3851 for (int x = 0; x < heightmapWidthSamples; x++)
3852 {
3853 for (int y = 0; y < heightmapHeightSamples; y++)
3854 {
3855 int xx = Util.Clip(x - 1, 0, regionsize - 1);
3856 int yy = Util.Clip(y - 1, 0, regionsize - 1);
3857
3858
3859 float val= heightMap[yy * (int)Constants.RegionSize + xx];
3860 _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val;
3861
3862 hfmin = (val < hfmin) ? val : hfmin;
3863 hfmax = (val > hfmax) ? val : hfmax;
3864 }
3865 }
3866
3867 lock (OdeLock)
3868 {
3869 IntPtr GroundGeom = IntPtr.Zero;
3870 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3871 {
3872 RegionTerrain.Remove(pOffset);
3873 if (GroundGeom != IntPtr.Zero)
3874 {
3875 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3876 {
3877 TerrainHeightFieldHeights.Remove(GroundGeom);
3878 }
3879 d.SpaceRemove(space, GroundGeom);
3880 d.GeomDestroy(GroundGeom);
3881 }
3882
3883 }
3884 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3885 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1,
3886 (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale,
3887 offset, thickness, wrap);
3888 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3889 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3890 if (GroundGeom != IntPtr.Zero)
3891 {
3892 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3893 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3894
3895 }
3896 geom_name_map[GroundGeom] = "Terrain";
3897
3898 d.Matrix3 R = new d.Matrix3();
3899
3900 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3901 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3902 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3903
3904 q1 = q1 * q2;
3905 //q1 = q1 * q3;
3906 Vector3 v3;
3907 float angle;
3908 q1.GetAxisAngle(out v3, out angle);
3909
3910 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3911 d.GeomSetRotation(GroundGeom, ref R);
3912 d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0);
3913 IntPtr testGround = IntPtr.Zero;
3914 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3915 {
3916 RegionTerrain.Remove(pOffset);
3917 }
3918 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3919 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3920 }
3921
3922 m_log.DebugFormat(
3923 "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime));
3924 }
3925
3926 public override void DeleteTerrain()
3927 {
3928 }
3929
3930 internal float GetWaterLevel()
3931 {
3932 return waterlevel;
3933 }
3934
3935 public override bool SupportsCombining()
3936 {
3937 return true;
3938 }
3939
3940// public override void UnCombine(PhysicsScene pScene)
3941// {
3942// IntPtr localGround = IntPtr.Zero;
3943//// float[] localHeightfield;
3944// bool proceed = false;
3945// List<IntPtr> geomDestroyList = new List<IntPtr>();
3946//
3947// lock (OdeLock)
3948// {
3949// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3950// {
3951// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3952// {
3953// if (geom == localGround)
3954// {
3955//// localHeightfield = TerrainHeightFieldHeights[geom];
3956// proceed = true;
3957// }
3958// else
3959// {
3960// geomDestroyList.Add(geom);
3961// }
3962// }
3963//
3964// if (proceed)
3965// {
3966// m_worldOffset = Vector3.Zero;
3967// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3968// m_parentScene = null;
3969//
3970// foreach (IntPtr g in geomDestroyList)
3971// {
3972// // removingHeightField needs to be done or the garbage collector will
3973// // collect the terrain data before we tell ODE to destroy it causing
3974// // memory corruption
3975// if (TerrainHeightFieldHeights.ContainsKey(g))
3976// {
3977//// float[] removingHeightField = TerrainHeightFieldHeights[g];
3978// TerrainHeightFieldHeights.Remove(g);
3979//
3980// if (RegionTerrain.ContainsKey(g))
3981// {
3982// RegionTerrain.Remove(g);
3983// }
3984//
3985// d.GeomDestroy(g);
3986// //removingHeightField = new float[0];
3987// }
3988// }
3989//
3990// }
3991// else
3992// {
3993// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3994// }
3995// }
3996// }
3997// }
3998
3999 public override void SetWaterLevel(float baseheight)
4000 {
4001 waterlevel = baseheight;
4002 randomizeWater(waterlevel);
4003 }
4004
4005 private void randomizeWater(float baseheight)
4006 {
4007 const uint heightmapWidth = m_regionWidth + 2;
4008 const uint heightmapHeight = m_regionHeight + 2;
4009 const uint heightmapWidthSamples = m_regionWidth + 2;
4010 const uint heightmapHeightSamples = m_regionHeight + 2;
4011 const float scale = 1.0f;
4012 const float offset = 0.0f;
4013 const float thickness = 2.9f;
4014 const int wrap = 0;
4015
4016 for (int i = 0; i < (258 * 258); i++)
4017 {
4018 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
4019 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
4020 }
4021
4022 lock (OdeLock)
4023 {
4024 if (WaterGeom != IntPtr.Zero)
4025 {
4026 d.SpaceRemove(space, WaterGeom);
4027 }
4028 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
4029 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
4030 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
4031 offset, thickness, wrap);
4032 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
4033 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
4034 if (WaterGeom != IntPtr.Zero)
4035 {
4036 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
4037 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
4038 }
4039
4040 geom_name_map[WaterGeom] = "Water";
4041
4042 d.Matrix3 R = new d.Matrix3();
4043
4044 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
4045 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
4046 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
4047
4048 q1 = q1 * q2;
4049 //q1 = q1 * q3;
4050 Vector3 v3;
4051 float angle;
4052 q1.GetAxisAngle(out v3, out angle);
4053
4054 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
4055 d.GeomSetRotation(WaterGeom, ref R);
4056 d.GeomSetPosition(WaterGeom, 128, 128, 0);
4057 }
4058 }
4059
4060 public override void Dispose()
4061 {
4062 _worldInitialized = false;
4063
4064 m_rayCastManager.Dispose();
4065 m_rayCastManager = null;
4066
4067 lock (OdeLock)
4068 {
4069 lock (_prims)
4070 {
4071 foreach (OdePrim prm in _prims)
4072 {
4073 RemovePrim(prm);
4074 }
4075 }
4076
4077 //foreach (OdeCharacter act in _characters)
4078 //{
4079 //RemoveAvatar(act);
4080 //}
4081 d.WorldDestroy(world);
4082 //d.CloseODE();
4083 }
4084
4085 }
4086
4087 public override Dictionary<uint, float> GetTopColliders()
4088 {
4089 Dictionary<uint, float> topColliders;
4090
4091 lock (_prims)
4092 {
4093 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
4094 orderedPrims.OrderByDescending(p => p.CollisionScore);
4095 topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
4096
4097 foreach (OdePrim p in _prims)
4098 p.CollisionScore = 0;
4099 }
4100
4101 return topColliders;
4102 }
4103
4104 public override bool SupportsRayCast()
4105 {
4106 return true;
4107 }
4108
4109 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
4110 {
4111 if (retMethod != null)
4112 {
4113 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
4114 }
4115 }
4116
4117 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
4118 {
4119 if (retMethod != null)
4120 {
4121 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
4122 }
4123 }
4124
4125 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
4126 {
4127 ContactResult[] ourResults = null;
4128 RayCallback retMethod = delegate(List<ContactResult> results)
4129 {
4130 ourResults = new ContactResult[results.Count];
4131 results.CopyTo(ourResults, 0);
4132 };
4133 int waitTime = 0;
4134 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
4135 while (ourResults == null && waitTime < 1000)
4136 {
4137 Thread.Sleep(1);
4138 waitTime++;
4139 }
4140 if (ourResults == null)
4141 return new List<ContactResult> ();
4142 return new List<ContactResult>(ourResults);
4143 }
4144
4145#if USE_DRAWSTUFF
4146 // Keyboard callback
4147 public void command(int cmd)
4148 {
4149 IntPtr geom;
4150 d.Mass mass;
4151 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
4152
4153
4154
4155 Char ch = Char.ToLower((Char)cmd);
4156 switch ((Char)ch)
4157 {
4158 case 'w':
4159 try
4160 {
4161 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));
4162
4163 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
4164 ds.SetViewpoint(ref xyz, ref hpr);
4165 }
4166 catch (ArgumentException)
4167 { hpr.X = 0; }
4168 break;
4169
4170 case 'a':
4171 hpr.X++;
4172 ds.SetViewpoint(ref xyz, ref hpr);
4173 break;
4174
4175 case 's':
4176 try
4177 {
4178 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));
4179
4180 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
4181 ds.SetViewpoint(ref xyz, ref hpr);
4182 }
4183 catch (ArgumentException)
4184 { hpr.X = 0; }
4185 break;
4186 case 'd':
4187 hpr.X--;
4188 ds.SetViewpoint(ref xyz, ref hpr);
4189 break;
4190 case 'r':
4191 xyz.Z++;
4192 ds.SetViewpoint(ref xyz, ref hpr);
4193 break;
4194 case 'f':
4195 xyz.Z--;
4196 ds.SetViewpoint(ref xyz, ref hpr);
4197 break;
4198 case 'e':
4199 xyz.Y++;
4200 ds.SetViewpoint(ref xyz, ref hpr);
4201 break;
4202 case 'q':
4203 xyz.Y--;
4204 ds.SetViewpoint(ref xyz, ref hpr);
4205 break;
4206 }
4207 }
4208
4209 public void step(int pause)
4210 {
4211
4212 ds.SetColor(1.0f, 1.0f, 0.0f);
4213 ds.SetTexture(ds.Texture.Wood);
4214 lock (_prims)
4215 {
4216 foreach (OdePrim prm in _prims)
4217 {
4218 //IntPtr body = d.GeomGetBody(prm.prim_geom);
4219 if (prm.prim_geom != IntPtr.Zero)
4220 {
4221 d.Vector3 pos;
4222 d.GeomCopyPosition(prm.prim_geom, out pos);
4223 //d.BodyCopyPosition(body, out pos);
4224
4225 d.Matrix3 R;
4226 d.GeomCopyRotation(prm.prim_geom, out R);
4227 //d.BodyCopyRotation(body, out R);
4228
4229
4230 d.Vector3 sides = new d.Vector3();
4231 sides.X = prm.Size.X;
4232 sides.Y = prm.Size.Y;
4233 sides.Z = prm.Size.Z;
4234
4235 ds.DrawBox(ref pos, ref R, ref sides);
4236 }
4237 }
4238 }
4239 ds.SetColor(1.0f, 0.0f, 0.0f);
4240
4241 foreach (OdeCharacter chr in _characters)
4242 {
4243 if (chr.Shell != IntPtr.Zero)
4244 {
4245 IntPtr body = d.GeomGetBody(chr.Shell);
4246
4247 d.Vector3 pos;
4248 d.GeomCopyPosition(chr.Shell, out pos);
4249 //d.BodyCopyPosition(body, out pos);
4250
4251 d.Matrix3 R;
4252 d.GeomCopyRotation(chr.Shell, out R);
4253 //d.BodyCopyRotation(body, out R);
4254
4255 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
4256 d.Vector3 sides = new d.Vector3();
4257 sides.X = 0.5f;
4258 sides.Y = 0.5f;
4259 sides.Z = 0.5f;
4260
4261 ds.DrawBox(ref pos, ref R, ref sides);
4262 }
4263 }
4264 }
4265
4266 public void start(int unused)
4267 {
4268 ds.SetViewpoint(ref xyz, ref hpr);
4269 }
4270#endif
4271
4272 public override Dictionary<string, float> GetStats()
4273 {
4274 if (!CollectStats)
4275 return null;
4276
4277 Dictionary<string, float> returnStats;
4278
4279 lock (OdeLock)
4280 {
4281 returnStats = new Dictionary<string, float>(m_stats);
4282
4283 // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
4284 // 3 from the SimStatsReporter.
4285 returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
4286 returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
4287 returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
4288
4289 InitializeExtraStats();
4290 }
4291
4292 returnStats[ODEOtherCollisionFrameMsStatName]
4293 = returnStats[ODEOtherCollisionFrameMsStatName]
4294 - returnStats[ODENativeSpaceCollisionFrameMsStatName]
4295 - returnStats[ODENativeGeomCollisionFrameMsStatName];
4296
4297 return returnStats;
4298 }
4299
4300 private void InitializeExtraStats()
4301 {
4302 m_stats[ODETotalFrameMsStatName] = 0;
4303 m_stats[ODEAvatarTaintMsStatName] = 0;
4304 m_stats[ODEPrimTaintMsStatName] = 0;
4305 m_stats[ODEAvatarForcesFrameMsStatName] = 0;
4306 m_stats[ODEPrimForcesFrameMsStatName] = 0;
4307 m_stats[ODERaycastingFrameMsStatName] = 0;
4308 m_stats[ODENativeStepFrameMsStatName] = 0;
4309 m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
4310 m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
4311 m_stats[ODEOtherCollisionFrameMsStatName] = 0;
4312 m_stats[ODECollisionNotificationFrameMsStatName] = 0;
4313 m_stats[ODEAvatarContactsStatsName] = 0;
4314 m_stats[ODEPrimContactsStatName] = 0;
4315 m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
4316 m_stats[ODEPrimUpdateFrameMsStatName] = 0;
4317 }
4318 }
4319}
diff --git a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
new file mode 100644
index 0000000..16404c6
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs
@@ -0,0 +1,128 @@
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 OpenSim.Region.Physics.OdePlugin;
35using OpenSim.Tests.Common;
36using log4net;
37using System.Reflection;
38
39namespace OpenSim.Region.Physics.OdePlugin.Tests
40{
41 [TestFixture]
42 public class ODETestClass : OpenSimTestCase
43 {
44 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
45
46 private OpenSim.Region.Physics.OdePlugin.OdePlugin cbt;
47 private PhysicsScene ps;
48 private IMeshingPlugin imp;
49
50 [SetUp]
51 public void Initialize()
52 {
53 IConfigSource TopConfig = new IniConfigSource();
54 IConfig config = TopConfig.AddConfig("Startup");
55 config.Set("DecodedSculptMapPath","j2kDecodeCache");
56
57 // Loading ODEPlugin
58 cbt = new OdePlugin();
59 // Loading Zero Mesher
60 imp = new ZeroMesherPlugin();
61 // Getting Physics Scene
62 ps = cbt.GetScene("test");
63 // Initializing Physics Scene.
64 ps.Initialise(imp.GetMesher(TopConfig),null);
65 float[] _heightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
66 for (int i = 0; i < ((int)Constants.RegionSize * (int)Constants.RegionSize); i++)
67 {
68 _heightmap[i] = 21f;
69 }
70 ps.SetTerrain(_heightmap);
71 }
72
73 [TearDown]
74 public void Terminate()
75 {
76 ps.DeleteTerrain();
77 ps.Dispose();
78
79 }
80
81 [Test]
82 public void CreateAndDropPhysicalCube()
83 {
84 PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
85 Vector3 position = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 128f);
86 Vector3 size = new Vector3(0.5f, 0.5f, 0.5f);
87 Quaternion rot = Quaternion.Identity;
88 PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true, 0);
89 OdePrim oprim = (OdePrim)prim;
90 OdeScene pscene = (OdeScene) ps;
91
92 Assert.That(oprim.m_taintadd);
93
94 prim.LocalID = 5;
95
96 for (int i = 0; i < 58; i++)
97 {
98 ps.Simulate(0.133f);
99
100 Assert.That(oprim.prim_geom != (IntPtr)0);
101
102 Assert.That(oprim.m_targetSpace != (IntPtr)0);
103
104 //Assert.That(oprim.m_targetSpace == pscene.space);
105 m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
106
107 Assert.That(!oprim.m_taintadd);
108 m_log.Info("Prim Position (" + oprim.LocalID + "): " + prim.Position);
109
110 // Make sure we're above the ground
111 //Assert.That(prim.Position.Z > 20f);
112 //m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
113
114 // Make sure we've got a Body
115 Assert.That(oprim.Body != (IntPtr)0);
116 //m_log.Info(
117 }
118
119 // Make sure we're not somewhere above the ground
120 Assert.That(prim.Position.Z < 21.5f);
121
122 ps.RemovePrim(prim);
123 Assert.That(oprim.m_taintremove);
124 ps.Simulate(0.133f);
125 Assert.That(oprim.Body == (IntPtr)0);
126 }
127 }
128}
diff --git a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs b/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs
new file mode 100644
index 0000000..87ca446
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/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}
generated by cgit v1.1 at 2024-12-15 04:15:42 +0000