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