aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs3271
1 files changed, 3271 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..983431d
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3271 @@
1/* Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders.
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright
6 * notice, this list of conditions and the following disclaimer.
7 * * Redistributions in binary form must reproduce the above copyright
8 * notice, this list of conditions and the following disclaimer in the
9 * documentation and/or other materials provided with the distribution.
10 * * Neither the name of the OpenSimulator Project nor the
11 * names of its contributors may be used to endorse or promote products
12 * derived from this software without specific prior written permission.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
15 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
16 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
18 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
19 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
21 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 *
25 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
26 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
27 * ODEPrim.cs contains methods dealing with Prim editing, Prim
28 * characteristics and Kinetic motion.
29 * ODEDynamics.cs contains methods dealing with Prim Physical motion
30 * (dynamics) and the associated settings. Old Linear and angular
31 * motors for dynamic motion have been replace with MoveLinear()
32 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
33 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
34 * switch between 'VEHICLE' parameter use and general dynamics
35 * settings use.
36 */
37
38/*
39 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
40 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
41 * ODEPrim.cs contains methods dealing with Prim editing, Prim
42 * characteristics and Kinetic motion.
43 * ODEDynamics.cs contains methods dealing with Prim Physical motion
44 * (dynamics) and the associated settings. Old Linear and angular
45 * motors for dynamic motion have been replace with MoveLinear()
46 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
47 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
48 * switch between 'VEHICLE' parameter use and general dynamics
49 * settings use.
50 */
51using System;
52using System.Collections.Generic;
53using System.Reflection;
54using System.Runtime.InteropServices;
55using System.Threading;
56using log4net;
57using OpenMetaverse;
58using Ode.NET;
59using OpenSim.Framework;
60using OpenSim.Region.Physics.Manager;
61
62namespace OpenSim.Region.Physics.OdePlugin
63{
64 /// <summary>
65 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
66 /// </summary>
67
68 public class OdePrim : PhysicsActor
69 {
70 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
71
72 private Vector3 _position;
73 private Vector3 _velocity;
74 private Vector3 _torque;
75 private Vector3 m_lastVelocity;
76 private Vector3 m_lastposition;
77 private Quaternion m_lastorientation = new Quaternion();
78 private Vector3 m_rotationalVelocity;
79 private Vector3 _size;
80 private Vector3 _acceleration;
81 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
82 private Quaternion _orientation;
83 private Vector3 m_taintposition;
84 private Vector3 m_taintsize;
85 private Vector3 m_taintVelocity;
86 private Vector3 m_taintTorque;
87 private Quaternion m_taintrot;
88 private Vector3 m_angularlock = Vector3.One;
89 private Vector3 m_taintAngularLock = Vector3.One;
90 private IntPtr Amotor = IntPtr.Zero;
91
92 private Vector3 m_PIDTarget;
93 private float m_PIDTau;
94 private float PID_D = 35f;
95 private float PID_G = 25f;
96 private bool m_usePID = false;
97
98 private Quaternion m_APIDTarget = new Quaternion();
99 private float m_APIDStrength = 0.5f;
100 private float m_APIDDamping = 0.5f;
101 private bool m_useAPID = false;
102
103 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
104 // and are for non-VEHICLES only.
105
106 private float m_PIDHoverHeight;
107 private float m_PIDHoverTau;
108 private bool m_useHoverPID;
109 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
110 private float m_targetHoverHeight;
111 private float m_groundHeight;
112 private float m_waterHeight;
113 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
114
115 // private float m_tensor = 5f;
116 private int body_autodisable_frames = 20;
117
118
119 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
120 | CollisionCategories.Space
121 | CollisionCategories.Body
122 | CollisionCategories.Character
123 );
124 private bool m_taintshape;
125 private bool m_taintPhysics;
126 private bool m_collidesLand = true;
127 private bool m_collidesWater;
128 public bool m_returnCollisions;
129
130 // Default we're a Geometry
131 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
132
133 // Default, Collide with Other Geometries, spaces and Bodies
134 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
135
136 public bool m_taintremove;
137 public bool m_taintdisable;
138 public bool m_disabled;
139 public bool m_taintadd;
140 public bool m_taintselected;
141 public bool m_taintCollidesWater;
142
143 public uint m_localID;
144
145 //public GCHandle gc;
146 private CollisionLocker ode;
147
148 private bool m_taintforce = false;
149 private bool m_taintaddangularforce = false;
150 private Vector3 m_force;
151 private List<Vector3> m_forcelist = new List<Vector3>();
152 private List<Vector3> m_angularforcelist = new List<Vector3>();
153
154 private IMesh _mesh;
155 private PrimitiveBaseShape _pbs;
156 private OdeScene _parent_scene;
157 public IntPtr m_targetSpace = IntPtr.Zero;
158 public IntPtr prim_geom;
159 public IntPtr prev_geom;
160 public IntPtr _triMeshData;
161
162 private IntPtr _linkJointGroup = IntPtr.Zero;
163 private PhysicsActor _parent;
164 private PhysicsActor m_taintparent;
165
166 private List<OdePrim> childrenPrim = new List<OdePrim>();
167
168 private bool iscolliding;
169 private bool m_isphysical;
170 private bool m_isSelected;
171
172 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
173
174 private bool m_throttleUpdates;
175 private int throttleCounter;
176 public int m_interpenetrationcount;
177 public float m_collisionscore;
178 public int m_roundsUnderMotionThreshold;
179 private int m_crossingfailures;
180
181 public bool outofBounds;
182 private float m_density = 10.000006836f; // Aluminum g/cm3;
183
184 public bool _zeroFlag;
185 private bool m_lastUpdateSent;
186
187 public IntPtr Body = IntPtr.Zero;
188 public String m_primName;
189 private Vector3 _target_velocity;
190 public d.Mass pMass;
191
192 public int m_eventsubscription;
193 private CollisionEventUpdate CollisionEventsThisFrame;
194
195 private IntPtr m_linkJoint = IntPtr.Zero;
196
197 public volatile bool childPrim;
198
199 private ODEDynamics m_vehicle;
200
201 internal int m_material = (int)Material.Wood;
202
203 private int frcount = 0; // Used to limit dynamics debug output to
204
205
206 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
207 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
208 {
209 m_vehicle = new ODEDynamics();
210 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
211 ode = dode;
212 if (!pos.IsFinite())
213 {
214 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
215 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
216 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
217 }
218 _position = pos;
219 m_taintposition = pos;
220 PID_D = parent_scene.bodyPIDD;
221 PID_G = parent_scene.bodyPIDG;
222 m_density = parent_scene.geomDefaultDensity;
223 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
224 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
225
226
227 prim_geom = IntPtr.Zero;
228 prev_geom = IntPtr.Zero;
229
230 if (!pos.IsFinite())
231 {
232 size = new Vector3(0.5f, 0.5f, 0.5f);
233 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
234 }
235
236 if (size.X <= 0) size.X = 0.01f;
237 if (size.Y <= 0) size.Y = 0.01f;
238 if (size.Z <= 0) size.Z = 0.01f;
239
240 _size = size;
241 m_taintsize = _size;
242
243 if (!QuaternionIsFinite(rotation))
244 {
245 rotation = Quaternion.Identity;
246 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
247 }
248
249 _orientation = rotation;
250 m_taintrot = _orientation;
251 _mesh = mesh;
252 _pbs = pbs;
253
254 _parent_scene = parent_scene;
255 m_targetSpace = (IntPtr)0;
256
257 if (pos.Z < 0)
258 m_isphysical = false;
259 else
260 {
261 m_isphysical = pisPhysical;
262 // If we're physical, we need to be in the master space for now.
263 // linksets *should* be in a space together.. but are not currently
264 if (m_isphysical)
265 m_targetSpace = _parent_scene.space;
266 }
267 m_primName = primName;
268 m_taintadd = true;
269 _parent_scene.AddPhysicsActorTaint(this);
270 // don't do .add() here; old geoms get recycled with the same hash
271 }
272
273 public override int PhysicsActorType
274 {
275 get { return (int) ActorTypes.Prim; }
276 set { return; }
277 }
278
279 public override bool SetAlwaysRun
280 {
281 get { return false; }
282 set { return; }
283 }
284
285 public override uint LocalID
286 {
287 set {
288 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
289 m_localID = value; }
290 }
291
292 public override bool Grabbed
293 {
294 set { return; }
295 }
296
297 public override bool Selected
298 {
299 set {
300
301
302 // This only makes the object not collidable if the object
303 // is physical or the object is modified somehow *IN THE FUTURE*
304 // without this, if an avatar selects prim, they can walk right
305 // through it while it's selected
306 m_collisionscore = 0;
307 if ((m_isphysical && !_zeroFlag) || !value)
308 {
309 m_taintselected = value;
310 _parent_scene.AddPhysicsActorTaint(this);
311 }
312 else
313 {
314 m_taintselected = value;
315 m_isSelected = value;
316 }
317 if(m_isSelected) disableBodySoft();
318 }
319 }
320
321 public void SetGeom(IntPtr geom)
322 {
323 prev_geom = prim_geom;
324 prim_geom = geom;
325//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
326 if (prim_geom != IntPtr.Zero)
327 {
328 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
329 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
330 }
331
332 if (childPrim)
333 {
334 if (_parent != null && _parent is OdePrim)
335 {
336 OdePrim parent = (OdePrim)_parent;
337//Console.WriteLine("SetGeom calls ChildSetGeom");
338 parent.ChildSetGeom(this);
339 }
340 }
341 //m_log.Warn("Setting Geom to: " + prim_geom);
342 }
343
344
345
346 public void enableBodySoft()
347 {
348 if (!childPrim)
349 {
350 if (m_isphysical && Body != IntPtr.Zero)
351 {
352 d.BodyEnable(Body);
353 if (m_vehicle.Type != Vehicle.TYPE_NONE)
354 m_vehicle.Enable(Body, _parent_scene);
355 }
356
357 m_disabled = false;
358 }
359 }
360
361 public void disableBodySoft()
362 {
363 m_disabled = true;
364
365 if (m_isphysical && Body != IntPtr.Zero)
366 {
367 d.BodyDisable(Body);
368 }
369 }
370
371 public void enableBody()
372 {
373 // Don't enable this body if we're a child prim
374 // this should be taken care of in the parent function not here
375 if (!childPrim)
376 {
377 // Sets the geom to a body
378 Body = d.BodyCreate(_parent_scene.world);
379
380 setMass();
381 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
382 d.Quaternion myrot = new d.Quaternion();
383 myrot.X = _orientation.X;
384 myrot.Y = _orientation.Y;
385 myrot.Z = _orientation.Z;
386 myrot.W = _orientation.W;
387 d.BodySetQuaternion(Body, ref myrot);
388 d.GeomSetBody(prim_geom, Body);
389 m_collisionCategories |= CollisionCategories.Body;
390 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
391
392 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
393 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
394
395 d.BodySetAutoDisableFlag(Body, true);
396 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
397
398 // disconnect from world gravity so we can apply buoyancy
399 d.BodySetGravityMode (Body, false);
400
401 m_interpenetrationcount = 0;
402 m_collisionscore = 0;
403 m_disabled = false;
404
405 // The body doesn't already have a finite rotation mode set here
406 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
407 {
408 createAMotor(m_angularlock);
409 }
410 if (m_vehicle.Type != Vehicle.TYPE_NONE)
411 {
412 m_vehicle.Enable(Body, _parent_scene);
413 }
414
415 _parent_scene.addActivePrim(this);
416 }
417 }
418
419 #region Mass Calculation
420
421 private float CalculateMass()
422 {
423 float volume = 0;
424
425 // No material is passed to the physics engines yet.. soo..
426 // we're using the m_density constant in the class definition
427
428 float returnMass = 0;
429
430 switch (_pbs.ProfileShape)
431 {
432 case ProfileShape.Square:
433 // Profile Volume
434
435 volume = _size.X*_size.Y*_size.Z;
436
437 // If the user has 'hollowed out'
438 // ProfileHollow is one of those 0 to 50000 values :P
439 // we like percentages better.. so turning into a percentage
440
441 if (((float) _pbs.ProfileHollow/50000f) > 0.0)
442 {
443 float hollowAmount = (float) _pbs.ProfileHollow/50000f;
444
445 // calculate the hollow volume by it's shape compared to the prim shape
446 float hollowVolume = 0;
447 switch (_pbs.HollowShape)
448 {
449 case HollowShape.Square:
450 case HollowShape.Same:
451 // Cube Hollow volume calculation
452 float hollowsizex = _size.X*hollowAmount;
453 float hollowsizey = _size.Y*hollowAmount;
454 float hollowsizez = _size.Z*hollowAmount;
455 hollowVolume = hollowsizex*hollowsizey*hollowsizez;
456 break;
457
458 case HollowShape.Circle:
459 // Hollow shape is a perfect cyllinder in respect to the cube's scale
460 // Cyllinder hollow volume calculation
461 float hRadius = _size.X/2;
462 float hLength = _size.Z;
463
464 // pi * r2 * h
465 hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
466 break;
467
468 case HollowShape.Triangle:
469 // Equilateral Triangular Prism volume hollow calculation
470 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
471
472 float aLength = _size.Y;
473 // 1/2 abh
474 hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
475 break;
476
477 default:
478 hollowVolume = 0;
479 break;
480 }
481 volume = volume - hollowVolume;
482 }
483
484 break;
485 case ProfileShape.Circle:
486 if (_pbs.PathCurve == (byte)Extrusion.Straight)
487 {
488 // Cylinder
489 float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
490 float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
491
492 // Approximating the cylinder's irregularity.
493 if (volume1 > volume2)
494 {
495 volume = (float)volume1 - (volume1 - volume2);
496 }
497 else if (volume2 > volume1)
498 {
499 volume = (float)volume2 - (volume2 - volume1);
500 }
501 else
502 {
503 // Regular cylinder
504 volume = volume1;
505 }
506 }
507 else
508 {
509 // We don't know what the shape is yet, so use default
510 volume = _size.X * _size.Y * _size.Z;
511 }
512 // If the user has 'hollowed out'
513 // ProfileHollow is one of those 0 to 50000 values :P
514 // we like percentages better.. so turning into a percentage
515
516 if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
517 {
518 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
519
520 // calculate the hollow volume by it's shape compared to the prim shape
521 float hollowVolume = 0;
522 switch (_pbs.HollowShape)
523 {
524 case HollowShape.Same:
525 case HollowShape.Circle:
526 // Hollow shape is a perfect cyllinder in respect to the cube's scale
527 // Cyllinder hollow volume calculation
528 float hRadius = _size.X / 2;
529 float hLength = _size.Z;
530
531 // pi * r2 * h
532 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
533 break;
534
535 case HollowShape.Square:
536 // Cube Hollow volume calculation
537 float hollowsizex = _size.X * hollowAmount;
538 float hollowsizey = _size.Y * hollowAmount;
539 float hollowsizez = _size.Z * hollowAmount;
540 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
541 break;
542
543 case HollowShape.Triangle:
544 // Equilateral Triangular Prism volume hollow calculation
545 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
546
547 float aLength = _size.Y;
548 // 1/2 abh
549 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
550 break;
551
552 default:
553 hollowVolume = 0;
554 break;
555 }
556 volume = volume - hollowVolume;
557 }
558 break;
559
560 case ProfileShape.HalfCircle:
561 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
562 {
563 if (_size.X == _size.Y && _size.Y == _size.Z)
564 {
565 // regular sphere
566 // v = 4/3 * pi * r^3
567 float sradius3 = (float)Math.Pow((_size.X / 2), 3);
568 volume = (float)((4f / 3f) * Math.PI * sradius3);
569 }
570 else
571 {
572 // we treat this as a box currently
573 volume = _size.X * _size.Y * _size.Z;
574 }
575 }
576 else
577 {
578 // We don't know what the shape is yet, so use default
579 volume = _size.X * _size.Y * _size.Z;
580 }
581 break;
582
583 case ProfileShape.EquilateralTriangle:
584 /*
585 v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
586
587 // seed mesh
588 Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
589 Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
590 Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
591 */
592 float xA = -0.25f * _size.X;
593 float yA = -0.45f * _size.Y;
594
595 float xB = 0.5f * _size.X;
596 float yB = 0;
597
598 float xC = -0.25f * _size.X;
599 float yC = 0.45f * _size.Y;
600
601 volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
602
603 // If the user has 'hollowed out'
604 // ProfileHollow is one of those 0 to 50000 values :P
605 // we like percentages better.. so turning into a percentage
606 float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
607 if (((float)fhollowFactor / 50000f) > 0.0)
608 {
609 float hollowAmount = (float)fhollowFactor / 50000f;
610
611 // calculate the hollow volume by it's shape compared to the prim shape
612 float hollowVolume = 0;
613 switch (_pbs.HollowShape)
614 {
615 case HollowShape.Same:
616 case HollowShape.Triangle:
617 // Equilateral Triangular Prism volume hollow calculation
618 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
619
620 float aLength = _size.Y;
621 // 1/2 abh
622 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
623 break;
624
625 case HollowShape.Square:
626 // Cube Hollow volume calculation
627 float hollowsizex = _size.X * hollowAmount;
628 float hollowsizey = _size.Y * hollowAmount;
629 float hollowsizez = _size.Z * hollowAmount;
630 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
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 float hRadius = _size.X / 2;
637 float hLength = _size.Z;
638
639 // pi * r2 * h
640 hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
641 break;
642
643 default:
644 hollowVolume = 0;
645 break;
646 }
647 volume = volume - hollowVolume;
648 }
649 break;
650
651 default:
652 // we don't have all of the volume formulas yet so
653 // use the common volume formula for all
654 volume = _size.X*_size.Y*_size.Z;
655 break;
656 }
657
658 // Calculate Path cut effect on volume
659 // Not exact, in the triangle hollow example
660 // They should never be zero or less then zero..
661 // we'll ignore it if it's less then zero
662
663 // ProfileEnd and ProfileBegin are values
664 // from 0 to 50000
665
666 // Turning them back into percentages so that I can cut that percentage off the volume
667
668 float PathCutEndAmount = _pbs.ProfileEnd;
669 float PathCutStartAmount = _pbs.ProfileBegin;
670 if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
671 {
672 float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
673
674 // Check the return amount for sanity
675 if (pathCutAmount >= 0.99f)
676 pathCutAmount = 0.99f;
677
678 volume = volume - (volume*pathCutAmount);
679 }
680 UInt16 taperX = _pbs.PathScaleX;
681 UInt16 taperY = _pbs.PathScaleY;
682 float taperFactorX = 0;
683 float taperFactorY = 0;
684
685 // Mass = density * volume
686 if (taperX != 100)
687 {
688 if (taperX > 100)
689 {
690 taperFactorX = 1.0f - ((float)taperX / 200);
691 //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
692 }
693 else
694 {
695 taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
696 //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
697 }
698 volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
699 }
700
701 if (taperY != 100)
702 {
703 if (taperY > 100)
704 {
705 taperFactorY = 1.0f - ((float)taperY / 200);
706 //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
707 }
708 else
709 {
710 taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
711 //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
712 }
713 volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
714 }
715 returnMass = m_density*volume;
716 if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
717
718
719
720 // Recursively calculate mass
721 bool HasChildPrim = false;
722 lock (childrenPrim)
723 {
724 if (childrenPrim.Count > 0)
725 {
726 HasChildPrim = true;
727 }
728
729 }
730 if (HasChildPrim)
731 {
732 OdePrim[] childPrimArr = new OdePrim[0];
733
734 lock (childrenPrim)
735 childPrimArr = childrenPrim.ToArray();
736
737 for (int i = 0; i < childPrimArr.Length; i++)
738 {
739 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
740 returnMass += childPrimArr[i].CalculateMass();
741 // failsafe, this shouldn't happen but with OpenSim, you never know :)
742 if (i > 256)
743 break;
744 }
745 }
746 if (returnMass > _parent_scene.maximumMassObject)
747 returnMass = _parent_scene.maximumMassObject;
748 return returnMass;
749 }// end CalculateMass
750
751 #endregion
752
753 public void setMass()
754 {
755 if (Body != (IntPtr) 0)
756 {
757 float newmass = CalculateMass();
758
759 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
760
761 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
762 d.BodySetMass(Body, ref pMass);
763 }
764 }
765
766 public void disableBody()
767 {
768 //this kills the body so things like 'mesh' can re-create it.
769 lock (this)
770 {
771 if (!childPrim)
772 {
773 if (Body != IntPtr.Zero)
774 {
775 _parent_scene.remActivePrim(this);
776 m_collisionCategories &= ~CollisionCategories.Body;
777 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
778
779 if (prim_geom != IntPtr.Zero)
780 {
781 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
782 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
783 }
784
785
786 d.BodyDestroy(Body);
787 lock (childrenPrim)
788 {
789 if (childrenPrim.Count > 0)
790 {
791 foreach (OdePrim prm in childrenPrim)
792 {
793 _parent_scene.remActivePrim(prm);
794 prm.Body = IntPtr.Zero;
795 }
796 }
797 }
798 Body = IntPtr.Zero;
799 }
800 }
801 else
802 {
803 _parent_scene.remActivePrim(this);
804
805 m_collisionCategories &= ~CollisionCategories.Body;
806 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
807
808 if (prim_geom != IntPtr.Zero)
809 {
810 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
811 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
812 }
813
814
815 Body = IntPtr.Zero;
816 }
817 }
818 m_disabled = true;
819 m_collisionscore = 0;
820 }
821
822 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
823
824 public void setMesh(OdeScene parent_scene, IMesh mesh)
825 {
826 // This sleeper is there to moderate how long it takes between
827 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
828
829 //Thread.Sleep(10);
830
831 //Kill Body so that mesh can re-make the geom
832 if (IsPhysical && Body != IntPtr.Zero)
833 {
834 if (childPrim)
835 {
836 if (_parent != null)
837 {
838 OdePrim parent = (OdePrim)_parent;
839 parent.ChildDelink(this);
840 }
841 }
842 else
843 {
844 disableBody();
845 }
846 }
847
848 IntPtr vertices, indices;
849 int vertexCount, indexCount;
850 int vertexStride, triStride;
851 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
852 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
853
854 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
855 if (m_MeshToTriMeshMap.ContainsKey(mesh))
856 {
857 _triMeshData = m_MeshToTriMeshMap[mesh];
858 }
859 else
860 {
861 _triMeshData = d.GeomTriMeshDataCreate();
862
863 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
864 d.GeomTriMeshDataPreprocess(_triMeshData);
865 m_MeshToTriMeshMap[mesh] = _triMeshData;
866 }
867
868 _parent_scene.waitForSpaceUnlock(m_targetSpace);
869 try
870 {
871 if (prim_geom == IntPtr.Zero)
872 {
873 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
874 }
875 }
876 catch (AccessViolationException)
877 {
878 m_log.Error("[PHYSICS]: MESH LOCKED");
879 return;
880 }
881
882
883 // if (IsPhysical && Body == (IntPtr) 0)
884 // {
885 // Recreate the body
886 // m_interpenetrationcount = 0;
887 // m_collisionscore = 0;
888
889 // enableBody();
890 // }
891 }
892
893 public void ProcessTaints(float timestep)
894 {
895//Console.WriteLine("ProcessTaints for " + m_primName );
896 if (m_taintadd)
897 {
898 changeadd(timestep);
899 }
900
901 if (prim_geom != IntPtr.Zero)
902 {
903 if (!_position.ApproxEquals(m_taintposition, 0f))
904 changemove(timestep);
905
906 if (m_taintrot != _orientation)
907 {
908 if(childPrim && IsPhysical) // For physical child prim...
909 {
910 rotate(timestep);
911 // KF: ODE will also rotate the parent prim!
912 // so rotate the root back to where it was
913 OdePrim parent = (OdePrim)_parent;
914 parent.rotate(timestep);
915 }
916 else
917 {
918 //Just rotate the prim
919 rotate(timestep);
920 }
921 }
922 //
923
924 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
925 changePhysicsStatus(timestep);
926 //
927
928 if (!_size.ApproxEquals(m_taintsize,0f))
929 changesize(timestep);
930 //
931
932 if (m_taintshape)
933 changeshape(timestep);
934 //
935
936 if (m_taintforce)
937 changeAddForce(timestep);
938
939 if (m_taintaddangularforce)
940 changeAddAngularForce(timestep);
941
942 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
943 changeSetTorque(timestep);
944
945 if (m_taintdisable)
946 changedisable(timestep);
947
948 if (m_taintselected != m_isSelected)
949 changeSelectedStatus(timestep);
950
951 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
952 changevelocity(timestep);
953
954 if (m_taintparent != _parent)
955 changelink(timestep);
956
957 if (m_taintCollidesWater != m_collidesWater)
958 changefloatonwater(timestep);
959
960 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
961 changeAngularLock(timestep);
962
963 }
964 else
965 {
966 m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)");
967 }
968 }
969
970
971 private void changeAngularLock(float timestep)
972 {
973 // do we have a Physical object?
974 if (Body != IntPtr.Zero)
975 {
976 //Check that we have a Parent
977 //If we have a parent then we're not authorative here
978 if (_parent == null)
979 {
980 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
981 {
982 //d.BodySetFiniteRotationMode(Body, 0);
983 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
984 createAMotor(m_taintAngularLock);
985 }
986 else
987 {
988 if (Amotor != IntPtr.Zero)
989 {
990 d.JointDestroy(Amotor);
991 Amotor = IntPtr.Zero;
992 }
993 }
994 }
995 }
996 // Store this for later in case we get turned into a separate body
997 m_angularlock = m_taintAngularLock;
998
999 }
1000
1001 private void changelink(float timestep)
1002 {
1003 // If the newly set parent is not null
1004 // create link
1005 if (_parent == null && m_taintparent != null)
1006 {
1007 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1008 {
1009 OdePrim obj = (OdePrim)m_taintparent;
1010 //obj.disableBody();
1011//Console.WriteLine("changelink calls ParentPrim");
1012 obj.ParentPrim(this);
1013
1014 /*
1015 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1016 {
1017 _linkJointGroup = d.JointGroupCreate(0);
1018 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1019 d.JointAttach(m_linkJoint, obj.Body, Body);
1020 d.JointSetFixed(m_linkJoint);
1021 }
1022 */
1023 }
1024 }
1025 // If the newly set parent is null
1026 // destroy link
1027 else if (_parent != null && m_taintparent == null)
1028 {
1029//Console.WriteLine(" changelink B");
1030
1031 if (_parent is OdePrim)
1032 {
1033 OdePrim obj = (OdePrim)_parent;
1034 obj.ChildDelink(this);
1035 childPrim = false;
1036 //_parent = null;
1037 }
1038
1039 /*
1040 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1041 d.JointGroupDestroy(_linkJointGroup);
1042
1043 _linkJointGroup = (IntPtr)0;
1044 m_linkJoint = (IntPtr)0;
1045 */
1046 }
1047
1048 _parent = m_taintparent;
1049 m_taintPhysics = m_isphysical;
1050 }
1051
1052 // I'm the parent
1053 // prim is the child
1054 public void ParentPrim(OdePrim prim)
1055 {
1056//Console.WriteLine("ParentPrim " + m_primName);
1057 if (this.m_localID != prim.m_localID)
1058 {
1059 if (Body == IntPtr.Zero)
1060 {
1061 Body = d.BodyCreate(_parent_scene.world);
1062 setMass();
1063 }
1064 if (Body != IntPtr.Zero)
1065 {
1066 lock (childrenPrim)
1067 {
1068 if (!childrenPrim.Contains(prim))
1069 {
1070//Console.WriteLine("childrenPrim.Add " + prim);
1071 childrenPrim.Add(prim);
1072
1073 foreach (OdePrim prm in childrenPrim)
1074 {
1075 d.Mass m2;
1076 d.MassSetZero(out m2);
1077 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1078
1079
1080 d.Quaternion quat = new d.Quaternion();
1081 quat.W = prm._orientation.W;
1082 quat.X = prm._orientation.X;
1083 quat.Y = prm._orientation.Y;
1084 quat.Z = prm._orientation.Z;
1085
1086 d.Matrix3 mat = new d.Matrix3();
1087 d.RfromQ(out mat, ref quat);
1088 d.MassRotate(ref m2, ref mat);
1089 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1090 d.MassAdd(ref pMass, ref m2);
1091 }
1092 foreach (OdePrim prm in childrenPrim)
1093 {
1094
1095 prm.m_collisionCategories |= CollisionCategories.Body;
1096 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1097
1098 if (prm.prim_geom == IntPtr.Zero)
1099 {
1100 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1101 continue;
1102 }
1103//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1104 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1105 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1106
1107
1108 d.Quaternion quat = new d.Quaternion();
1109 quat.W = prm._orientation.W;
1110 quat.X = prm._orientation.X;
1111 quat.Y = prm._orientation.Y;
1112 quat.Z = prm._orientation.Z;
1113
1114 d.Matrix3 mat = new d.Matrix3();
1115 d.RfromQ(out mat, ref quat);
1116 if (Body != IntPtr.Zero)
1117 {
1118 d.GeomSetBody(prm.prim_geom, Body);
1119 prm.childPrim = true;
1120 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1121 //d.GeomSetOffsetPosition(prim.prim_geom,
1122 // (Position.X - prm.Position.X) - pMass.c.X,
1123 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1124 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1125 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1126 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1127 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1128 d.BodySetMass(Body, ref pMass);
1129 }
1130 else
1131 {
1132 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1133 }
1134
1135
1136 prm.m_interpenetrationcount = 0;
1137 prm.m_collisionscore = 0;
1138 prm.m_disabled = false;
1139
1140 // The body doesn't already have a finite rotation mode set here
1141 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1142 {
1143 prm.createAMotor(m_angularlock);
1144 }
1145 prm.Body = Body;
1146 _parent_scene.addActivePrim(prm);
1147 }
1148 m_collisionCategories |= CollisionCategories.Body;
1149 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1150
1151//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1152 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1153//Console.WriteLine(" Post GeomSetCategoryBits 2");
1154 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1155
1156
1157 d.Quaternion quat2 = new d.Quaternion();
1158 quat2.W = _orientation.W;
1159 quat2.X = _orientation.X;
1160 quat2.Y = _orientation.Y;
1161 quat2.Z = _orientation.Z;
1162
1163 d.Matrix3 mat2 = new d.Matrix3();
1164 d.RfromQ(out mat2, ref quat2);
1165 d.GeomSetBody(prim_geom, Body);
1166 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1167 //d.GeomSetOffsetPosition(prim.prim_geom,
1168 // (Position.X - prm.Position.X) - pMass.c.X,
1169 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1170 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1171 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1172 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1173 d.BodySetMass(Body, ref pMass);
1174
1175 d.BodySetAutoDisableFlag(Body, true);
1176 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1177
1178
1179 m_interpenetrationcount = 0;
1180 m_collisionscore = 0;
1181 m_disabled = false;
1182
1183 // The body doesn't already have a finite rotation mode set here
1184 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1185 {
1186 createAMotor(m_angularlock);
1187 }
1188 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1189 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene);
1190 _parent_scene.addActivePrim(this);
1191 }
1192 }
1193 }
1194 }
1195
1196 }
1197
1198 private void ChildSetGeom(OdePrim odePrim)
1199 {
1200 //if (m_isphysical && Body != IntPtr.Zero)
1201 lock (childrenPrim)
1202 {
1203 foreach (OdePrim prm in childrenPrim)
1204 {
1205 //prm.childPrim = true;
1206 prm.disableBody();
1207 //prm.m_taintparent = null;
1208 //prm._parent = null;
1209 //prm.m_taintPhysics = false;
1210 //prm.m_disabled = true;
1211 //prm.childPrim = false;
1212 }
1213 }
1214 disableBody();
1215
1216
1217 if (Body != IntPtr.Zero)
1218 {
1219 _parent_scene.remActivePrim(this);
1220 }
1221
1222 lock (childrenPrim)
1223 {
1224 foreach (OdePrim prm in childrenPrim)
1225 {
1226//Console.WriteLine("ChildSetGeom calls ParentPrim");
1227 ParentPrim(prm);
1228 }
1229 }
1230
1231 }
1232
1233 private void ChildDelink(OdePrim odePrim)
1234 {
1235 // Okay, we have a delinked child.. need to rebuild the body.
1236 lock (childrenPrim)
1237 {
1238 foreach (OdePrim prm in childrenPrim)
1239 {
1240 prm.childPrim = true;
1241 prm.disableBody();
1242 //prm.m_taintparent = null;
1243 //prm._parent = null;
1244 //prm.m_taintPhysics = false;
1245 //prm.m_disabled = true;
1246 //prm.childPrim = false;
1247 }
1248 }
1249 disableBody();
1250
1251 lock (childrenPrim)
1252 {
1253 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1254 childrenPrim.Remove(odePrim);
1255 }
1256
1257
1258
1259
1260 if (Body != IntPtr.Zero)
1261 {
1262 _parent_scene.remActivePrim(this);
1263 }
1264
1265
1266
1267 lock (childrenPrim)
1268 {
1269 foreach (OdePrim prm in childrenPrim)
1270 {
1271//Console.WriteLine("ChildDelink calls ParentPrim");
1272 ParentPrim(prm);
1273 }
1274 }
1275
1276
1277 }
1278
1279 private void changeSelectedStatus(float timestep)
1280 {
1281 if (m_taintselected)
1282 {
1283 m_collisionCategories = CollisionCategories.Selected;
1284 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1285
1286 // We do the body disable soft twice because 'in theory' a collision could have happened
1287 // in between the disabling and the collision properties setting
1288 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1289 // through the ground.
1290
1291 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1292 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1293 // so that causes the selected part to wake up and continue moving.
1294
1295 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1296 // assembly will stop simulating during the selection, because of the lack of atomicity
1297 // of select operations (their processing could be interrupted by a thread switch, causing
1298 // simulation to continue before all of the selected object notifications trickle down to
1299 // the physics engine).
1300
1301 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1302 // selected and disabled. then, due to a thread switch, the selection processing is
1303 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1304 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1305 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1306 // up, start simulating again, which in turn wakes up the last 50.
1307
1308 if (m_isphysical)
1309 {
1310 disableBodySoft();
1311 }
1312
1313 if (prim_geom != IntPtr.Zero)
1314 {
1315 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1316 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1317 }
1318
1319 if (m_isphysical)
1320 {
1321 disableBodySoft();
1322 }
1323 }
1324 else
1325 {
1326 m_collisionCategories = CollisionCategories.Geom;
1327
1328 if (m_isphysical)
1329 m_collisionCategories |= CollisionCategories.Body;
1330
1331 m_collisionFlags = m_default_collisionFlags;
1332
1333 if (m_collidesLand)
1334 m_collisionFlags |= CollisionCategories.Land;
1335 if (m_collidesWater)
1336 m_collisionFlags |= CollisionCategories.Water;
1337
1338 if (prim_geom != IntPtr.Zero)
1339 {
1340 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1341 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1342 }
1343 if (m_isphysical)
1344 {
1345 if (Body != IntPtr.Zero)
1346 {
1347 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1348 d.BodySetForce(Body, 0, 0, 0);
1349 enableBodySoft();
1350 }
1351 }
1352 }
1353
1354 resetCollisionAccounting();
1355 m_isSelected = m_taintselected;
1356 }//end changeSelectedStatus
1357
1358 public void ResetTaints()
1359 {
1360 m_taintposition = _position;
1361 m_taintrot = _orientation;
1362 m_taintPhysics = m_isphysical;
1363 m_taintselected = m_isSelected;
1364 m_taintsize = _size;
1365 m_taintshape = false;
1366 m_taintforce = false;
1367 m_taintdisable = false;
1368 m_taintVelocity = Vector3.Zero;
1369 }
1370
1371 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1372 {
1373//Console.WriteLine("CreateGeom:");
1374 if (_mesh != null)
1375 {
1376 setMesh(_parent_scene, _mesh);
1377 }
1378 else
1379 {
1380 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1381 {
1382 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1383 {
1384 if (((_size.X / 2f) > 0f))
1385 {
1386 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1387 try
1388 {
1389//Console.WriteLine(" CreateGeom 1");
1390 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1391 }
1392 catch (AccessViolationException)
1393 {
1394 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1395 ode.dunlock(_parent_scene.world);
1396 return;
1397 }
1398 }
1399 else
1400 {
1401 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1402 try
1403 {
1404//Console.WriteLine(" CreateGeom 2");
1405 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1406 }
1407 catch (AccessViolationException)
1408 {
1409 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1410 ode.dunlock(_parent_scene.world);
1411 return;
1412 }
1413 }
1414 }
1415 else
1416 {
1417 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1418 try
1419 {
1420//Console.WriteLine(" CreateGeom 3");
1421 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1422 }
1423 catch (AccessViolationException)
1424 {
1425 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1426 ode.dunlock(_parent_scene.world);
1427 return;
1428 }
1429 }
1430 }
1431
1432 else
1433 {
1434 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1435 try
1436 {
1437//Console.WriteLine(" CreateGeom 4");
1438 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1439 }
1440 catch (AccessViolationException)
1441 {
1442 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1443 ode.dunlock(_parent_scene.world);
1444 return;
1445 }
1446 }
1447 }
1448 }
1449
1450 public void changeadd(float timestep)
1451 {
1452 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1453 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1454
1455 if (targetspace == IntPtr.Zero)
1456 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1457
1458 m_targetSpace = targetspace;
1459
1460 if (_mesh == null)
1461 {
1462 if (_parent_scene.needsMeshing(_pbs))
1463 {
1464 // Don't need to re-enable body.. it's done in SetMesh
1465 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1466 // createmesh returns null when it's a shape that isn't a cube.
1467 // m_log.Debug(m_localID);
1468 }
1469 }
1470
1471
1472 lock (_parent_scene.OdeLock)
1473 {
1474//Console.WriteLine("changeadd 1");
1475 CreateGeom(m_targetSpace, _mesh);
1476
1477 if (prim_geom != IntPtr.Zero)
1478 {
1479 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1480 d.Quaternion myrot = new d.Quaternion();
1481 myrot.X = _orientation.X;
1482 myrot.Y = _orientation.Y;
1483 myrot.Z = _orientation.Z;
1484 myrot.W = _orientation.W;
1485 d.GeomSetQuaternion(prim_geom, ref myrot);
1486 }
1487
1488 if (m_isphysical && Body == IntPtr.Zero)
1489 {
1490 enableBody();
1491 }
1492 }
1493
1494 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
1495 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
1496
1497 changeSelectedStatus(timestep);
1498
1499 m_taintadd = false;
1500 }
1501
1502 public void changemove(float timestep)
1503 {
1504 if (m_isphysical)
1505 {
1506
1507 if (!m_disabled && !m_taintremove && !childPrim)
1508 {
1509 if (Body == IntPtr.Zero)
1510 enableBody();
1511 //Prim auto disable after 20 frames,
1512 //if you move it, re-enable the prim manually.
1513 if (_parent != null)
1514 {
1515 if (m_linkJoint != IntPtr.Zero)
1516 {
1517 d.JointDestroy(m_linkJoint);
1518 m_linkJoint = IntPtr.Zero;
1519 }
1520 }
1521 if (Body != IntPtr.Zero)
1522 {
1523 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1524
1525 if (_parent != null)
1526 {
1527 OdePrim odParent = (OdePrim)_parent;
1528 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1529 {
1530// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1531Console.WriteLine(" JointCreateFixed");
1532 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1533 d.JointAttach(m_linkJoint, Body, odParent.Body);
1534 d.JointSetFixed(m_linkJoint);
1535 }
1536 }
1537 d.BodyEnable(Body);
1538 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1539 {
1540 m_vehicle.Enable(Body, _parent_scene);
1541 }
1542 }
1543 else
1544 {
1545 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
1546 }
1547 }
1548 //else
1549 // {
1550 //m_log.Debug("[BUG]: race!");
1551 //}
1552 }
1553 else
1554 {
1555 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1556 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1557 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1558
1559 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1560 m_targetSpace = tempspace;
1561
1562 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1563 if (prim_geom != IntPtr.Zero)
1564 {
1565 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1566
1567 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1568 d.SpaceAdd(m_targetSpace, prim_geom);
1569 }
1570 }
1571
1572 changeSelectedStatus(timestep);
1573
1574 resetCollisionAccounting();
1575 m_taintposition = _position;
1576 }
1577
1578 public void Move(float timestep)
1579 {
1580 float fx = 0;
1581 float fy = 0;
1582 float fz = 0;
1583
1584 frcount++; // used to limit debug comment output
1585 if (frcount > 100)
1586 frcount = 0;
1587
1588 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1589 {
1590//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
1591 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
1592 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1593 {
1594 // 'VEHICLES' are dealt with in ODEDynamics.cs
1595 m_vehicle.Step(timestep, _parent_scene);
1596 }
1597 else
1598 {
1599 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1600 // NON-'VEHICLES' are dealt with here
1601 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1602 {
1603 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1604 if (m_angularlock.X == 1)
1605 avel2.X = 0;
1606 if (m_angularlock.Y == 1)
1607 avel2.Y = 0;
1608 if (m_angularlock.Z == 1)
1609 avel2.Z = 0;
1610 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1611 }
1612 //float PID_P = 900.0f;
1613
1614 float m_mass = CalculateMass();
1615
1616// fz = 0f;
1617 //m_log.Info(m_collisionFlags.ToString());
1618
1619
1620 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
1621 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1622 // NB Prims in ODE are no subject to global gravity
1623 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
1624
1625 if (m_usePID)
1626 {
1627//if(frcount == 0) Console.WriteLine("PID " + m_primName);
1628 // KF - this is for object MoveToTarget.
1629
1630 //if (!d.BodyIsEnabled(Body))
1631 //d.BodySetForce(Body, 0f, 0f, 0f);
1632
1633 // no lock; for now it's only called from within Simulate()
1634
1635 // If the PID Controller isn't active then we set our force
1636 // calculating base velocity to the current position
1637
1638 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1639 {
1640 //PID_G = PID_G / m_PIDTau;
1641 m_PIDTau = 1;
1642 }
1643
1644 if ((PID_G - m_PIDTau) <= 0)
1645 {
1646 PID_G = m_PIDTau + 1;
1647 }
1648 //PidStatus = true;
1649
1650 // PhysicsVector vec = new PhysicsVector();
1651 d.Vector3 vel = d.BodyGetLinearVel(Body);
1652
1653 d.Vector3 pos = d.BodyGetPosition(Body);
1654 _target_velocity =
1655 new Vector3(
1656 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1657 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1658 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1659 );
1660
1661 // if velocity is zero, use position control; otherwise, velocity control
1662
1663 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1664 {
1665 // keep track of where we stopped. No more slippin' & slidin'
1666
1667 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1668 // react to the physics scene by moving it's position.
1669 // Avatar to Avatar collisions
1670 // Prim to avatar collisions
1671
1672 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1673 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1674 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1675 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1676 d.BodySetLinearVel(Body, 0, 0, 0);
1677 d.BodyAddForce(Body, 0, 0, fz);
1678 return;
1679 }
1680 else
1681 {
1682 _zeroFlag = false;
1683
1684 // We're flying and colliding with something
1685 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1686 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1687
1688 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1689
1690 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1691 }
1692 } // end if (m_usePID)
1693
1694 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1695 if (m_useHoverPID && !m_usePID)
1696 {
1697//Console.WriteLine("Hover " + m_primName);
1698
1699 // If we're using the PID controller, then we have no gravity
1700 fz = (-1 * _parent_scene.gravityz) * m_mass;
1701
1702 // no lock; for now it's only called from within Simulate()
1703
1704 // If the PID Controller isn't active then we set our force
1705 // calculating base velocity to the current position
1706
1707 if ((m_PIDTau < 1))
1708 {
1709 PID_G = PID_G / m_PIDTau;
1710 }
1711
1712 if ((PID_G - m_PIDTau) <= 0)
1713 {
1714 PID_G = m_PIDTau + 1;
1715 }
1716
1717
1718 // Where are we, and where are we headed?
1719 d.Vector3 pos = d.BodyGetPosition(Body);
1720 d.Vector3 vel = d.BodyGetLinearVel(Body);
1721
1722
1723 // Non-Vehicles have a limited set of Hover options.
1724 // determine what our target height really is based on HoverType
1725 switch (m_PIDHoverType)
1726 {
1727 case PIDHoverType.Ground:
1728 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1729 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1730 break;
1731 case PIDHoverType.GroundAndWater:
1732 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1733 m_waterHeight = _parent_scene.GetWaterLevel();
1734 if (m_groundHeight > m_waterHeight)
1735 {
1736 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1737 }
1738 else
1739 {
1740 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1741 }
1742 break;
1743
1744 } // end switch (m_PIDHoverType)
1745
1746
1747 _target_velocity =
1748 new Vector3(0.0f, 0.0f,
1749 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1750 );
1751
1752 // if velocity is zero, use position control; otherwise, velocity control
1753
1754 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1755 {
1756 // keep track of where we stopped. No more slippin' & slidin'
1757
1758 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1759 // react to the physics scene by moving it's position.
1760 // Avatar to Avatar collisions
1761 // Prim to avatar collisions
1762
1763 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1764 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1765 d.BodyAddForce(Body, 0, 0, fz);
1766 //KF this prevents furthur motions return;
1767 }
1768 else
1769 {
1770 _zeroFlag = false;
1771
1772 // We're flying and colliding with something
1773 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1774 }
1775 } // end m_useHoverPID && !m_usePID
1776
1777 if (m_useAPID)
1778 {
1779 // RotLookAt, apparently overrides all other rotation sources. Inputs:
1780 // Quaternion m_APIDTarget
1781 // float m_APIDStrength // From SL experiments, this is the time to get there
1782 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
1783 // Also in SL the mass of the object has no effect on time to get there.
1784 // Factors:
1785//if(frcount == 0) Console.WriteLine("APID ");
1786 // get present body rotation
1787 float limit = 1.0f;
1788 float scaler = 50f; // adjusts damping time
1789 float RLAservo = 0f;
1790
1791 d.Quaternion rot = d.BodyGetQuaternion(Body);
1792 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
1793 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
1794 float diff_angle;
1795 Vector3 diff_axis;
1796 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
1797 diff_axis.Normalize();
1798 if(diff_angle > 0.01f) // diff_angle is always +ve
1799 {
1800// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
1801 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
1802 rotforce = rotforce * rotq;
1803 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
1804// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
1805 // rotforce = rotforce * RLAservo * diff_angle ;
1806 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1807 RLAservo = timestep / m_APIDStrength * scaler;
1808 rotforce = rotforce * RLAservo * diff_angle ;
1809 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1810//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1811 }
1812//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
1813 } // end m_useAPID
1814
1815 fx *= m_mass;
1816 fy *= m_mass;
1817 //fz *= m_mass;
1818
1819 fx += m_force.X;
1820 fy += m_force.Y;
1821 fz += m_force.Z;
1822
1823 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1824 if (fx != 0 || fy != 0 || fz != 0)
1825 {
1826 //m_taintdisable = true;
1827 //base.RaiseOutOfBounds(Position);
1828 //d.BodySetLinearVel(Body, fx, fy, 0f);
1829 if (!d.BodyIsEnabled(Body))
1830 {
1831 // A physical body at rest on a surface will auto-disable after a while,
1832 // this appears to re-enable it incase the surface it is upon vanishes,
1833 // and the body should fall again.
1834 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1835 d.BodySetForce(Body, 0, 0, 0);
1836 enableBodySoft();
1837 }
1838
1839 // 35x10 = 350n times the mass per second applied maximum.
1840 float nmax = 35f * m_mass;
1841 float nmin = -35f * m_mass;
1842
1843
1844 if (fx > nmax)
1845 fx = nmax;
1846 if (fx < nmin)
1847 fx = nmin;
1848 if (fy > nmax)
1849 fy = nmax;
1850 if (fy < nmin)
1851 fy = nmin;
1852 d.BodyAddForce(Body, fx, fy, fz);
1853//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1854 }
1855 }
1856 }
1857 else
1858 { // is not physical, or is not a body or is selected
1859 // _zeroPosition = d.BodyGetPosition(Body);
1860 return;
1861//Console.WriteLine("Nothing " + m_primName);
1862
1863 }
1864 }
1865
1866
1867
1868 public void rotate(float timestep)
1869 {
1870 d.Quaternion myrot = new d.Quaternion();
1871 myrot.X = _orientation.X;
1872 myrot.Y = _orientation.Y;
1873 myrot.Z = _orientation.Z;
1874 myrot.W = _orientation.W;
1875 if (Body != IntPtr.Zero)
1876 {
1877 // KF: If this is a root prim do BodySet
1878 d.BodySetQuaternion(Body, ref myrot);
1879 if (m_isphysical)
1880 {
1881 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1882 createAMotor(m_angularlock);
1883 }
1884 }
1885 else
1886 {
1887 // daughter prim, do Geom set
1888 d.GeomSetQuaternion(prim_geom, ref myrot);
1889 }
1890
1891 resetCollisionAccounting();
1892 m_taintrot = _orientation;
1893 }
1894
1895 private void resetCollisionAccounting()
1896 {
1897 m_collisionscore = 0;
1898 m_interpenetrationcount = 0;
1899 m_disabled = false;
1900 }
1901
1902 public void changedisable(float timestep)
1903 {
1904 m_disabled = true;
1905 if (Body != IntPtr.Zero)
1906 {
1907 d.BodyDisable(Body);
1908 Body = IntPtr.Zero;
1909 }
1910
1911 m_taintdisable = false;
1912 }
1913
1914 public void changePhysicsStatus(float timestep)
1915 {
1916 if (m_isphysical == true)
1917 {
1918 if (Body == IntPtr.Zero)
1919 {
1920 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
1921 {
1922 changeshape(2f);
1923 }
1924 else
1925 {
1926 enableBody();
1927 }
1928 }
1929 }
1930 else
1931 {
1932 if (Body != IntPtr.Zero)
1933 {
1934 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
1935 {
1936
1937
1938 if (prim_geom != IntPtr.Zero)
1939 {
1940 try
1941 {
1942 d.GeomDestroy(prim_geom);
1943 prim_geom = IntPtr.Zero;
1944 _mesh = null;
1945 }
1946 catch (System.AccessViolationException)
1947 {
1948 prim_geom = IntPtr.Zero;
1949 m_log.Error("[PHYSICS]: PrimGeom dead");
1950 }
1951 }
1952//Console.WriteLine("changePhysicsStatus for " + m_primName );
1953 changeadd(2f);
1954 }
1955 if (childPrim)
1956 {
1957 if (_parent != null)
1958 {
1959 OdePrim parent = (OdePrim)_parent;
1960 parent.ChildDelink(this);
1961 }
1962 }
1963 else
1964 {
1965 disableBody();
1966 }
1967 }
1968 }
1969
1970 changeSelectedStatus(timestep);
1971
1972 resetCollisionAccounting();
1973 m_taintPhysics = m_isphysical;
1974 }
1975
1976 public void changesize(float timestamp)
1977 {
1978
1979 string oldname = _parent_scene.geom_name_map[prim_geom];
1980
1981 if (_size.X <= 0) _size.X = 0.01f;
1982 if (_size.Y <= 0) _size.Y = 0.01f;
1983 if (_size.Z <= 0) _size.Z = 0.01f;
1984
1985 // Cleanup of old prim geometry
1986 if (_mesh != null)
1987 {
1988 // Cleanup meshing here
1989 }
1990 //kill body to rebuild
1991 if (IsPhysical && Body != IntPtr.Zero)
1992 {
1993 if (childPrim)
1994 {
1995 if (_parent != null)
1996 {
1997 OdePrim parent = (OdePrim)_parent;
1998 parent.ChildDelink(this);
1999 }
2000 }
2001 else
2002 {
2003 disableBody();
2004 }
2005 }
2006 if (d.SpaceQuery(m_targetSpace, prim_geom))
2007 {
2008 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2009 d.SpaceRemove(m_targetSpace, prim_geom);
2010 }
2011 d.GeomDestroy(prim_geom);
2012 prim_geom = IntPtr.Zero;
2013 // we don't need to do space calculation because the client sends a position update also.
2014
2015 // Construction of new prim
2016 if (_parent_scene.needsMeshing(_pbs))
2017 {
2018 float meshlod = _parent_scene.meshSculptLOD;
2019
2020 if (IsPhysical)
2021 meshlod = _parent_scene.MeshSculptphysicalLOD;
2022 // Don't need to re-enable body.. it's done in SetMesh
2023
2024 IMesh mesh = null;
2025
2026 if (_parent_scene.needsMeshing(_pbs))
2027 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2028
2029 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2030//Console.WriteLine("changesize 1");
2031 CreateGeom(m_targetSpace, mesh);
2032
2033
2034 }
2035 else
2036 {
2037 _mesh = null;
2038//Console.WriteLine("changesize 2");
2039 CreateGeom(m_targetSpace, _mesh);
2040 }
2041
2042 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2043 d.Quaternion myrot = new d.Quaternion();
2044 myrot.X = _orientation.X;
2045 myrot.Y = _orientation.Y;
2046 myrot.Z = _orientation.Z;
2047 myrot.W = _orientation.W;
2048 d.GeomSetQuaternion(prim_geom, ref myrot);
2049
2050 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2051 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2052 {
2053 // Re creates body on size.
2054 // EnableBody also does setMass()
2055 enableBody();
2056 d.BodyEnable(Body);
2057 }
2058
2059 _parent_scene.geom_name_map[prim_geom] = oldname;
2060
2061 changeSelectedStatus(timestamp);
2062 if (childPrim)
2063 {
2064 if (_parent is OdePrim)
2065 {
2066 OdePrim parent = (OdePrim)_parent;
2067 parent.ChildSetGeom(this);
2068 }
2069 }
2070 resetCollisionAccounting();
2071 m_taintsize = _size;
2072 }
2073
2074
2075
2076 public void changefloatonwater(float timestep)
2077 {
2078 m_collidesWater = m_taintCollidesWater;
2079
2080 if (prim_geom != IntPtr.Zero)
2081 {
2082 if (m_collidesWater)
2083 {
2084 m_collisionFlags |= CollisionCategories.Water;
2085 }
2086 else
2087 {
2088 m_collisionFlags &= ~CollisionCategories.Water;
2089 }
2090 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2091 }
2092 }
2093
2094 public void changeshape(float timestamp)
2095 {
2096 string oldname = _parent_scene.geom_name_map[prim_geom];
2097
2098 // Cleanup of old prim geometry and Bodies
2099 if (IsPhysical && Body != IntPtr.Zero)
2100 {
2101 if (childPrim)
2102 {
2103 if (_parent != null)
2104 {
2105 OdePrim parent = (OdePrim)_parent;
2106 parent.ChildDelink(this);
2107 }
2108 }
2109 else
2110 {
2111 disableBody();
2112 }
2113 }
2114 try
2115 {
2116 d.GeomDestroy(prim_geom);
2117 }
2118 catch (System.AccessViolationException)
2119 {
2120 prim_geom = IntPtr.Zero;
2121 m_log.Error("[PHYSICS]: PrimGeom dead");
2122 }
2123 prim_geom = IntPtr.Zero;
2124 // we don't need to do space calculation because the client sends a position update also.
2125 if (_size.X <= 0) _size.X = 0.01f;
2126 if (_size.Y <= 0) _size.Y = 0.01f;
2127 if (_size.Z <= 0) _size.Z = 0.01f;
2128 // Construction of new prim
2129
2130 if (_parent_scene.needsMeshing(_pbs))
2131 {
2132 // Don't need to re-enable body.. it's done in SetMesh
2133 float meshlod = _parent_scene.meshSculptLOD;
2134
2135 if (IsPhysical)
2136 meshlod = _parent_scene.MeshSculptphysicalLOD;
2137
2138 IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2139 // createmesh returns null when it doesn't mesh.
2140 CreateGeom(m_targetSpace, mesh);
2141 }
2142 else
2143 {
2144 _mesh = null;
2145//Console.WriteLine("changeshape");
2146 CreateGeom(m_targetSpace, null);
2147 }
2148
2149 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2150 d.Quaternion myrot = new d.Quaternion();
2151 //myrot.W = _orientation.w;
2152 myrot.W = _orientation.W;
2153 myrot.X = _orientation.X;
2154 myrot.Y = _orientation.Y;
2155 myrot.Z = _orientation.Z;
2156 d.GeomSetQuaternion(prim_geom, ref myrot);
2157
2158 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2159 if (IsPhysical && Body == IntPtr.Zero)
2160 {
2161 // Re creates body on size.
2162 // EnableBody also does setMass()
2163 enableBody();
2164 if (Body != IntPtr.Zero)
2165 {
2166 d.BodyEnable(Body);
2167 }
2168 }
2169 _parent_scene.geom_name_map[prim_geom] = oldname;
2170
2171 changeSelectedStatus(timestamp);
2172 if (childPrim)
2173 {
2174 if (_parent is OdePrim)
2175 {
2176 OdePrim parent = (OdePrim)_parent;
2177 parent.ChildSetGeom(this);
2178 }
2179 }
2180 resetCollisionAccounting();
2181 m_taintshape = false;
2182 }
2183
2184 public void changeAddForce(float timestamp)
2185 {
2186 if (!m_isSelected)
2187 {
2188 lock (m_forcelist)
2189 {
2190 //m_log.Info("[PHYSICS]: dequeing forcelist");
2191 if (IsPhysical)
2192 {
2193 Vector3 iforce = Vector3.Zero;
2194 int i = 0;
2195 try
2196 {
2197 for (i = 0; i < m_forcelist.Count; i++)
2198 {
2199
2200 iforce = iforce + (m_forcelist[i] * 100);
2201 }
2202 }
2203 catch (IndexOutOfRangeException)
2204 {
2205 m_forcelist = new List<Vector3>();
2206 m_collisionscore = 0;
2207 m_interpenetrationcount = 0;
2208 m_taintforce = false;
2209 return;
2210 }
2211 catch (ArgumentOutOfRangeException)
2212 {
2213 m_forcelist = new List<Vector3>();
2214 m_collisionscore = 0;
2215 m_interpenetrationcount = 0;
2216 m_taintforce = false;
2217 return;
2218 }
2219 d.BodyEnable(Body);
2220 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2221 }
2222 m_forcelist.Clear();
2223 }
2224
2225 m_collisionscore = 0;
2226 m_interpenetrationcount = 0;
2227 }
2228
2229 m_taintforce = false;
2230
2231 }
2232
2233
2234
2235 public void changeSetTorque(float timestamp)
2236 {
2237 if (!m_isSelected)
2238 {
2239 if (IsPhysical && Body != IntPtr.Zero)
2240 {
2241 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2242 }
2243 }
2244
2245 m_taintTorque = Vector3.Zero;
2246 }
2247
2248 public void changeAddAngularForce(float timestamp)
2249 {
2250 if (!m_isSelected)
2251 {
2252 lock (m_angularforcelist)
2253 {
2254 //m_log.Info("[PHYSICS]: dequeing forcelist");
2255 if (IsPhysical)
2256 {
2257 Vector3 iforce = Vector3.Zero;
2258 for (int i = 0; i < m_angularforcelist.Count; i++)
2259 {
2260 iforce = iforce + (m_angularforcelist[i] * 100);
2261 }
2262 d.BodyEnable(Body);
2263 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2264
2265 }
2266 m_angularforcelist.Clear();
2267 }
2268
2269 m_collisionscore = 0;
2270 m_interpenetrationcount = 0;
2271 }
2272
2273 m_taintaddangularforce = false;
2274 }
2275
2276 private void changevelocity(float timestep)
2277 {
2278 if (!m_isSelected)
2279 {
2280 Thread.Sleep(20);
2281 if (IsPhysical)
2282 {
2283 if (Body != IntPtr.Zero)
2284 {
2285 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2286 }
2287 }
2288
2289 //resetCollisionAccounting();
2290 }
2291 m_taintVelocity = Vector3.Zero;
2292 }
2293
2294 public override bool IsPhysical
2295 {
2296 get { return m_isphysical; }
2297 set {
2298 m_isphysical = value;
2299 if (!m_isphysical) // Zero the remembered last velocity
2300 m_lastVelocity = Vector3.Zero;
2301 }
2302 }
2303
2304 public void setPrimForRemoval()
2305 {
2306 m_taintremove = true;
2307 }
2308
2309 public override bool Flying
2310 {
2311 // no flying prims for you
2312 get { return false; }
2313 set { }
2314 }
2315
2316 public override bool IsColliding
2317 {
2318 get { return iscolliding; }
2319 set { iscolliding = value; }
2320 }
2321
2322 public override bool CollidingGround
2323 {
2324 get { return false; }
2325 set { return; }
2326 }
2327
2328 public override bool CollidingObj
2329 {
2330 get { return false; }
2331 set { return; }
2332 }
2333
2334 public override bool ThrottleUpdates
2335 {
2336 get { return m_throttleUpdates; }
2337 set { m_throttleUpdates = value; }
2338 }
2339
2340 public override bool Stopped
2341 {
2342 get { return _zeroFlag; }
2343 }
2344
2345 public override Vector3 Position
2346 {
2347 get { return _position; }
2348
2349 set { _position = value;
2350 //m_log.Info("[PHYSICS]: " + _position.ToString());
2351 }
2352 }
2353
2354 public override Vector3 Size
2355 {
2356 get { return _size; }
2357 set
2358 {
2359 if (value.IsFinite())
2360 {
2361 _size = value;
2362 }
2363 else
2364 {
2365 m_log.Warn("[PHYSICS]: Got NaN Size on object");
2366 }
2367 }
2368 }
2369
2370 public override float Mass
2371 {
2372 get { return CalculateMass(); }
2373 }
2374
2375 public override Vector3 Force
2376 {
2377 //get { return Vector3.Zero; }
2378 get { return m_force; }
2379 set
2380 {
2381 if (value.IsFinite())
2382 {
2383 m_force = value;
2384 }
2385 else
2386 {
2387 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
2388 }
2389 }
2390 }
2391
2392 public override int VehicleType
2393 {
2394 get { return (int)m_vehicle.Type; }
2395 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2396 }
2397
2398 public override void VehicleFloatParam(int param, float value)
2399 {
2400 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2401 }
2402
2403 public override void VehicleVectorParam(int param, Vector3 value)
2404 {
2405 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2406 }
2407
2408 public override void VehicleRotationParam(int param, Quaternion rotation)
2409 {
2410 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2411 }
2412
2413 public override void SetVolumeDetect(int param)
2414 {
2415 lock (_parent_scene.OdeLock)
2416 {
2417 m_isVolumeDetect = (param!=0);
2418 }
2419 }
2420
2421 public override Vector3 CenterOfMass
2422 {
2423 get { return Vector3.Zero; }
2424 }
2425
2426 public override Vector3 GeometricCenter
2427 {
2428 get { return Vector3.Zero; }
2429 }
2430
2431 public override PrimitiveBaseShape Shape
2432 {
2433 set
2434 {
2435 _pbs = value;
2436 m_taintshape = true;
2437 }
2438 }
2439
2440 public override Vector3 Velocity
2441 {
2442 get
2443 {
2444 // Averate previous velocity with the new one so
2445 // client object interpolation works a 'little' better
2446 if (_zeroFlag)
2447 return Vector3.Zero;
2448
2449 Vector3 returnVelocity = Vector3.Zero;
2450 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
2451 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
2452 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
2453 return returnVelocity;
2454 }
2455 set
2456 {
2457 if (value.IsFinite())
2458 {
2459 _velocity = value;
2460
2461 m_taintVelocity = value;
2462 _parent_scene.AddPhysicsActorTaint(this);
2463 }
2464 else
2465 {
2466 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
2467 }
2468
2469 }
2470 }
2471
2472 public override Vector3 Torque
2473 {
2474 get
2475 {
2476 if (!m_isphysical || Body == IntPtr.Zero)
2477 return Vector3.Zero;
2478
2479 return _torque;
2480 }
2481
2482 set
2483 {
2484 if (value.IsFinite())
2485 {
2486 m_taintTorque = value;
2487 _parent_scene.AddPhysicsActorTaint(this);
2488 }
2489 else
2490 {
2491 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
2492 }
2493 }
2494 }
2495
2496 public override float CollisionScore
2497 {
2498 get { return m_collisionscore; }
2499 set { m_collisionscore = value; }
2500 }
2501
2502 public override bool Kinematic
2503 {
2504 get { return false; }
2505 set { }
2506 }
2507
2508 public override Quaternion Orientation
2509 {
2510 get { return _orientation; }
2511 set
2512 {
2513 if (QuaternionIsFinite(value))
2514 {
2515 _orientation = value;
2516 }
2517 else
2518 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2519
2520 }
2521 }
2522
2523 internal static bool QuaternionIsFinite(Quaternion q)
2524 {
2525 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2526 return false;
2527 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2528 return false;
2529 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2530 return false;
2531 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2532 return false;
2533 return true;
2534 }
2535
2536 public override Vector3 Acceleration
2537 {
2538 get { return _acceleration; }
2539 }
2540
2541
2542 public void SetAcceleration(Vector3 accel)
2543 {
2544 _acceleration = accel;
2545 }
2546
2547 public override void AddForce(Vector3 force, bool pushforce)
2548 {
2549 if (force.IsFinite())
2550 {
2551 lock (m_forcelist)
2552 m_forcelist.Add(force);
2553
2554 m_taintforce = true;
2555 }
2556 else
2557 {
2558 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
2559 }
2560 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2561 }
2562
2563 public override void AddAngularForce(Vector3 force, bool pushforce)
2564 {
2565 if (force.IsFinite())
2566 {
2567 m_angularforcelist.Add(force);
2568 m_taintaddangularforce = true;
2569 }
2570 else
2571 {
2572 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
2573 }
2574 }
2575
2576 public override Vector3 RotationalVelocity
2577 {
2578 get
2579 {
2580 Vector3 pv = Vector3.Zero;
2581 if (_zeroFlag)
2582 return pv;
2583 m_lastUpdateSent = false;
2584
2585 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2586 return pv;
2587
2588 return m_rotationalVelocity;
2589 }
2590 set
2591 {
2592 if (value.IsFinite())
2593 {
2594 m_rotationalVelocity = value;
2595 }
2596 else
2597 {
2598 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
2599 }
2600 }
2601 }
2602
2603 public override void CrossingFailure()
2604 {
2605 m_crossingfailures++;
2606 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2607 {
2608 base.RaiseOutOfBounds(_position);
2609 return;
2610 }
2611 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2612 {
2613 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
2614 }
2615 }
2616
2617 public override float Buoyancy
2618 {
2619 get { return m_buoyancy; }
2620 set { m_buoyancy = value; }
2621 }
2622
2623 public override void link(PhysicsActor obj)
2624 {
2625 m_taintparent = obj;
2626 }
2627
2628 public override void delink()
2629 {
2630 m_taintparent = null;
2631 }
2632
2633 public override void LockAngularMotion(Vector3 axis)
2634 {
2635 // reverse the zero/non zero values for ODE.
2636 if (axis.IsFinite())
2637 {
2638 axis.X = (axis.X > 0) ? 1f : 0f;
2639 axis.Y = (axis.Y > 0) ? 1f : 0f;
2640 axis.Z = (axis.Z > 0) ? 1f : 0f;
2641 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2642 m_taintAngularLock = axis;
2643 }
2644 else
2645 {
2646 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
2647 }
2648 }
2649
2650 public void UpdatePositionAndVelocity()
2651 {
2652 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2653 if (_parent == null)
2654 {
2655 Vector3 pv = Vector3.Zero;
2656 bool lastZeroFlag = _zeroFlag;
2657 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2658 {
2659 d.Vector3 vec = d.BodyGetPosition(Body);
2660 d.Quaternion ori = d.BodyGetQuaternion(Body);
2661 d.Vector3 vel = d.BodyGetLinearVel(Body);
2662 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2663 d.Vector3 torque = d.BodyGetTorque(Body);
2664 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2665 Vector3 l_position = Vector3.Zero;
2666 Quaternion l_orientation = Quaternion.Identity;
2667
2668 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2669 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2670 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2671 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2672 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2673
2674 m_lastposition = _position;
2675 m_lastorientation = _orientation;
2676
2677 l_position.X = vec.X;
2678 l_position.Y = vec.Y;
2679 l_position.Z = vec.Z;
2680 l_orientation.X = ori.X;
2681 l_orientation.Y = ori.Y;
2682 l_orientation.Z = ori.Z;
2683 l_orientation.W = ori.W;
2684
2685// if(l_position.Y != m_lastposition.Y){
2686// Console.WriteLine("UP&V {0} {1}", m_primName, l_position);
2687// }
2688
2689 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)
2690 {
2691 //base.RaiseOutOfBounds(l_position);
2692
2693 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2694 {
2695 _position = l_position;
2696 //_parent_scene.remActivePrim(this);
2697 if (_parent == null)
2698 base.RequestPhysicsterseUpdate();
2699 return;
2700 }
2701 else
2702 {
2703 if (_parent == null)
2704 base.RaiseOutOfBounds(l_position);
2705 return;
2706 }
2707 }
2708
2709 if (l_position.Z < 0)
2710 {
2711 // This is so prim that get lost underground don't fall forever and suck up
2712 //
2713 // Sim resources and memory.
2714 // Disables the prim's movement physics....
2715 // It's a hack and will generate a console message if it fails.
2716
2717 //IsPhysical = false;
2718 if (_parent == null)
2719 base.RaiseOutOfBounds(_position);
2720
2721 _acceleration.X = 0;
2722 _acceleration.Y = 0;
2723 _acceleration.Z = 0;
2724
2725 _velocity.X = 0;
2726 _velocity.Y = 0;
2727 _velocity.Z = 0;
2728 m_rotationalVelocity.X = 0;
2729 m_rotationalVelocity.Y = 0;
2730 m_rotationalVelocity.Z = 0;
2731
2732 if (_parent == null)
2733 base.RequestPhysicsterseUpdate();
2734
2735 m_throttleUpdates = false;
2736 throttleCounter = 0;
2737 _zeroFlag = true;
2738 //outofBounds = true;
2739 }
2740
2741 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2742//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2743 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2744 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2745 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2746// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2747 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2748 {
2749 _zeroFlag = true;
2750//Console.WriteLine("ZFT 2");
2751 m_throttleUpdates = false;
2752 }
2753 else
2754 {
2755 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2756 _zeroFlag = false;
2757 m_lastUpdateSent = false;
2758 //m_throttleUpdates = false;
2759 }
2760
2761 if (_zeroFlag)
2762 {
2763 _velocity.X = 0.0f;
2764 _velocity.Y = 0.0f;
2765 _velocity.Z = 0.0f;
2766
2767 _acceleration.X = 0;
2768 _acceleration.Y = 0;
2769 _acceleration.Z = 0;
2770
2771 //_orientation.w = 0f;
2772 //_orientation.X = 0f;
2773 //_orientation.Y = 0f;
2774 //_orientation.Z = 0f;
2775 m_rotationalVelocity.X = 0;
2776 m_rotationalVelocity.Y = 0;
2777 m_rotationalVelocity.Z = 0;
2778 if (!m_lastUpdateSent)
2779 {
2780 m_throttleUpdates = false;
2781 throttleCounter = 0;
2782 m_rotationalVelocity = pv;
2783
2784 if (_parent == null)
2785 {
2786 base.RequestPhysicsterseUpdate();
2787 }
2788
2789 m_lastUpdateSent = true;
2790 }
2791 }
2792 else
2793 {
2794 if (lastZeroFlag != _zeroFlag)
2795 {
2796 if (_parent == null)
2797 {
2798 base.RequestPhysicsterseUpdate();
2799 }
2800 }
2801
2802 m_lastVelocity = _velocity;
2803
2804 _position = l_position;
2805
2806 _velocity.X = vel.X;
2807 _velocity.Y = vel.Y;
2808 _velocity.Z = vel.Z;
2809
2810 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2811 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2812 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2813
2814 if (_velocity.ApproxEquals(pv, 0.5f))
2815 {
2816 m_rotationalVelocity = pv;
2817 }
2818 else
2819 {
2820 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2821 }
2822
2823 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2824 _orientation.X = ori.X;
2825 _orientation.Y = ori.Y;
2826 _orientation.Z = ori.Z;
2827 _orientation.W = ori.W;
2828 m_lastUpdateSent = false;
2829 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2830 {
2831 if (_parent == null)
2832 {
2833 base.RequestPhysicsterseUpdate();
2834 }
2835 }
2836 else
2837 {
2838 throttleCounter++;
2839 }
2840 }
2841 m_lastposition = l_position;
2842 }
2843 else
2844 {
2845 // Not a body.. so Make sure the client isn't interpolating
2846 _velocity.X = 0;
2847 _velocity.Y = 0;
2848 _velocity.Z = 0;
2849
2850 _acceleration.X = 0;
2851 _acceleration.Y = 0;
2852 _acceleration.Z = 0;
2853
2854 m_rotationalVelocity.X = 0;
2855 m_rotationalVelocity.Y = 0;
2856 m_rotationalVelocity.Z = 0;
2857 _zeroFlag = true;
2858 }
2859 }
2860 }
2861
2862 public override bool FloatOnWater
2863 {
2864 set {
2865 m_taintCollidesWater = value;
2866 _parent_scene.AddPhysicsActorTaint(this);
2867 }
2868 }
2869
2870 public override void SetMomentum(Vector3 momentum)
2871 {
2872 }
2873
2874 public override Vector3 PIDTarget
2875 {
2876 set
2877 {
2878 if (value.IsFinite())
2879 {
2880 m_PIDTarget = value;
2881 }
2882 else
2883 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
2884 }
2885 }
2886 public override bool PIDActive { set { m_usePID = value; } }
2887 public override float PIDTau { set { m_PIDTau = value; } }
2888
2889 // For RotLookAt
2890 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
2891 public override bool APIDActive { set { m_useAPID = value; } }
2892 public override float APIDStrength { set { m_APIDStrength = value; } }
2893 public override float APIDDamping { set { m_APIDDamping = value; } }
2894
2895 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2896 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2897 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2898 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2899
2900 private void createAMotor(Vector3 axis)
2901 {
2902 if (Body == IntPtr.Zero)
2903 return;
2904
2905 if (Amotor != IntPtr.Zero)
2906 {
2907 d.JointDestroy(Amotor);
2908 Amotor = IntPtr.Zero;
2909 }
2910
2911 float axisnum = 3;
2912
2913 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2914
2915 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2916
2917
2918 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2919 d.Mass objMass;
2920 d.MassSetZero(out objMass);
2921 DMassCopy(ref pMass, ref objMass);
2922
2923 //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);
2924
2925 Matrix4 dMassMat = FromDMass(objMass);
2926
2927 Matrix4 mathmat = Inverse(dMassMat);
2928
2929 /*
2930 //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]);
2931
2932 mathmat = Inverse(mathmat);
2933
2934
2935 objMass = FromMatrix4(mathmat, ref objMass);
2936 //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);
2937
2938 mathmat = Inverse(mathmat);
2939 */
2940 if (axis.X == 0)
2941 {
2942 mathmat.M33 = 50.0000001f;
2943 //objMass.I.M22 = 0;
2944 }
2945 if (axis.Y == 0)
2946 {
2947 mathmat.M22 = 50.0000001f;
2948 //objMass.I.M11 = 0;
2949 }
2950 if (axis.Z == 0)
2951 {
2952 mathmat.M11 = 50.0000001f;
2953 //objMass.I.M00 = 0;
2954 }
2955
2956
2957
2958 mathmat = Inverse(mathmat);
2959 objMass = FromMatrix4(mathmat, ref objMass);
2960 //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);
2961
2962 //return;
2963 if (d.MassCheck(ref objMass))
2964 {
2965 d.BodySetMass(Body, ref objMass);
2966 }
2967 else
2968 {
2969 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
2970 }
2971
2972 if (axisnum <= 0)
2973 return;
2974 // int dAMotorEuler = 1;
2975
2976 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
2977 d.JointAttach(Amotor, Body, IntPtr.Zero);
2978 d.JointSetAMotorMode(Amotor, 0);
2979
2980 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
2981 int i = 0;
2982
2983 if (axis.X == 0)
2984 {
2985 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
2986 i++;
2987 }
2988
2989 if (axis.Y == 0)
2990 {
2991 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
2992 i++;
2993 }
2994
2995 if (axis.Z == 0)
2996 {
2997 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
2998 i++;
2999 }
3000
3001 for (int j = 0; j < (int)axisnum; j++)
3002 {
3003 //d.JointSetAMotorAngle(Amotor, j, 0);
3004 }
3005
3006 //d.JointSetAMotorAngle(Amotor, 1, 0);
3007 //d.JointSetAMotorAngle(Amotor, 2, 0);
3008
3009 // These lowstops and high stops are effectively (no wiggle room)
3010 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3011 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3012 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3013 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3014 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3015 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3016 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3017 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3018 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3019
3020 }
3021
3022 public Matrix4 FromDMass(d.Mass pMass)
3023 {
3024 Matrix4 obj;
3025 obj.M11 = pMass.I.M00;
3026 obj.M12 = pMass.I.M01;
3027 obj.M13 = pMass.I.M02;
3028 obj.M14 = 0;
3029 obj.M21 = pMass.I.M10;
3030 obj.M22 = pMass.I.M11;
3031 obj.M23 = pMass.I.M12;
3032 obj.M24 = 0;
3033 obj.M31 = pMass.I.M20;
3034 obj.M32 = pMass.I.M21;
3035 obj.M33 = pMass.I.M22;
3036 obj.M34 = 0;
3037 obj.M41 = 0;
3038 obj.M42 = 0;
3039 obj.M43 = 0;
3040 obj.M44 = 1;
3041 return obj;
3042 }
3043
3044 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3045 {
3046 obj.I.M00 = pMat[0, 0];
3047 obj.I.M01 = pMat[0, 1];
3048 obj.I.M02 = pMat[0, 2];
3049 obj.I.M10 = pMat[1, 0];
3050 obj.I.M11 = pMat[1, 1];
3051 obj.I.M12 = pMat[1, 2];
3052 obj.I.M20 = pMat[2, 0];
3053 obj.I.M21 = pMat[2, 1];
3054 obj.I.M22 = pMat[2, 2];
3055 return obj;
3056 }
3057
3058 public override void SubscribeEvents(int ms)
3059 {
3060 m_eventsubscription = ms;
3061 _parent_scene.addCollisionEventReporting(this);
3062 }
3063
3064 public override void UnSubscribeEvents()
3065 {
3066 _parent_scene.remCollisionEventReporting(this);
3067 m_eventsubscription = 0;
3068 }
3069
3070 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3071 {
3072 if (CollisionEventsThisFrame == null)
3073 CollisionEventsThisFrame = new CollisionEventUpdate();
3074 CollisionEventsThisFrame.addCollider(CollidedWith, contact);
3075 }
3076
3077 public void SendCollisions()
3078 {
3079 if (CollisionEventsThisFrame == null)
3080 return;
3081
3082 base.SendCollisionUpdate(CollisionEventsThisFrame);
3083
3084 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
3085 CollisionEventsThisFrame = null;
3086 else
3087 CollisionEventsThisFrame = new CollisionEventUpdate();
3088 }
3089
3090 public override bool SubscribedEvents()
3091 {
3092 if (m_eventsubscription > 0)
3093 return true;
3094 return false;
3095 }
3096
3097 public static Matrix4 Inverse(Matrix4 pMat)
3098 {
3099 if (determinant3x3(pMat) == 0)
3100 {
3101 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3102 }
3103
3104
3105
3106 return (Adjoint(pMat) / determinant3x3(pMat));
3107 }
3108
3109 public static Matrix4 Adjoint(Matrix4 pMat)
3110 {
3111 Matrix4 adjointMatrix = new Matrix4();
3112 for (int i=0; i<4; i++)
3113 {
3114 for (int j=0; j<4; j++)
3115 {
3116 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3117 }
3118 }
3119
3120 adjointMatrix = Transpose(adjointMatrix);
3121 return adjointMatrix;
3122 }
3123
3124 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3125 {
3126 Matrix4 minor = new Matrix4();
3127 int m = 0, n = 0;
3128 for (int i = 0; i < 4; i++)
3129 {
3130 if (i == iRow)
3131 continue;
3132 n = 0;
3133 for (int j = 0; j < 4; j++)
3134 {
3135 if (j == iCol)
3136 continue;
3137 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3138 n++;
3139 }
3140 m++;
3141 }
3142 return minor;
3143 }
3144
3145 public static Matrix4 Transpose(Matrix4 pMat)
3146 {
3147 Matrix4 transposeMatrix = new Matrix4();
3148 for (int i = 0; i < 4; i++)
3149 for (int j = 0; j < 4; j++)
3150 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3151 return transposeMatrix;
3152 }
3153
3154 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3155 {
3156 switch (r)
3157 {
3158 case 0:
3159 switch (c)
3160 {
3161 case 0:
3162 pMat.M11 = val;
3163 break;
3164 case 1:
3165 pMat.M12 = val;
3166 break;
3167 case 2:
3168 pMat.M13 = val;
3169 break;
3170 case 3:
3171 pMat.M14 = val;
3172 break;
3173 }
3174
3175 break;
3176 case 1:
3177 switch (c)
3178 {
3179 case 0:
3180 pMat.M21 = val;
3181 break;
3182 case 1:
3183 pMat.M22 = val;
3184 break;
3185 case 2:
3186 pMat.M23 = val;
3187 break;
3188 case 3:
3189 pMat.M24 = val;
3190 break;
3191 }
3192
3193 break;
3194 case 2:
3195 switch (c)
3196 {
3197 case 0:
3198 pMat.M31 = val;
3199 break;
3200 case 1:
3201 pMat.M32 = val;
3202 break;
3203 case 2:
3204 pMat.M33 = val;
3205 break;
3206 case 3:
3207 pMat.M34 = val;
3208 break;
3209 }
3210
3211 break;
3212 case 3:
3213 switch (c)
3214 {
3215 case 0:
3216 pMat.M41 = val;
3217 break;
3218 case 1:
3219 pMat.M42 = val;
3220 break;
3221 case 2:
3222 pMat.M43 = val;
3223 break;
3224 case 3:
3225 pMat.M44 = val;
3226 break;
3227 }
3228
3229 break;
3230 }
3231 }
3232 private static float determinant3x3(Matrix4 pMat)
3233 {
3234 float det = 0;
3235 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3236 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3237 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3238 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3239 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3240 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3241
3242 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3243 return det;
3244
3245 }
3246
3247 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3248 {
3249 dst.c.W = src.c.W;
3250 dst.c.X = src.c.X;
3251 dst.c.Y = src.c.Y;
3252 dst.c.Z = src.c.Z;
3253 dst.mass = src.mass;
3254 dst.I.M00 = src.I.M00;
3255 dst.I.M01 = src.I.M01;
3256 dst.I.M02 = src.I.M02;
3257 dst.I.M10 = src.I.M10;
3258 dst.I.M11 = src.I.M11;
3259 dst.I.M12 = src.I.M12;
3260 dst.I.M20 = src.I.M20;
3261 dst.I.M21 = src.I.M21;
3262 dst.I.M22 = src.I.M22;
3263 }
3264
3265 public override void SetMaterial(int pMaterial)
3266 {
3267 m_material = pMaterial;
3268 }
3269
3270 }
3271}
generated by cgit v1.1 at 2024-11-11 10:09:37 +0000