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