aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs')
-rw-r--r--OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs2719
1 files changed, 0 insertions, 2719 deletions
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
deleted file mode 100644
index 920ed96..0000000
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
+++ /dev/null
@@ -1,2719 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Threading;
33using log4net;
34using OpenMetaverse;
35using BulletDotNET;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38
39
40namespace OpenSim.Region.Physics.BulletDotNETPlugin
41{
42 public class BulletDotNETPrim : PhysicsActor
43 {
44 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
45
46 private Vector3 _position;
47 private Vector3 m_zeroPosition;
48 private Vector3 _velocity;
49 private Vector3 _torque;
50 private Vector3 m_lastVelocity;
51 private Vector3 m_lastposition;
52 private Quaternion m_lastorientation = Quaternion.Identity;
53 private Vector3 m_rotationalVelocity;
54 private Vector3 _size;
55 private Vector3 _acceleration;
56 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
57 private Quaternion _orientation;
58 private Vector3 m_taintposition;
59 private Vector3 m_taintsize;
60 private Vector3 m_taintVelocity;
61 private Vector3 m_taintTorque;
62 private Quaternion m_taintrot;
63 private Vector3 m_angularlock = Vector3.One;
64 private Vector3 m_taintAngularLock = Vector3.One;
65 // private btGeneric6DofConstraint Amotor;
66
67 private Vector3 m_PIDTarget;
68 private float m_PIDTau;
69 private float m_PIDHoverHeight;
70 private float m_PIDHoverTau;
71 private bool m_useHoverPID;
72 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
73 private float m_targetHoverHeight;
74 private float m_groundHeight;
75 private float m_waterHeight;
76 private float PID_D = 35f;
77 private float PID_G = 25f;
78 // private float m_tensor = 5f;
79 // private int body_autodisable_frames = 20;
80 private IMesh primMesh;
81
82 private bool m_usePID;
83
84 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
85 | CollisionCategories.Space
86 | CollisionCategories.Body
87 | CollisionCategories.Character
88 );
89
90 private bool m_taintshape;
91 private bool m_taintPhysics;
92 // private bool m_collidesLand = true;
93 private bool m_collidesWater;
94 public bool m_returnCollisions;
95
96 // Default we're a Geometry
97 // private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
98
99 // Default, Collide with Other Geometries, spaces and Bodies
100 // private CollisionCategories m_collisionFlags = m_default_collisionFlags;
101
102 public bool m_taintremove;
103 public bool m_taintdisable;
104 public bool m_disabled;
105 public bool m_taintadd;
106 public bool m_taintselected;
107 public bool m_taintCollidesWater;
108
109 public uint m_localID;
110
111 //public GCHandle gc;
112 // private CollisionLocker ode;
113
114 private bool m_taintforce;
115 private bool m_taintaddangularforce;
116 private Vector3 m_force;
117 private List<Vector3> m_forcelist = new List<Vector3>();
118 private List<Vector3> m_angularforcelist = new List<Vector3>();
119
120 private IMesh _mesh;
121 private PrimitiveBaseShape _pbs;
122 private BulletDotNETScene _parent_scene;
123 public btCollisionShape prim_geom;
124 public IntPtr _triMeshData;
125
126 private PhysicsActor _parent;
127 private PhysicsActor m_taintparent;
128
129 private List<BulletDotNETPrim> childrenPrim = new List<BulletDotNETPrim>();
130
131 private bool iscolliding;
132 private bool m_isphysical;
133 private bool m_isSelected;
134
135 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
136
137 private bool m_throttleUpdates;
138 // private int throttleCounter;
139 public int m_interpenetrationcount;
140 public float m_collisionscore;
141 public int m_roundsUnderMotionThreshold;
142 private int m_crossingfailures;
143
144 public float m_buoyancy;
145
146 public bool outofBounds;
147 private float m_density = 10.000006836f; // Aluminum g/cm3;
148
149 public bool _zeroFlag;
150 private bool m_lastUpdateSent;
151
152
153 private String m_primName;
154 private Vector3 _target_velocity;
155
156 public int m_eventsubscription;
157 // private CollisionEventUpdate CollisionEventsThisFrame = null;
158
159 public volatile bool childPrim;
160
161 private btVector3 tempPosition1;
162 private btVector3 tempPosition2;
163 private btVector3 tempPosition3;
164 private btVector3 tempSize1;
165 private btVector3 tempSize2;
166 private btVector3 tempLinearVelocity1;
167 private btVector3 tempLinearVelocity2;
168 private btVector3 tempAngularVelocity1;
169 private btVector3 tempAngularVelocity2;
170 private btVector3 tempInertia1;
171 private btVector3 tempInertia2;
172 private btVector3 tempAddForce;
173 private btQuaternion tempOrientation1;
174 private btQuaternion tempOrientation2;
175 private btMotionState tempMotionState1;
176 private btMotionState tempMotionState2;
177 private btMotionState tempMotionState3;
178 private btTransform tempTransform1;
179 private btTransform tempTransform2;
180 private btTransform tempTransform3;
181 private btTransform tempTransform4;
182 private btTriangleIndexVertexArray btshapeArray;
183 private btVector3 AxisLockAngleHigh;
184 private btVector3 AxisLockLinearLow;
185 private btVector3 AxisLockLinearHigh;
186 private bool forceenable = false;
187
188 private btGeneric6DofConstraint m_aMotor;
189
190 public btRigidBody Body;
191
192 public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size,
193 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)
194 {
195 tempPosition1 = new btVector3(0, 0, 0);
196 tempPosition2 = new btVector3(0, 0, 0);
197 tempPosition3 = new btVector3(0, 0, 0);
198 tempSize1 = new btVector3(0, 0, 0);
199 tempSize2 = new btVector3(0, 0, 0);
200 tempLinearVelocity1 = new btVector3(0, 0, 0);
201 tempLinearVelocity2 = new btVector3(0, 0, 0);
202 tempAngularVelocity1 = new btVector3(0, 0, 0);
203 tempAngularVelocity2 = new btVector3(0, 0, 0);
204 tempInertia1 = new btVector3(0, 0, 0);
205 tempInertia2 = new btVector3(0, 0, 0);
206 tempOrientation1 = new btQuaternion(0, 0, 0, 1);
207 tempOrientation2 = new btQuaternion(0, 0, 0, 1);
208 _parent_scene = parent_scene;
209 tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);
210 tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
211 tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
212 tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
213
214 tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);
215 tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);
216 tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);
217
218
219 AxisLockLinearLow = new btVector3(-1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize);
220 int regionsize = (int)Constants.RegionSize;
221
222 if (regionsize == 256)
223 regionsize = 512;
224
225 AxisLockLinearHigh = new btVector3((int)Constants.RegionSize, (int)Constants.RegionSize, (int)Constants.RegionSize);
226
227 _target_velocity = Vector3.Zero;
228 _velocity = Vector3.Zero;
229 _position = pos;
230 m_taintposition = pos;
231 PID_D = parent_scene.bodyPIDD;
232 PID_G = parent_scene.bodyPIDG;
233 m_density = parent_scene.geomDefaultDensity;
234 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
235 // body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
236
237 prim_geom = null;
238 Body = null;
239
240 if (size.X <= 0) size.X = 0.01f;
241 if (size.Y <= 0) size.Y = 0.01f;
242 if (size.Z <= 0) size.Z = 0.01f;
243
244 _size = size;
245 m_taintsize = _size;
246 _acceleration = Vector3.Zero;
247 m_rotationalVelocity = Vector3.Zero;
248 _orientation = rotation;
249 m_taintrot = _orientation;
250 _mesh = mesh;
251 _pbs = pbs;
252
253 _parent_scene = parent_scene;
254
255 if (pos.Z < 0)
256 m_isphysical = false;
257 else
258 {
259 m_isphysical = pisPhysical;
260 // If we're physical, we need to be in the master space for now.
261 // linksets *should* be in a space together.. but are not currently
262 }
263 m_primName = primName;
264 m_taintadd = true;
265 _parent_scene.AddPhysicsActorTaint(this);
266
267 }
268
269 #region PhysicsActor overrides
270
271 public override bool Stopped
272 {
273 get { return _zeroFlag; }
274 }
275
276 public override Vector3 Size
277 {
278 get { return _size; }
279 set { _size = value; }
280 }
281
282 public override PrimitiveBaseShape Shape
283 {
284 set
285 {
286 _pbs = value;
287 m_taintshape = true;
288 }
289 }
290
291 public override uint LocalID
292 {
293 set
294 {
295 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
296 m_localID = value;
297 }
298 }
299
300 public override bool Grabbed
301 {
302 set { return; }
303 }
304
305 public override bool Selected
306 {
307 set
308 {
309 // This only makes the object not collidable if the object
310 // is physical or the object is modified somehow *IN THE FUTURE*
311 // without this, if an avatar selects prim, they can walk right
312 // through it while it's selected
313 m_collisionscore = 0;
314 if ((m_isphysical && !_zeroFlag) || !value)
315 {
316 m_taintselected = value;
317 _parent_scene.AddPhysicsActorTaint(this);
318 }
319 else
320 {
321 m_taintselected = value;
322 m_isSelected = value;
323 }
324 }
325 }
326
327 public override void CrossingFailure()
328 {
329 m_crossingfailures++;
330 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
331 {
332 base.RaiseOutOfBounds(_position);
333 return;
334 }
335 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
336 {
337 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
338 }
339 }
340 public override void link(PhysicsActor obj)
341 {
342 m_taintparent = obj;
343 }
344
345 public override void delink()
346 {
347 m_taintparent = null;
348 }
349
350 public override void LockAngularMotion(Vector3 axis)
351 {
352 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
353 m_taintAngularLock = axis;
354 }
355
356 public override Vector3 Position
357 {
358 get { return _position; }
359
360 set
361 {
362 _position = value;
363 //m_log.Info("[PHYSICS]: " + _position.ToString());
364 }
365 }
366
367 public override float Mass
368 {
369 get { return CalculateMass(); }
370 }
371
372 public override Vector3 Force
373 {
374 //get { return Vector3.Zero; }
375 get { return m_force; }
376 set { m_force = value; }
377 }
378
379 public override int VehicleType
380 {
381 get { return 0; }
382 set { return; }
383 }
384
385 public override void VehicleFloatParam(int param, float value)
386 {
387 //TODO:
388 }
389
390 public override void VehicleVectorParam(int param, Vector3 value)
391 {
392 //TODO:
393 }
394
395 public override void VehicleRotationParam(int param, Quaternion rotation)
396 {
397 //TODO:
398 }
399
400 public override void VehicleFlags(int param, bool remove)
401 {
402
403 }
404
405 public override void SetVolumeDetect(int param)
406 {
407 //TODO: GhostObject
408 m_isVolumeDetect = (param != 0);
409
410 }
411
412 public override Vector3 GeometricCenter
413 {
414 get { return Vector3.Zero; }
415 }
416
417 public override Vector3 CenterOfMass
418 {
419 get { return Vector3.Zero; }
420 }
421
422 public override Vector3 Velocity
423 {
424 get
425 {
426 // Averate previous velocity with the new one so
427 // client object interpolation works a 'little' better
428 Vector3 returnVelocity;
429 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
430 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
431 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
432 return returnVelocity;
433 }
434 set
435 {
436 _velocity = value;
437
438 m_taintVelocity = value;
439 _parent_scene.AddPhysicsActorTaint(this);
440 }
441 }
442
443 public override Vector3 Torque
444 {
445 get
446 {
447 if (!m_isphysical || Body.Handle == IntPtr.Zero)
448 return Vector3.Zero;
449
450 return _torque;
451 }
452
453 set
454 {
455 m_taintTorque = value;
456 _parent_scene.AddPhysicsActorTaint(this);
457 }
458 }
459
460 public override float CollisionScore
461 {
462 get { return m_collisionscore; }
463 set { m_collisionscore = value; }
464 }
465
466 public override Vector3 Acceleration
467 {
468 get { return _acceleration; }
469 }
470
471 public override Quaternion Orientation
472 {
473 get { return _orientation; }
474 set { _orientation = value; }
475 }
476
477 public override int PhysicsActorType
478 {
479 get { return (int)ActorTypes.Prim; }
480 set { return; }
481 }
482
483 public override bool IsPhysical
484 {
485 get { return m_isphysical; }
486 set { m_isphysical = value; }
487 }
488
489 public override bool Flying
490 {
491 // no flying prims for you
492 get { return false; }
493 set { }
494 }
495
496 public override bool SetAlwaysRun
497 {
498 get { return false; }
499 set { return; }
500 }
501
502 public override bool ThrottleUpdates
503 {
504 get { return m_throttleUpdates; }
505 set { m_throttleUpdates = value; }
506 }
507
508 public override bool IsColliding
509 {
510 get { return iscolliding; }
511 set { iscolliding = value; }
512 }
513
514 public override bool CollidingGround
515 {
516 get { return false; }
517 set { return; }
518 }
519
520 public override bool CollidingObj
521 {
522 get { return false; }
523 set { return; }
524 }
525
526 public override bool FloatOnWater
527 {
528 set
529 {
530 m_taintCollidesWater = value;
531 _parent_scene.AddPhysicsActorTaint(this);
532 }
533 }
534
535 public override Vector3 RotationalVelocity
536 {
537 get
538 {
539 Vector3 pv = Vector3.Zero;
540 if (_zeroFlag)
541 return pv;
542 m_lastUpdateSent = false;
543
544 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
545 return pv;
546
547 return m_rotationalVelocity;
548 }
549 set { m_rotationalVelocity = value; }
550 }
551
552 public override bool Kinematic
553 {
554 get { return false; }
555 set { }
556 }
557
558 public override float Buoyancy
559 {
560 get { return m_buoyancy; }
561 set { m_buoyancy = value; }
562 }
563
564 public override Vector3 PIDTarget { set { m_PIDTarget = value; ; } }
565 public override bool PIDActive { set { m_usePID = value; } }
566 public override float PIDTau { set { m_PIDTau = value; } }
567
568 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
569 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
570 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
571 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
572
573 public override Quaternion APIDTarget { set { return; } }
574 public override bool APIDActive { set { return; } }
575 public override float APIDStrength { set { return; } }
576 public override float APIDDamping { set { return; } }
577
578 public override void AddForce(Vector3 force, bool pushforce)
579 {
580 m_forcelist.Add(force);
581 m_taintforce = true;
582 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
583 }
584
585 public override void AddAngularForce(Vector3 force, bool pushforce)
586 {
587 m_angularforcelist.Add(force);
588 m_taintaddangularforce = true;
589 }
590
591 public override void SetMomentum(Vector3 momentum)
592 {
593 }
594
595 public override void SubscribeEvents(int ms)
596 {
597 m_eventsubscription = ms;
598 _parent_scene.addCollisionEventReporting(this);
599 }
600
601 public override void UnSubscribeEvents()
602 {
603 _parent_scene.remCollisionEventReporting(this);
604 m_eventsubscription = 0;
605 }
606
607 public override bool SubscribedEvents()
608 {
609 return (m_eventsubscription > 0);
610 }
611
612 #endregion
613
614
615
616 internal void Dispose()
617 {
618 //TODO:
619 DisableAxisMotor();
620 DisposeOfBody();
621 SetCollisionShape(null);
622
623 if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)
624 {
625 tempMotionState3.Dispose();
626 tempMotionState3 = null;
627 }
628
629 if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)
630 {
631 tempMotionState2.Dispose();
632 tempMotionState2 = null;
633 }
634
635 if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
636 {
637 tempMotionState1.Dispose();
638 tempMotionState1 = null;
639 }
640
641 if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)
642 {
643 tempTransform4.Dispose();
644 tempTransform4 = null;
645 }
646
647 if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
648 {
649 tempTransform3.Dispose();
650 tempTransform3 = null;
651 }
652
653 if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
654 {
655 tempTransform2.Dispose();
656 tempTransform2 = null;
657 }
658
659 if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
660 {
661 tempTransform1.Dispose();
662 tempTransform1 = null;
663 }
664
665 if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
666 {
667 tempOrientation2.Dispose();
668 tempOrientation2 = null;
669 }
670
671 if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)
672 {
673 tempOrientation1.Dispose();
674 tempOrientation1 = null;
675 }
676
677 if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
678 {
679 tempInertia1.Dispose();
680 tempInertia1 = null;
681 }
682
683 if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)
684 {
685 tempInertia2.Dispose();
686 tempInertia1 = null;
687 }
688
689
690 if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)
691 {
692 tempAngularVelocity2.Dispose();
693 tempAngularVelocity2 = null;
694 }
695
696 if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
697 {
698 tempAngularVelocity1.Dispose();
699 tempAngularVelocity1 = null;
700 }
701
702 if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)
703 {
704 tempLinearVelocity2.Dispose();
705 tempLinearVelocity2 = null;
706 }
707
708 if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
709 {
710 tempLinearVelocity1.Dispose();
711 tempLinearVelocity1 = null;
712 }
713
714 if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)
715 {
716 tempSize2.Dispose();
717 tempSize2 = null;
718 }
719
720 if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)
721 {
722 tempSize1.Dispose();
723 tempSize1 = null;
724 }
725
726 if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
727 {
728 tempPosition3.Dispose();
729 tempPosition3 = null;
730 }
731
732 if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
733 {
734 tempPosition2.Dispose();
735 tempPosition2 = null;
736 }
737
738 if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)
739 {
740 tempPosition1.Dispose();
741 tempPosition1 = null;
742 }
743 if (AxisLockLinearLow != null && AxisLockLinearLow.Handle != IntPtr.Zero)
744 {
745 AxisLockLinearLow.Dispose();
746 AxisLockLinearLow = null;
747 }
748 if (AxisLockLinearHigh != null && AxisLockLinearHigh.Handle != IntPtr.Zero)
749 {
750 AxisLockLinearHigh.Dispose();
751 AxisLockLinearHigh = null;
752 }
753
754 }
755
756
757
758 public void ProcessTaints(float timestep)
759 {
760 if (m_taintadd)
761 {
762 m_log.Debug("[PHYSICS]: TaintAdd");
763 changeadd(timestep);
764 }
765
766 if (prim_geom == null)
767 {
768 CreateGeom(IntPtr.Zero, primMesh);
769
770 if (IsPhysical)
771 SetBody(Mass);
772 else
773 SetBody(0);
774 m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
775 }
776
777 if (prim_geom.Handle == IntPtr.Zero)
778 {
779 CreateGeom(IntPtr.Zero, primMesh);
780
781 if (IsPhysical)
782 SetBody(Mass);
783 else
784 SetBody(0);
785 m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
786
787 }
788
789 if (!_position.ApproxEquals(m_taintposition, 0f))
790 {
791 m_log.Debug("[PHYSICS]: TaintMove");
792 changemove(timestep);
793 }
794 if (m_taintrot != _orientation)
795 {
796 m_log.Debug("[PHYSICS]: TaintRotate");
797 rotate(timestep);
798 } //
799
800 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
801 {
802 m_log.Debug("[PHYSICS]: TaintPhysics");
803 changePhysicsStatus(timestep);
804 }
805 //
806
807 if (!_size.ApproxEquals(m_taintsize, 0f))
808 {
809 m_log.Debug("[PHYSICS]: TaintSize");
810 changesize(timestep);
811 }
812
813 //
814
815 if (m_taintshape)
816 {
817 m_log.Debug("[PHYSICS]: TaintShape");
818 changeshape(timestep);
819 } //
820
821 if (m_taintforce)
822 {
823 m_log.Debug("[PHYSICS]: TaintForce");
824 changeAddForce(timestep);
825 }
826 if (m_taintaddangularforce)
827 {
828 m_log.Debug("[PHYSICS]: TaintAngularForce");
829 changeAddAngularForce(timestep);
830 }
831 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
832 {
833 m_log.Debug("[PHYSICS]: TaintTorque");
834 changeSetTorque(timestep);
835 }
836 if (m_taintdisable)
837 {
838 m_log.Debug("[PHYSICS]: TaintDisable");
839 changedisable(timestep);
840 }
841 if (m_taintselected != m_isSelected)
842 {
843 m_log.Debug("[PHYSICS]: TaintSelected");
844 changeSelectedStatus(timestep);
845 }
846 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
847 {
848 m_log.Debug("[PHYSICS]: TaintVelocity");
849 changevelocity(timestep);
850 }
851 if (m_taintparent != _parent)
852 {
853 m_log.Debug("[PHYSICS]: TaintLink");
854 changelink(timestep);
855 }
856 if (m_taintCollidesWater != m_collidesWater)
857 {
858 changefloatonwater(timestep);
859 }
860 if (!m_angularlock.ApproxEquals(m_taintAngularLock, 0))
861 {
862 m_log.Debug("[PHYSICS]: TaintAngularLock");
863 changeAngularLock(timestep);
864 }
865 if (m_taintremove)
866 {
867 DisposeOfBody();
868 Dispose();
869 }
870
871 }
872
873 #region Physics Scene Change Action routines
874
875 private void changeadd(float timestep)
876 {
877 //SetCollisionShape(null);
878 // Construction of new prim
879 if (Body != null)
880 {
881 if (Body.Handle != IntPtr.Zero)
882 {
883 DisableAxisMotor();
884 _parent_scene.removeFromWorld(this, Body);
885 //Body.Dispose();
886 }
887 //Body = null;
888 // TODO: dispose parts that make up body
889 }
890 if (_parent_scene.needsMeshing(_pbs))
891 {
892 // Don't need to re-enable body.. it's done in SetMesh
893 float meshlod = _parent_scene.meshSculptLOD;
894
895 if (IsPhysical)
896 meshlod = _parent_scene.MeshSculptphysicalLOD;
897
898 IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
899 // createmesh returns null when it doesn't mesh.
900 CreateGeom(IntPtr.Zero, mesh);
901 }
902 else
903 {
904 _mesh = null;
905 CreateGeom(IntPtr.Zero, null);
906 }
907
908 if (IsPhysical)
909 SetBody(Mass);
910 else
911 SetBody(0);
912 //changeSelectedStatus(timestep);
913 m_taintadd = false;
914
915 }
916
917 private void changemove(float timestep)
918 {
919
920 m_log.Debug("[PHYSICS]: _________ChangeMove");
921 if (!m_isphysical)
922 {
923 tempTransform2 = Body.getWorldTransform();
924 btQuaternion quat = tempTransform2.getRotation();
925 tempPosition2.setValue(_position.X, _position.Y, _position.Z);
926 tempTransform2.Dispose();
927 tempTransform2 = new btTransform(quat, tempPosition2);
928 Body.setWorldTransform(tempTransform2);
929
930 changeSelectedStatus(timestep);
931
932 resetCollisionAccounting();
933 }
934 else
935 {
936 if (Body != null)
937 {
938 if (Body.Handle != IntPtr.Zero)
939 {
940 DisableAxisMotor();
941 _parent_scene.removeFromWorld(this, Body);
942 //Body.Dispose();
943 }
944 //Body = null;
945 // TODO: dispose parts that make up body
946 }
947 /*
948 if (_parent_scene.needsMeshing(_pbs))
949 {
950 // Don't need to re-enable body.. it's done in SetMesh
951 float meshlod = _parent_scene.meshSculptLOD;
952
953 if (IsPhysical)
954 meshlod = _parent_scene.MeshSculptphysicalLOD;
955
956 IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
957 // createmesh returns null when it doesn't mesh.
958 CreateGeom(IntPtr.Zero, mesh);
959 }
960 else
961 {
962 _mesh = null;
963 CreateGeom(IntPtr.Zero, null);
964 }
965 SetCollisionShape(prim_geom);
966 */
967 if (m_isphysical)
968 SetBody(Mass);
969 else
970 SetBody(0);
971 changeSelectedStatus(timestep);
972
973 resetCollisionAccounting();
974 }
975 m_taintposition = _position;
976 }
977
978 private void rotate(float timestep)
979 {
980 m_log.Debug("[PHYSICS]: _________ChangeRotate");
981 tempTransform2 = Body.getWorldTransform();
982 tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
983 tempTransform2.setRotation(tempOrientation2);
984 Body.setWorldTransform(tempTransform2);
985
986 resetCollisionAccounting();
987 m_taintrot = _orientation;
988 }
989
990 private void changePhysicsStatus(float timestep)
991 {
992 if (Body != null)
993 {
994 if (Body.Handle != IntPtr.Zero)
995 {
996 DisableAxisMotor();
997 _parent_scene.removeFromWorld(this, Body);
998 //Body.Dispose();
999 }
1000 //Body = null;
1001 // TODO: dispose parts that make up body
1002 }
1003 m_log.Debug("[PHYSICS]: _________ChangePhysics");
1004
1005 ProcessGeomCreation();
1006
1007 if (m_isphysical)
1008 SetBody(Mass);
1009 else
1010 SetBody(0);
1011 changeSelectedStatus(timestep);
1012
1013 resetCollisionAccounting();
1014 m_taintPhysics = m_isphysical;
1015 }
1016
1017
1018
1019 internal void ProcessGeomCreation()
1020 {
1021 if (_parent_scene.needsMeshing(_pbs))
1022 {
1023 ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
1024 // createmesh returns null when it doesn't mesh.
1025 CreateGeom(IntPtr.Zero, _mesh);
1026 }
1027 else
1028 {
1029 _mesh = null;
1030 CreateGeom(IntPtr.Zero, null);
1031 }
1032 SetCollisionShape(prim_geom);
1033 }
1034
1035 internal bool NeedsMeshing()
1036 {
1037 return _parent_scene.needsMeshing(_pbs);
1038 }
1039
1040 internal void ProcessGeomCreationAsTriMesh(Vector3 positionOffset, Quaternion orientation)
1041 {
1042 // Don't need to re-enable body.. it's done in SetMesh
1043 float meshlod = _parent_scene.meshSculptLOD;
1044
1045 if (IsPhysical)
1046 meshlod = _parent_scene.MeshSculptphysicalLOD;
1047
1048 IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
1049 if (!positionOffset.ApproxEquals(Vector3.Zero, 0.001f) || orientation != Quaternion.Identity)
1050 {
1051
1052 float[] xyz = new float[3];
1053 xyz[0] = positionOffset.X;
1054 xyz[1] = positionOffset.Y;
1055 xyz[2] = positionOffset.Z;
1056
1057 Matrix4 m4 = Matrix4.CreateFromQuaternion(orientation);
1058
1059 float[,] matrix = new float[3, 3];
1060
1061 matrix[0, 0] = m4.M11;
1062 matrix[0, 1] = m4.M12;
1063 matrix[0, 2] = m4.M13;
1064 matrix[1, 0] = m4.M21;
1065 matrix[1, 1] = m4.M22;
1066 matrix[1, 2] = m4.M23;
1067 matrix[2, 0] = m4.M31;
1068 matrix[2, 1] = m4.M32;
1069 matrix[2, 2] = m4.M33;
1070
1071
1072 mesh.TransformLinear(matrix, xyz);
1073
1074
1075
1076 }
1077
1078 _mesh = mesh;
1079 }
1080
1081 private void changesize(float timestep)
1082 {
1083 if (Body != null)
1084 {
1085 if (Body.Handle != IntPtr.Zero)
1086 {
1087 DisableAxisMotor();
1088 _parent_scene.removeFromWorld(this, Body);
1089 //Body.Dispose();
1090 }
1091 //Body = null;
1092 // TODO: dispose parts that make up body
1093 }
1094
1095 m_log.Debug("[PHYSICS]: _________ChangeSize");
1096 SetCollisionShape(null);
1097 // Construction of new prim
1098 ProcessGeomCreation();
1099
1100 if (IsPhysical)
1101 SetBody(Mass);
1102 else
1103 SetBody(0);
1104
1105 m_taintsize = _size;
1106
1107 }
1108
1109 private void changeshape(float timestep)
1110 {
1111 if (Body != null)
1112 {
1113 if (Body.Handle != IntPtr.Zero)
1114 {
1115 DisableAxisMotor();
1116 _parent_scene.removeFromWorld(this, Body);
1117 //Body.Dispose();
1118 }
1119 //Body = null;
1120 // TODO: dispose parts that make up body
1121 }
1122 // Cleanup of old prim geometry and Bodies
1123 if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
1124 {
1125 if (childPrim)
1126 {
1127 if (_parent != null)
1128 {
1129 BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
1130 parent.ChildDelink(this);
1131 }
1132 }
1133 else
1134 {
1135 //disableBody();
1136 }
1137 }
1138 try
1139 {
1140 //SetCollisionShape(null);
1141 }
1142 catch (System.AccessViolationException)
1143 {
1144 //prim_geom = IntPtr.Zero;
1145 m_log.Error("[PHYSICS]: PrimGeom dead");
1146 }
1147
1148 // we don't need to do space calculation because the client sends a position update also.
1149 if (_size.X <= 0) _size.X = 0.01f;
1150 if (_size.Y <= 0) _size.Y = 0.01f;
1151 if (_size.Z <= 0) _size.Z = 0.01f;
1152 // Construction of new prim
1153
1154 ProcessGeomCreation();
1155
1156 tempPosition1.setValue(_position.X, _position.Y, _position.Z);
1157 if (tempOrientation1.Handle != IntPtr.Zero)
1158 tempOrientation1.Dispose();
1159 tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);
1160 if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
1161 tempTransform1.Dispose();
1162 tempTransform1 = new btTransform(tempOrientation1, tempPosition1);
1163
1164
1165
1166
1167 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
1168 if (IsPhysical)
1169 {
1170 SetBody(Mass);
1171 // Re creates body on size.
1172 // EnableBody also does setMass()
1173
1174 }
1175 else
1176 {
1177 SetBody(0);
1178 }
1179
1180 changeSelectedStatus(timestep);
1181 if (childPrim)
1182 {
1183 if (_parent is BulletDotNETPrim)
1184 {
1185 BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
1186 parent.ChildSetGeom(this);
1187 }
1188 }
1189 resetCollisionAccounting();
1190
1191 m_taintshape = false;
1192 }
1193
1194 private void resetCollisionAccounting()
1195 {
1196 m_collisionscore = 0;
1197 }
1198
1199 private void ChildSetGeom(BulletDotNETPrim bulletDotNETPrim)
1200 {
1201 // TODO: throw new NotImplementedException();
1202 }
1203
1204 private void changeAddForce(float timestep)
1205 {
1206 if (!m_isSelected)
1207 {
1208 lock (m_forcelist)
1209 {
1210 //m_log.Info("[PHYSICS]: dequeing forcelist");
1211 if (IsPhysical)
1212 {
1213 Vector3 iforce = Vector3.Zero;
1214 for (int i = 0; i < m_forcelist.Count; i++)
1215 {
1216 iforce = iforce + m_forcelist[i];
1217 }
1218
1219 if (Body != null && Body.Handle != IntPtr.Zero)
1220 {
1221 if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
1222 tempAddForce.Dispose();
1223 enableBodySoft();
1224 tempAddForce = new btVector3(iforce.X, iforce.Y, iforce.Z);
1225 Body.applyCentralImpulse(tempAddForce);
1226 }
1227 }
1228 m_forcelist.Clear();
1229 }
1230
1231 m_collisionscore = 0;
1232 m_interpenetrationcount = 0;
1233 }
1234
1235 m_taintforce = false;
1236
1237 }
1238
1239 private void changeAddAngularForce(float timestep)
1240 {
1241 if (!m_isSelected)
1242 {
1243 lock (m_angularforcelist)
1244 {
1245 //m_log.Info("[PHYSICS]: dequeing forcelist");
1246 if (IsPhysical)
1247 {
1248 Vector3 iforce = Vector3.Zero;
1249 for (int i = 0; i < m_angularforcelist.Count; i++)
1250 {
1251 iforce = iforce + m_angularforcelist[i];
1252 }
1253
1254 if (Body != null && Body.Handle != IntPtr.Zero)
1255 {
1256 if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
1257 tempAddForce.Dispose();
1258 enableBodySoft();
1259 tempAddForce = new btVector3(iforce.X, iforce.Y, iforce.Z);
1260 Body.applyTorqueImpulse(tempAddForce);
1261 }
1262
1263 }
1264 m_angularforcelist.Clear();
1265 }
1266
1267 m_collisionscore = 0;
1268 m_interpenetrationcount = 0;
1269 }
1270
1271 m_taintaddangularforce = false;
1272 }
1273
1274 private void changeSetTorque(float timestep)
1275 {
1276 if (!m_isSelected)
1277 {
1278 if (IsPhysical)
1279 {
1280 if (Body != null && Body.Handle != IntPtr.Zero)
1281 {
1282 tempAngularVelocity2.setValue(m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
1283 Body.applyTorque(tempAngularVelocity2);
1284 }
1285 }
1286 }
1287 m_taintTorque = Vector3.Zero;
1288 }
1289
1290 private void changedisable(float timestep)
1291 {
1292 // TODO: throw new NotImplementedException();
1293 }
1294
1295 private void changeSelectedStatus(float timestep)
1296 {
1297 // TODO: throw new NotImplementedException();
1298 if (m_taintselected)
1299 {
1300 Body.setCollisionFlags((int)ContactFlags.CF_NO_CONTACT_RESPONSE);
1301 disableBodySoft();
1302
1303 }
1304 else
1305 {
1306 Body.setCollisionFlags(0 | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
1307 enableBodySoft();
1308 }
1309 m_isSelected = m_taintselected;
1310
1311 }
1312
1313 private void changevelocity(float timestep)
1314 {
1315 if (!m_isSelected)
1316 {
1317 if (IsPhysical)
1318 {
1319 if (Body != null && Body.Handle != IntPtr.Zero)
1320 {
1321 tempLinearVelocity2.setValue(m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
1322 Body.setLinearVelocity(tempLinearVelocity2);
1323 }
1324 }
1325
1326 //resetCollisionAccounting();
1327 }
1328 m_taintVelocity = Vector3.Zero;
1329 }
1330
1331 private void changelink(float timestep)
1332 {
1333 if (IsPhysical)
1334 {
1335 // Construction of new prim
1336 if (Body != null)
1337 {
1338 if (Body.Handle != IntPtr.Zero)
1339 {
1340 DisableAxisMotor();
1341 _parent_scene.removeFromWorld(this, Body);
1342 //Body.Dispose();
1343 }
1344 //Body = null;
1345 // TODO: dispose parts that make up body
1346 }
1347
1348 if (_parent == null && m_taintparent != null)
1349 {
1350
1351 if (m_taintparent is BulletDotNETPrim)
1352 {
1353 BulletDotNETPrim obj = (BulletDotNETPrim)m_taintparent;
1354 obj.ParentPrim(this);
1355 childPrim = true;
1356
1357 }
1358 }
1359 else if (_parent != null && m_taintparent == null)
1360 {
1361 if (_parent is BulletDotNETPrim)
1362 {
1363 BulletDotNETPrim obj = (BulletDotNETPrim)_parent;
1364 obj.ChildDelink(obj);
1365
1366 childPrim = false;
1367 }
1368 }
1369
1370 if (m_taintparent != null)
1371 {
1372 Vector3 taintparentPosition = m_taintparent.Position;
1373 taintparentPosition.Z = m_taintparent.Position.Z + 0.02f;
1374 m_taintparent.Position = taintparentPosition;
1375 _parent_scene.AddPhysicsActorTaint(m_taintparent);
1376 }
1377 }
1378 _parent = m_taintparent;
1379
1380 m_taintPhysics = m_isphysical;
1381
1382 }
1383
1384 private void changefloatonwater(float timestep)
1385 {
1386 // TODO: throw new NotImplementedException();
1387 }
1388
1389 private void changeAngularLock(float timestep)
1390 {
1391 if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
1392 {
1393 if (_parent == null)
1394 {
1395 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
1396 {
1397 //d.BodySetFiniteRotationMode(Body, 0);
1398 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
1399 EnableAxisMotor(m_taintAngularLock);
1400 }
1401 else
1402 {
1403 DisableAxisMotor();
1404 }
1405 }
1406
1407 }
1408 m_angularlock = m_taintAngularLock;
1409
1410 }
1411 #endregion
1412
1413
1414
1415
1416 internal void Move(float timestep)
1417 {
1418 //TODO:
1419 float fx = 0;
1420 float fy = 0;
1421 float fz = 0;
1422
1423 if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)
1424 {
1425 float m_mass = CalculateMass();
1426
1427 fz = 0f;
1428 //m_log.Info(m_collisionFlags.ToString());
1429
1430 if (m_buoyancy != 0)
1431 {
1432 if (m_buoyancy > 0)
1433 {
1434 fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass) * 0.035f;
1435
1436 //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
1437 //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
1438 }
1439 else
1440 {
1441 fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass) * 0.035f);
1442 }
1443 }
1444
1445 if (m_usePID)
1446 {
1447 PID_D = 61f;
1448 PID_G = 65f;
1449 //if (!d.BodyIsEnabled(Body))
1450 //d.BodySetForce(Body, 0f, 0f, 0f);
1451 // If we're using the PID controller, then we have no gravity
1452 fz = ((-1 * _parent_scene.gravityz) * m_mass) * 1.025f;
1453
1454 // no lock; for now it's only called from within Simulate()
1455
1456 // If the PID Controller isn't active then we set our force
1457 // calculating base velocity to the current position
1458
1459 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1460 {
1461 //PID_G = PID_G / m_PIDTau;
1462 m_PIDTau = 1;
1463 }
1464
1465 if ((PID_G - m_PIDTau) <= 0)
1466 {
1467 PID_G = m_PIDTau + 1;
1468 }
1469
1470 // TODO: NEED btVector3 for Linear Velocity
1471 // NEED btVector3 for Position
1472
1473 Vector3 pos = _position; //TODO: Insert values gotten from bullet
1474 Vector3 vel = _velocity;
1475
1476 _target_velocity =
1477 new Vector3(
1478 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1479 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1480 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1481 );
1482
1483 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1484 {
1485
1486 /* TODO: Do Bullet equiv
1487 *
1488 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1489 d.BodySetLinearVel(Body, 0, 0, 0);
1490 d.BodyAddForce(Body, 0, 0, fz);
1491 return;
1492 */
1493 }
1494 else
1495 {
1496 _zeroFlag = false;
1497
1498 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1499 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1500 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1501
1502 }
1503
1504 }
1505
1506 if (m_useHoverPID && !m_usePID)
1507 {
1508 // If we're using the PID controller, then we have no gravity
1509 fz = (-1 * _parent_scene.gravityz) * m_mass;
1510
1511 // no lock; for now it's only called from within Simulate()
1512
1513 // If the PID Controller isn't active then we set our force
1514 // calculating base velocity to the current position
1515
1516 if ((m_PIDTau < 1))
1517 {
1518 PID_G = PID_G / m_PIDTau;
1519 }
1520
1521 if ((PID_G - m_PIDTau) <= 0)
1522 {
1523 PID_G = m_PIDTau + 1;
1524 }
1525 Vector3 pos = Vector3.Zero; //TODO: Insert values gotten from bullet
1526 Vector3 vel = Vector3.Zero;
1527
1528 // determine what our target height really is based on HoverType
1529 switch (m_PIDHoverType)
1530 {
1531 case PIDHoverType.Absolute:
1532 m_targetHoverHeight = m_PIDHoverHeight;
1533 break;
1534 case PIDHoverType.Ground:
1535 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1536 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1537 break;
1538 case PIDHoverType.GroundAndWater:
1539 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1540 m_waterHeight = _parent_scene.GetWaterLevel();
1541 if (m_groundHeight > m_waterHeight)
1542 {
1543 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1544 }
1545 else
1546 {
1547 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1548 }
1549 break;
1550 case PIDHoverType.Water:
1551 m_waterHeight = _parent_scene.GetWaterLevel();
1552 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1553 break;
1554 }
1555
1556
1557 _target_velocity =
1558 new Vector3(0.0f, 0.0f,
1559 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1560 );
1561
1562 // if velocity is zero, use position control; otherwise, velocity control
1563
1564 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1565 {
1566
1567 /* TODO: Do Bullet Equiv
1568 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1569 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1570 d.BodyAddForce(Body, 0, 0, fz);
1571 */
1572 if (Body != null && Body.Handle != IntPtr.Zero)
1573 {
1574 Body.setLinearVelocity(_parent_scene.VectorZero);
1575 Body.clearForces();
1576 }
1577 return;
1578 }
1579 else
1580 {
1581 _zeroFlag = false;
1582
1583 // We're flying and colliding with something
1584 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1585 }
1586 }
1587
1588 fx *= m_mass;
1589 fy *= m_mass;
1590 //fz *= m_mass;
1591
1592 fx += m_force.X;
1593 fy += m_force.Y;
1594 fz += m_force.Z;
1595
1596 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1597 if (fx != 0 || fy != 0 || fz != 0)
1598 {
1599 /*
1600 * TODO: Do Bullet Equiv
1601 if (!d.BodyIsEnabled(Body))
1602 {
1603 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1604 d.BodySetForce(Body, 0, 0, 0);
1605 enableBodySoft();
1606 }
1607 */
1608 // 35x10 = 350n times the mass per second applied maximum.
1609
1610 float nmax = 35f * m_mass;
1611 float nmin = -35f * m_mass;
1612
1613
1614 if (fx > nmax)
1615 fx = nmax;
1616 if (fx < nmin)
1617 fx = nmin;
1618 if (fy > nmax)
1619 fy = nmax;
1620 if (fy < nmin)
1621 fy = nmin;
1622
1623 // TODO: Do Bullet Equiv
1624 // d.BodyAddForce(Body, fx, fy, fz);
1625 if (Body != null && Body.Handle != IntPtr.Zero)
1626 {
1627 Body.activate(true);
1628 if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
1629 tempAddForce.Dispose();
1630
1631 tempAddForce = new btVector3(fx * 0.01f, fy * 0.01f, fz * 0.01f);
1632 Body.applyCentralImpulse(tempAddForce);
1633 }
1634 }
1635 }
1636 else
1637 {
1638 if (m_zeroPosition == null)
1639 m_zeroPosition = Vector3.Zero;
1640 m_zeroPosition = _position;
1641 return;
1642 }
1643 }
1644
1645
1646
1647
1648 #region Mass Calculation
1649
1650 private float CalculateMass()
1651 {
1652 float volume = 0;
1653
1654 // No material is passed to the physics engines yet.. soo..
1655 // we're using the m_density constant in the class definition
1656
1657 float returnMass = 0;
1658
1659 switch (_pbs.ProfileShape)
1660 {
1661 case ProfileShape.Square:
1662 // Profile Volume
1663
1664 volume = _size.X * _size.Y * _size.Z;
1665
1666 // If the user has 'hollowed out'
1667 // ProfileHollow is one of those 0 to 50000 values :P
1668 // we like percentages better.. so turning into a percentage
1669
1670 if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
1671 {
1672 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
1673
1674 // calculate the hollow volume by it's shape compared to the prim shape
1675 float hollowVolume = 0;
1676 switch (_pbs.HollowShape)
1677 {
1678 case HollowShape.Square:
1679 case HollowShape.Same:
1680 // Cube Hollow volume calculation
1681 float hollowsizex = _size.X * hollowAmount;
1682 float hollowsizey = _size.Y * hollowAmount;
1683 float hollowsizez = _size.Z * hollowAmount;
1684 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1685 break;
1686
1687 case HollowShape.Circle:
1688 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1689 // Cyllinder hollow volume calculation
1690 float hRadius = _size.X / 2;
1691 float hLength = _size.Z;
1692
1693 // pi * r2 * h
1694 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
1695 break;
1696
1697 case HollowShape.Triangle:
1698 // Equilateral Triangular Prism volume hollow calculation
1699 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1700
1701 float aLength = _size.Y;
1702 // 1/2 abh
1703 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1704 break;
1705
1706 default:
1707 hollowVolume = 0;
1708 break;
1709 }
1710 volume = volume - hollowVolume;
1711 }
1712
1713 break;
1714 case ProfileShape.Circle:
1715 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1716 {
1717 // Cylinder
1718 float volume1 = (float)(Math.PI * Math.Pow(_size.X / 2, 2) * _size.Z);
1719 float volume2 = (float)(Math.PI * Math.Pow(_size.Y / 2, 2) * _size.Z);
1720
1721 // Approximating the cylinder's irregularity.
1722 if (volume1 > volume2)
1723 {
1724 volume = (float)volume1 - (volume1 - volume2);
1725 }
1726 else if (volume2 > volume1)
1727 {
1728 volume = (float)volume2 - (volume2 - volume1);
1729 }
1730 else
1731 {
1732 // Regular cylinder
1733 volume = volume1;
1734 }
1735 }
1736 else
1737 {
1738 // We don't know what the shape is yet, so use default
1739 volume = _size.X * _size.Y * _size.Z;
1740 }
1741 // If the user has 'hollowed out'
1742 // ProfileHollow is one of those 0 to 50000 values :P
1743 // we like percentages better.. so turning into a percentage
1744
1745 if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
1746 {
1747 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
1748
1749 // calculate the hollow volume by it's shape compared to the prim shape
1750 float hollowVolume = 0;
1751 switch (_pbs.HollowShape)
1752 {
1753 case HollowShape.Same:
1754 case HollowShape.Circle:
1755 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1756 // Cyllinder hollow volume calculation
1757 float hRadius = _size.X / 2;
1758 float hLength = _size.Z;
1759
1760 // pi * r2 * h
1761 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
1762 break;
1763
1764 case HollowShape.Square:
1765 // Cube Hollow volume calculation
1766 float hollowsizex = _size.X * hollowAmount;
1767 float hollowsizey = _size.Y * hollowAmount;
1768 float hollowsizez = _size.Z * hollowAmount;
1769 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1770 break;
1771
1772 case HollowShape.Triangle:
1773 // Equilateral Triangular Prism volume hollow calculation
1774 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1775
1776 float aLength = _size.Y;
1777 // 1/2 abh
1778 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1779 break;
1780
1781 default:
1782 hollowVolume = 0;
1783 break;
1784 }
1785 volume = volume - hollowVolume;
1786 }
1787 break;
1788
1789 case ProfileShape.HalfCircle:
1790 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1791 {
1792 if (_size.X == _size.Y && _size.Z == _size.X)
1793 {
1794 // regular sphere
1795 // v = 4/3 * pi * r^3
1796 float sradius3 = (float)Math.Pow((_size.X / 2), 3);
1797 volume = (float)((4 / 3f) * Math.PI * sradius3);
1798 }
1799 else
1800 {
1801 // we treat this as a box currently
1802 volume = _size.X * _size.Y * _size.Z;
1803 }
1804 }
1805 else
1806 {
1807 // We don't know what the shape is yet, so use default
1808 volume = _size.X * _size.Y * _size.Z;
1809 }
1810 break;
1811
1812 case ProfileShape.EquilateralTriangle:
1813 /*
1814 v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
1815
1816 // seed mesh
1817 Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
1818 Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
1819 Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
1820 */
1821 float xA = -0.25f * _size.X;
1822 float yA = -0.45f * _size.Y;
1823
1824 float xB = 0.5f * _size.X;
1825 float yB = 0;
1826
1827 float xC = -0.25f * _size.X;
1828 float yC = 0.45f * _size.Y;
1829
1830 volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
1831
1832 // If the user has 'hollowed out'
1833 // ProfileHollow is one of those 0 to 50000 values :P
1834 // we like percentages better.. so turning into a percentage
1835 float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
1836 if (((float)fhollowFactor / 50000f) > 0.0)
1837 {
1838 float hollowAmount = (float)fhollowFactor / 50000f;
1839
1840 // calculate the hollow volume by it's shape compared to the prim shape
1841 float hollowVolume = 0;
1842 switch (_pbs.HollowShape)
1843 {
1844 case HollowShape.Same:
1845 case HollowShape.Triangle:
1846 // Equilateral Triangular Prism volume hollow calculation
1847 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1848
1849 float aLength = _size.Y;
1850 // 1/2 abh
1851 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1852 break;
1853
1854 case HollowShape.Square:
1855 // Cube Hollow volume calculation
1856 float hollowsizex = _size.X * hollowAmount;
1857 float hollowsizey = _size.Y * hollowAmount;
1858 float hollowsizez = _size.Z * hollowAmount;
1859 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1860 break;
1861
1862 case HollowShape.Circle:
1863 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1864 // Cyllinder hollow volume calculation
1865 float hRadius = _size.X / 2;
1866 float hLength = _size.Z;
1867
1868 // pi * r2 * h
1869 hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength) / 2) * hollowAmount);
1870 break;
1871
1872 default:
1873 hollowVolume = 0;
1874 break;
1875 }
1876 volume = volume - hollowVolume;
1877 }
1878 break;
1879
1880 default:
1881 // we don't have all of the volume formulas yet so
1882 // use the common volume formula for all
1883 volume = _size.X * _size.Y * _size.Z;
1884 break;
1885 }
1886
1887 // Calculate Path cut effect on volume
1888 // Not exact, in the triangle hollow example
1889 // They should never be zero or less then zero..
1890 // we'll ignore it if it's less then zero
1891
1892 // ProfileEnd and ProfileBegin are values
1893 // from 0 to 50000
1894
1895 // Turning them back into percentages so that I can cut that percentage off the volume
1896
1897 float PathCutEndAmount = _pbs.ProfileEnd;
1898 float PathCutStartAmount = _pbs.ProfileBegin;
1899 if (((PathCutStartAmount + PathCutEndAmount) / 50000f) > 0.0f)
1900 {
1901 float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f);
1902
1903 // Check the return amount for sanity
1904 if (pathCutAmount >= 0.99f)
1905 pathCutAmount = 0.99f;
1906
1907 volume = volume - (volume * pathCutAmount);
1908 }
1909 UInt16 taperX = _pbs.PathScaleX;
1910 UInt16 taperY = _pbs.PathScaleY;
1911 float taperFactorX = 0;
1912 float taperFactorY = 0;
1913
1914 // Mass = density * volume
1915 if (taperX != 100)
1916 {
1917 if (taperX > 100)
1918 {
1919 taperFactorX = 1.0f - ((float)taperX / 200);
1920 //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
1921 }
1922 else
1923 {
1924 taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
1925 //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
1926 }
1927 volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
1928 }
1929
1930 if (taperY != 100)
1931 {
1932 if (taperY > 100)
1933 {
1934 taperFactorY = 1.0f - ((float)taperY / 200);
1935 //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
1936 }
1937 else
1938 {
1939 taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
1940 //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
1941 }
1942 volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
1943 }
1944 returnMass = m_density * volume;
1945 if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1946
1947
1948
1949 // Recursively calculate mass
1950 bool HasChildPrim = false;
1951 lock (childrenPrim)
1952 {
1953 if (childrenPrim.Count > 0)
1954 {
1955 HasChildPrim = true;
1956 }
1957
1958 }
1959 if (HasChildPrim)
1960 {
1961 BulletDotNETPrim[] childPrimArr = new BulletDotNETPrim[0];
1962
1963 lock (childrenPrim)
1964 childPrimArr = childrenPrim.ToArray();
1965
1966 for (int i = 0; i < childPrimArr.Length; i++)
1967 {
1968 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1969 returnMass += childPrimArr[i].CalculateMass();
1970 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1971 if (i > 256)
1972 break;
1973 }
1974 }
1975
1976
1977
1978
1979
1980 return returnMass;
1981 }
1982
1983 #endregion
1984
1985
1986 public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh)
1987 {
1988 m_log.Debug("[PHYSICS]: _________CreateGeom");
1989 if (p_mesh != null)
1990 {
1991 //_mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1992 _mesh = p_mesh;
1993 setMesh(_parent_scene, _mesh);
1994
1995 }
1996 else
1997 {
1998 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1999 {
2000 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
2001 {
2002 if (((_size.X / 2f) > 0f))
2003 {
2004 //SetGeom to a Regular Sphere
2005 if (tempSize1 == null)
2006 tempSize1 = new btVector3(0, 0, 0);
2007 tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
2008 SetCollisionShape(new btSphereShape(_size.X * 0.5f));
2009 }
2010 else
2011 {
2012 // uses halfextents
2013 if (tempSize1 == null)
2014 tempSize1 = new btVector3(0, 0, 0);
2015 tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
2016 SetCollisionShape(new btBoxShape(tempSize1));
2017 }
2018 }
2019 else
2020 {
2021 // uses halfextents
2022 if (tempSize1 == null)
2023 tempSize1 = new btVector3(0, 0, 0);
2024 tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
2025 SetCollisionShape(new btBoxShape(tempSize1));
2026 }
2027
2028 }
2029 else
2030 {
2031 if (tempSize1 == null)
2032 tempSize1 = new btVector3(0, 0, 0);
2033 // uses halfextents
2034 tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
2035 SetCollisionShape(new btBoxShape(tempSize1));
2036 }
2037 }
2038 }
2039
2040 private void setMesh(BulletDotNETScene _parent_scene, IMesh mesh)
2041 {
2042 // TODO: Set Collision Body Mesh
2043 // This sleeper is there to moderate how long it takes between
2044 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
2045 m_log.Debug("_________SetMesh");
2046 Thread.Sleep(10);
2047
2048 //Kill Body so that mesh can re-make the geom
2049 if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
2050 {
2051 if (childPrim)
2052 {
2053 if (_parent != null)
2054 {
2055 BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
2056 parent.ChildDelink(this);
2057 }
2058 }
2059 else
2060 {
2061 //disableBody();
2062 }
2063 }
2064
2065 //IMesh oldMesh = primMesh;
2066
2067 //primMesh = mesh;
2068
2069 //float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
2070 //int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
2071 ////Array.Reverse(indexList);
2072 //primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory
2073
2074 IMesh oldMesh = primMesh;
2075
2076 primMesh = mesh;
2077
2078 float[] vertexList = mesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
2079 int[] indexList = mesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
2080 //Array.Reverse(indexList);
2081 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
2082
2083
2084 int VertexCount = vertexList.GetLength(0) / 3;
2085 int IndexCount = indexList.GetLength(0);
2086
2087 if (btshapeArray != null && btshapeArray.Handle != IntPtr.Zero)
2088 btshapeArray.Dispose();
2089 //Array.Reverse(indexList);
2090 btshapeArray = new btTriangleIndexVertexArray(IndexCount / 3, indexList, (3 * sizeof(int)),
2091 VertexCount, vertexList, 3 * sizeof(float));
2092 SetCollisionShape(new btGImpactMeshShape(btshapeArray));
2093 //((btGImpactMeshShape) prim_geom).updateBound();
2094 ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
2095 ((btGImpactMeshShape)prim_geom).updateBound();
2096 _parent_scene.SetUsingGImpact();
2097 //if (oldMesh != null)
2098 //{
2099 // oldMesh.releasePinned();
2100 // oldMesh = null;
2101 //}
2102
2103 }
2104
2105 private void SetCollisionShape(btCollisionShape shape)
2106 {
2107 /*
2108 if (shape == null)
2109 m_log.Debug("[PHYSICS]:SetShape!Null");
2110 else
2111 m_log.Debug("[PHYSICS]:SetShape!");
2112
2113 if (Body != null)
2114 {
2115 DisposeOfBody();
2116 }
2117
2118 if (prim_geom != null)
2119 {
2120 prim_geom.Dispose();
2121 prim_geom = null;
2122 }
2123 */
2124 prim_geom = shape;
2125
2126 //Body.set
2127 }
2128
2129 public void SetBody(float mass)
2130 {
2131
2132 if (!IsPhysical || childrenPrim.Count == 0)
2133 {
2134 if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
2135 tempMotionState1.Dispose();
2136 if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
2137 tempTransform2.Dispose();
2138 if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
2139 tempOrientation2.Dispose();
2140
2141 if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
2142 tempPosition2.Dispose();
2143
2144 tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
2145 tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
2146 tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
2147 tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
2148 if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
2149 tempInertia1.Dispose();
2150 tempInertia1 = new btVector3(0, 0, 0);
2151
2152
2153 prim_geom.calculateLocalInertia(mass, tempInertia1);
2154
2155 if (mass != 0)
2156 _parent_scene.addActivePrim(this);
2157 else
2158 _parent_scene.remActivePrim(this);
2159
2160 // Body = new btRigidBody(mass, tempMotionState1, prim_geom);
2161 //else
2162 Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
2163
2164 if (prim_geom is btGImpactMeshShape)
2165 {
2166 ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
2167 ((btGImpactMeshShape)prim_geom).updateBound();
2168 }
2169 //Body.setCollisionFlags(Body.getCollisionFlags() | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
2170 //Body.setUserPointer((IntPtr) (int)m_localID);
2171 _parent_scene.AddPrimToScene(this);
2172 }
2173 else
2174 {
2175 // bool hasTrimesh = false;
2176 lock (childrenPrim)
2177 {
2178 foreach (BulletDotNETPrim chld in childrenPrim)
2179 {
2180 if (chld == null)
2181 continue;
2182
2183 // if (chld.NeedsMeshing())
2184 // hasTrimesh = true;
2185 }
2186 }
2187
2188 //if (hasTrimesh)
2189 //{
2190 ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
2191 // createmesh returns null when it doesn't mesh.
2192
2193 /*
2194 if (_mesh is Mesh)
2195 {
2196 }
2197 else
2198 {
2199 m_log.Warn("[PHYSICS]: Can't link a OpenSim.Region.Physics.Meshing.Mesh object");
2200 return;
2201 }
2202 */
2203
2204
2205
2206 foreach (BulletDotNETPrim chld in childrenPrim)
2207 {
2208 if (chld == null)
2209 continue;
2210 Vector3 offset = chld.Position - Position;
2211 Vector3 pos = new Vector3(offset.X, offset.Y, offset.Z);
2212 pos *= Quaternion.Inverse(Orientation);
2213 //pos *= Orientation;
2214 offset = pos;
2215 chld.ProcessGeomCreationAsTriMesh(offset, chld.Orientation);
2216
2217 _mesh.Append(chld._mesh);
2218
2219
2220 }
2221 setMesh(_parent_scene, _mesh);
2222
2223 //}
2224
2225 if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
2226 tempMotionState1.Dispose();
2227 if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
2228 tempTransform2.Dispose();
2229 if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
2230 tempOrientation2.Dispose();
2231
2232 if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
2233 tempPosition2.Dispose();
2234
2235 tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
2236 tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
2237 tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
2238 tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
2239 if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
2240 tempInertia1.Dispose();
2241 tempInertia1 = new btVector3(0, 0, 0);
2242
2243
2244 prim_geom.calculateLocalInertia(mass, tempInertia1);
2245
2246 if (mass != 0)
2247 _parent_scene.addActivePrim(this);
2248 else
2249 _parent_scene.remActivePrim(this);
2250
2251 // Body = new btRigidBody(mass, tempMotionState1, prim_geom);
2252 //else
2253 Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
2254
2255 if (prim_geom is btGImpactMeshShape)
2256 {
2257 ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
2258 ((btGImpactMeshShape)prim_geom).updateBound();
2259 }
2260 _parent_scene.AddPrimToScene(this);
2261
2262 }
2263
2264 if (IsPhysical)
2265 changeAngularLock(0);
2266 }
2267
2268 private void DisposeOfBody()
2269 {
2270 if (Body != null)
2271 {
2272 if (Body.Handle != IntPtr.Zero)
2273 {
2274 DisableAxisMotor();
2275 _parent_scene.removeFromWorld(this, Body);
2276 Body.Dispose();
2277 }
2278 Body = null;
2279 // TODO: dispose parts that make up body
2280 }
2281 }
2282
2283 private void ChildDelink(BulletDotNETPrim pPrim)
2284 {
2285 // Okay, we have a delinked child.. need to rebuild the body.
2286 lock (childrenPrim)
2287 {
2288 foreach (BulletDotNETPrim prm in childrenPrim)
2289 {
2290 prm.childPrim = true;
2291 prm.disableBody();
2292
2293 }
2294 }
2295 disableBody();
2296
2297 lock (childrenPrim)
2298 {
2299 childrenPrim.Remove(pPrim);
2300 }
2301
2302
2303
2304
2305 if (Body != null && Body.Handle != IntPtr.Zero)
2306 {
2307 _parent_scene.remActivePrim(this);
2308 }
2309
2310
2311
2312 lock (childrenPrim)
2313 {
2314 foreach (BulletDotNETPrim prm in childrenPrim)
2315 {
2316 ParentPrim(prm);
2317 }
2318 }
2319
2320 }
2321
2322 internal void ParentPrim(BulletDotNETPrim prm)
2323 {
2324 if (prm == null)
2325 return;
2326
2327
2328
2329 lock (childrenPrim)
2330 {
2331 if (!childrenPrim.Contains(prm))
2332 {
2333 childrenPrim.Add(prm);
2334 }
2335 }
2336
2337
2338 }
2339
2340 public void disableBody()
2341 {
2342 //this kills the body so things like 'mesh' can re-create it.
2343 /*
2344 lock (this)
2345 {
2346 if (!childPrim)
2347 {
2348 if (Body != null && Body.Handle != IntPtr.Zero)
2349 {
2350 _parent_scene.remActivePrim(this);
2351
2352 m_collisionCategories &= ~CollisionCategories.Body;
2353 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
2354
2355 if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
2356 {
2357 // TODO: Set Category bits and Flags
2358 }
2359
2360 // TODO: destroy body
2361 DisposeOfBody();
2362
2363 lock (childrenPrim)
2364 {
2365 if (childrenPrim.Count > 0)
2366 {
2367 foreach (BulletDotNETPrim prm in childrenPrim)
2368 {
2369 _parent_scene.remActivePrim(prm);
2370 prm.DisposeOfBody();
2371 prm.SetCollisionShape(null);
2372 }
2373 }
2374
2375 }
2376
2377 DisposeOfBody();
2378 }
2379 }
2380 else
2381 {
2382 _parent_scene.remActivePrim(this);
2383 m_collisionCategories &= ~CollisionCategories.Body;
2384 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
2385
2386 if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
2387 {
2388 // TODO: Set Category bits and Flags
2389 }
2390
2391 DisposeOfBody();
2392 }
2393
2394 }
2395 */
2396 DisableAxisMotor();
2397 m_disabled = true;
2398 m_collisionscore = 0;
2399 }
2400
2401 public void disableBodySoft()
2402 {
2403 m_disabled = true;
2404
2405 if (m_isphysical && Body.Handle != IntPtr.Zero)
2406 {
2407 Body.clearForces();
2408 Body.forceActivationState(0);
2409
2410 }
2411
2412 }
2413
2414 public void enableBodySoft()
2415 {
2416 if (!childPrim)
2417 {
2418 if (m_isphysical && Body.Handle != IntPtr.Zero)
2419 {
2420 Body.clearForces();
2421 Body.forceActivationState(4);
2422 forceenable = true;
2423
2424 }
2425 m_disabled = false;
2426 }
2427 }
2428
2429 public void enableBody()
2430 {
2431 if (!childPrim)
2432 {
2433 //SetCollisionShape(prim_geom);
2434 if (IsPhysical)
2435 SetBody(Mass);
2436 else
2437 SetBody(0);
2438
2439 // TODO: Set Collision Category Bits and Flags
2440 // TODO: Set Auto Disable data
2441
2442 m_interpenetrationcount = 0;
2443 m_collisionscore = 0;
2444 m_disabled = false;
2445 // The body doesn't already have a finite rotation mode set here
2446 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
2447 {
2448 // TODO: Create Angular Motor on Axis Lock!
2449 }
2450 _parent_scene.addActivePrim(this);
2451 }
2452 }
2453
2454 public void UpdatePositionAndVelocity()
2455 {
2456 if (!m_isSelected)
2457 {
2458 if (_parent == null)
2459 {
2460 Vector3 pv = Vector3.Zero;
2461 bool lastZeroFlag = _zeroFlag;
2462 if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
2463 tempPosition3.Dispose();
2464 if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
2465 tempTransform3.Dispose();
2466
2467 if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
2468 tempOrientation2.Dispose();
2469
2470 if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
2471 tempAngularVelocity1.Dispose();
2472
2473 if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
2474 tempLinearVelocity1.Dispose();
2475
2476
2477
2478 tempTransform3 = Body.getInterpolationWorldTransform();
2479 tempPosition3 = tempTransform3.getOrigin(); // vec
2480 tempOrientation2 = tempTransform3.getRotation(); // ori
2481 tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
2482 tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
2483
2484 _torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
2485 tempAngularVelocity1.getZ());
2486 Vector3 l_position = Vector3.Zero;
2487 Quaternion l_orientation = Quaternion.Identity;
2488 m_lastposition = _position;
2489 m_lastorientation = _orientation;
2490
2491 l_position.X = tempPosition3.getX();
2492 l_position.Y = tempPosition3.getY();
2493 l_position.Z = tempPosition3.getZ();
2494 l_orientation.X = tempOrientation2.getX();
2495 l_orientation.Y = tempOrientation2.getY();
2496 l_orientation.Z = tempOrientation2.getZ();
2497 l_orientation.W = tempOrientation2.getW();
2498
2499 if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
2500 {
2501 //base.RaiseOutOfBounds(l_position);
2502
2503 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2504 {
2505 _position = l_position;
2506 //_parent_scene.remActivePrim(this);
2507 if (_parent == null)
2508 base.RequestPhysicsterseUpdate();
2509 return;
2510 }
2511 else
2512 {
2513 if (_parent == null)
2514 base.RaiseOutOfBounds(l_position);
2515 return;
2516 }
2517 }
2518
2519 if (l_position.Z < -200000f)
2520 {
2521 // This is so prim that get lost underground don't fall forever and suck up
2522 //
2523 // Sim resources and memory.
2524 // Disables the prim's movement physics....
2525 // It's a hack and will generate a console message if it fails.
2526
2527 //IsPhysical = false;
2528 //if (_parent == null)
2529 //base.RaiseOutOfBounds(_position);
2530
2531 _acceleration.X = 0;
2532 _acceleration.Y = 0;
2533 _acceleration.Z = 0;
2534
2535 _velocity.X = 0;
2536 _velocity.Y = 0;
2537 _velocity.Z = 0;
2538 m_rotationalVelocity.X = 0;
2539 m_rotationalVelocity.Y = 0;
2540 m_rotationalVelocity.Z = 0;
2541
2542 if (_parent == null)
2543 base.RequestPhysicsterseUpdate();
2544
2545 m_throttleUpdates = false;
2546 // throttleCounter = 0;
2547 _zeroFlag = true;
2548 //outofBounds = true;
2549 }
2550
2551 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2552 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2553 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2554 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2555 {
2556 _zeroFlag = true;
2557 m_throttleUpdates = false;
2558 }
2559 else
2560 {
2561 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2562 _zeroFlag = false;
2563 }
2564
2565 if (_zeroFlag)
2566 {
2567 _velocity.X = 0.0f;
2568 _velocity.Y = 0.0f;
2569 _velocity.Z = 0.0f;
2570
2571 _acceleration.X = 0;
2572 _acceleration.Y = 0;
2573 _acceleration.Z = 0;
2574
2575 //_orientation.w = 0f;
2576 //_orientation.X = 0f;
2577 //_orientation.Y = 0f;
2578 //_orientation.Z = 0f;
2579 m_rotationalVelocity.X = 0;
2580 m_rotationalVelocity.Y = 0;
2581 m_rotationalVelocity.Z = 0;
2582 if (!m_lastUpdateSent)
2583 {
2584 m_throttleUpdates = false;
2585 // throttleCounter = 0;
2586 m_rotationalVelocity = pv;
2587
2588 if (_parent == null)
2589 base.RequestPhysicsterseUpdate();
2590
2591 m_lastUpdateSent = true;
2592 }
2593 }
2594 else
2595 {
2596 if (lastZeroFlag != _zeroFlag)
2597 {
2598 if (_parent == null)
2599 base.RequestPhysicsterseUpdate();
2600 }
2601
2602 m_lastVelocity = _velocity;
2603
2604 _position = l_position;
2605
2606 _velocity.X = tempLinearVelocity1.getX();
2607 _velocity.Y = tempLinearVelocity1.getY();
2608 _velocity.Z = tempLinearVelocity1.getZ();
2609
2610 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2611 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
2612 _velocity.Y - m_lastVelocity.Y / 0.1f,
2613 _velocity.Z - m_lastVelocity.Z / 0.1f);
2614 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2615
2616 if (_velocity.ApproxEquals(pv, 0.5f))
2617 {
2618 m_rotationalVelocity = pv;
2619 }
2620 else
2621 {
2622 m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
2623 }
2624
2625 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2626
2627 _orientation.X = l_orientation.X;
2628 _orientation.Y = l_orientation.Y;
2629 _orientation.Z = l_orientation.Z;
2630 _orientation.W = l_orientation.W;
2631 m_lastUpdateSent = false;
2632
2633 //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2634 //{
2635 if (_parent == null)
2636 base.RequestPhysicsterseUpdate();
2637 // }
2638 // else
2639 // {
2640 // throttleCounter++;
2641 //}
2642
2643 }
2644 m_lastposition = l_position;
2645 if (forceenable)
2646 {
2647 Body.forceActivationState(1);
2648 forceenable = false;
2649 }
2650 }
2651 else
2652 {
2653 // Not a body.. so Make sure the client isn't interpolating
2654 _velocity.X = 0;
2655 _velocity.Y = 0;
2656 _velocity.Z = 0;
2657
2658 _acceleration.X = 0;
2659 _acceleration.Y = 0;
2660 _acceleration.Z = 0;
2661
2662 m_rotationalVelocity.X = 0;
2663 m_rotationalVelocity.Y = 0;
2664 m_rotationalVelocity.Z = 0;
2665 _zeroFlag = true;
2666 }
2667 }
2668 }
2669
2670
2671 internal void setPrimForRemoval()
2672 {
2673 m_taintremove = true;
2674 }
2675
2676 internal void EnableAxisMotor(Vector3 axislock)
2677 {
2678 if (m_aMotor != null)
2679 DisableAxisMotor();
2680
2681 if (Body == null)
2682 return;
2683
2684 if (Body.Handle == IntPtr.Zero)
2685 return;
2686
2687 if (AxisLockAngleHigh != null && AxisLockAngleHigh.Handle != IntPtr.Zero)
2688 AxisLockAngleHigh.Dispose();
2689
2690
2691
2692 m_aMotor = new btGeneric6DofConstraint(Body, _parent_scene.TerrainBody, _parent_scene.TransZero,
2693 _parent_scene.TransZero, false);
2694
2695 float endNoLock = (360 * Utils.DEG_TO_RAD);
2696 AxisLockAngleHigh = new btVector3((axislock.X == 0) ? 0 : endNoLock, (axislock.Y == 0) ? 0 : endNoLock, (axislock.Z == 0) ? 0 : endNoLock);
2697
2698 m_aMotor.setAngularLowerLimit(_parent_scene.VectorZero);
2699 m_aMotor.setAngularUpperLimit(AxisLockAngleHigh);
2700 m_aMotor.setLinearLowerLimit(AxisLockLinearLow);
2701 m_aMotor.setLinearUpperLimit(AxisLockLinearHigh);
2702 _parent_scene.getBulletWorld().addConstraint((btTypedConstraint)m_aMotor);
2703 //m_aMotor.
2704
2705
2706 }
2707 internal void DisableAxisMotor()
2708 {
2709 if (m_aMotor != null && m_aMotor.Handle != IntPtr.Zero)
2710 {
2711 _parent_scene.getBulletWorld().removeConstraint(m_aMotor);
2712 m_aMotor.Dispose();
2713 m_aMotor = null;
2714 }
2715 }
2716
2717 }
2718}
2719
generated by cgit v1.1 at 2025-01-24 07:58:55 +0000