aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1950
1 files changed, 1950 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..bea34d4
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1950 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28
29// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private Vector3 _velocity;
78 private Vector3 _target_velocity;
79 private Vector3 _acceleration;
80 private Vector3 m_rotationalVelocity;
81 private Vector3 m_size;
82 private Quaternion m_orientation;
83 private Quaternion m_orientation2D;
84 private float m_mass = 80f;
85 public float m_density = 60f;
86 private bool m_pidControllerActive = true;
87
88 const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
89 const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
90 public float PID_D;
91 public float PID_P;
92
93 private float timeStep;
94 private float invtimeStep;
95
96 private float m_feetOffset = 0;
97 private float feetOff = 0;
98 private float feetSZ = 0.5f;
99 const float feetScale = 0.8f;
100 private float boneOff = 0;
101 private float m_lastVelocitySqr = 0;
102
103 public float walkDivisor = 1.3f;
104 public float runDivisor = 0.8f;
105 private bool flying = false;
106 private bool m_iscolliding = false;
107 private bool m_iscollidingGround = false;
108 private bool m_iscollidingObj = false;
109 private bool m_alwaysRun = false;
110
111 private bool _zeroFlag = false;
112
113 private int m_requestedUpdateFrequency = 0;
114 private uint m_localID = 0;
115 public bool m_returnCollisions = false;
116 // taints and their non-tainted counterparts
117 public bool m_isPhysical = false; // the current physical status
118 public float MinimumGroundFlightOffset = 3f;
119
120 private float m_buoyancy = 0f;
121
122 private bool m_freemove = false;
123 // private CollisionLocker ode;
124
125// private string m_name = String.Empty;
126 // other filter control
127 int m_colliderfilter = 0;
128 int m_colliderGroundfilter = 0;
129 int m_colliderObjectfilter = 0;
130 bool m_collisionException = false;
131
132 // Default we're a Character
133 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
134
135 // Default, Collide with Other Geometries, spaces, bodies and characters.
136 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
137 | CollisionCategories.Geom
138 | CollisionCategories.VolumeDtc
139 );
140 // we do land collisions not ode | CollisionCategories.Land);
141 public IntPtr Body = IntPtr.Zero;
142 private OdeScene _parent_scene;
143 private IntPtr topbox = IntPtr.Zero;
144 private IntPtr midbox = IntPtr.Zero;
145 private IntPtr feetbox = IntPtr.Zero;
146 private IntPtr bbox = IntPtr.Zero;
147 public IntPtr collider = IntPtr.Zero;
148
149 public IntPtr Amotor = IntPtr.Zero;
150
151 public d.Mass ShellMass;
152
153
154
155
156 public int m_eventsubscription = 0;
157 private int m_cureventsubscription = 0;
158 private CollisionEventUpdate CollisionEventsThisFrame = null;
159 private bool SentEmptyCollisionsEvent;
160
161 // unique UUID of this character object
162 public UUID m_uuid;
163 public bool bad = false;
164
165 float mu;
166
167
168
169 public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
170 {
171 m_uuid = UUID.Random();
172 m_localID = localID;
173
174 timeStep = parent_scene.ODE_STEPSIZE;
175 invtimeStep = 1 / timeStep;
176
177 if (pos.IsFinite())
178 {
179 if (pos.Z > 99999f)
180 {
181 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
182 }
183 if (pos.Z < -100f) // shouldn't this be 0 ?
184 {
185 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
186 }
187 _position = pos;
188 }
189 else
190 {
191 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
192 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
193 }
194
195 _parent_scene = parent_scene;
196
197
198 m_size.X = pSize.X;
199 m_size.Y = pSize.Y;
200 m_size.Z = pSize.Z;
201
202 if(m_size.X <0.01f)
203 m_size.X = 0.01f;
204 if(m_size.Y <0.01f)
205 m_size.Y = 0.01f;
206 if(m_size.Z <0.01f)
207 m_size.Z = 0.01f;
208
209 m_feetOffset = pfeetOffset;
210 m_orientation = Quaternion.Identity;
211 m_orientation2D = Quaternion.Identity;
212 m_density = density;
213
214 // force lower density for testing
215 m_density = 3.0f;
216
217 m_density *= 1.4f; // scale to have mass similar to capsule
218
219 mu = parent_scene.AvatarFriction;
220
221 walkDivisor = walk_divisor;
222 runDivisor = rundivisor;
223
224 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
225
226 PID_D = basePID_D * m_mass * invtimeStep;
227 PID_P = basePID_P * m_mass * invtimeStep;
228
229 m_isPhysical = false; // current status: no ODE information exists
230
231 Name = avName;
232
233 AddChange(changes.Add, null);
234 }
235
236 public override int PhysicsActorType
237 {
238 get { return (int)ActorTypes.Agent; }
239 set { return; }
240 }
241
242 public override void getContactData(ref ContactData cdata)
243 {
244 cdata.mu = mu;
245 cdata.bounce = 0;
246 cdata.softcolide = false;
247 }
248
249 public override bool Building { get; set; }
250
251 /// <summary>
252 /// If this is set, the avatar will move faster
253 /// </summary>
254 public override bool SetAlwaysRun
255 {
256 get { return m_alwaysRun; }
257 set { m_alwaysRun = value; }
258 }
259
260 public override uint LocalID
261 {
262 get { return m_localID; }
263 set { m_localID = value; }
264 }
265
266 public override PhysicsActor ParentActor
267 {
268 get { return (PhysicsActor)this; }
269 }
270
271 public override bool Grabbed
272 {
273 set { return; }
274 }
275
276 public override bool Selected
277 {
278 set { return; }
279 }
280
281 public override float Buoyancy
282 {
283 get { return m_buoyancy; }
284 set { m_buoyancy = value; }
285 }
286
287 public override bool FloatOnWater
288 {
289 set { return; }
290 }
291
292 public override bool IsPhysical
293 {
294 get { return m_isPhysical; }
295 set { return; }
296 }
297
298 public override bool ThrottleUpdates
299 {
300 get { return false; }
301 set { return; }
302 }
303
304 public override bool Flying
305 {
306 get { return flying; }
307 set
308 {
309 flying = value;
310// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
311 }
312 }
313
314 /// <summary>
315 /// Returns if the avatar is colliding in general.
316 /// This includes the ground and objects and avatar.
317 /// </summary>
318 public override bool IsColliding
319 {
320 get { return (m_iscolliding || m_iscollidingGround); }
321 set
322 {
323 if (value)
324 {
325 m_colliderfilter += 2;
326 if (m_colliderfilter > 2)
327 m_colliderfilter = 2;
328 }
329 else
330 {
331 m_colliderfilter--;
332 if (m_colliderfilter < 0)
333 m_colliderfilter = 0;
334 }
335
336 if (m_colliderfilter == 0)
337 m_iscolliding = false;
338 else
339 {
340 m_pidControllerActive = true;
341 m_iscolliding = true;
342 }
343 }
344 }
345
346 /// <summary>
347 /// Returns if an avatar is colliding with the ground
348 /// </summary>
349 public override bool CollidingGround
350 {
351 get { return m_iscollidingGround; }
352 set
353 {
354/* we now control this
355 if (value)
356 {
357 m_colliderGroundfilter += 2;
358 if (m_colliderGroundfilter > 2)
359 m_colliderGroundfilter = 2;
360 }
361 else
362 {
363 m_colliderGroundfilter--;
364 if (m_colliderGroundfilter < 0)
365 m_colliderGroundfilter = 0;
366 }
367
368 if (m_colliderGroundfilter == 0)
369 m_iscollidingGround = false;
370 else
371 m_iscollidingGround = true;
372 */
373 }
374
375 }
376
377 /// <summary>
378 /// Returns if the avatar is colliding with an object
379 /// </summary>
380 public override bool CollidingObj
381 {
382 get { return m_iscollidingObj; }
383 set
384 {
385 // Ubit filter this also
386 if (value)
387 {
388 m_colliderObjectfilter += 2;
389 if (m_colliderObjectfilter > 2)
390 m_colliderObjectfilter = 2;
391 }
392 else
393 {
394 m_colliderObjectfilter--;
395 if (m_colliderObjectfilter < 0)
396 m_colliderObjectfilter = 0;
397 }
398
399 if (m_colliderObjectfilter == 0)
400 m_iscollidingObj = false;
401 else
402 m_iscollidingObj = true;
403
404// m_iscollidingObj = value;
405
406 if (m_iscollidingObj)
407 m_pidControllerActive = false;
408 else
409 m_pidControllerActive = true;
410 }
411 }
412
413 /// <summary>
414 /// turn the PID controller on or off.
415 /// The PID Controller will turn on all by itself in many situations
416 /// </summary>
417 /// <param name="status"></param>
418 public void SetPidStatus(bool status)
419 {
420 m_pidControllerActive = status;
421 }
422
423 public override bool Stopped
424 {
425 get { return _zeroFlag; }
426 }
427
428 /// <summary>
429 /// This 'puts' an avatar somewhere in the physics space.
430 /// Not really a good choice unless you 'know' it's a good
431 /// spot otherwise you're likely to orbit the avatar.
432 /// </summary>
433 public override Vector3 Position
434 {
435 get { return _position; }
436 set
437 {
438 if (value.IsFinite())
439 {
440 if (value.Z > 9999999f)
441 {
442 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
443 }
444 if (value.Z < -100f)
445 {
446 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
447 }
448 AddChange(changes.Position, value);
449 }
450 else
451 {
452 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
453 }
454 }
455 }
456
457 public override Vector3 RotationalVelocity
458 {
459 get { return m_rotationalVelocity; }
460 set { m_rotationalVelocity = value; }
461 }
462
463 /// <summary>
464 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
465 /// and use it to offset landings properly
466 /// </summary>
467 public override Vector3 Size
468 {
469 get
470 {
471 return m_size;
472 }
473 set
474 {
475 if (value.IsFinite())
476 {
477 if(value.X <0.01f)
478 value.X = 0.01f;
479 if(value.Y <0.01f)
480 value.Y = 0.01f;
481 if(value.Z <0.01f)
482 value.Z = 0.01f;
483
484 AddChange(changes.Size, value);
485 }
486 else
487 {
488 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
489 }
490 }
491 }
492
493 public override void setAvatarSize(Vector3 size, float feetOffset)
494 {
495 if (size.IsFinite())
496 {
497 if (size.X < 0.01f)
498 size.X = 0.01f;
499 if (size.Y < 0.01f)
500 size.Y = 0.01f;
501 if (size.Z < 0.01f)
502 size.Z = 0.01f;
503
504 strAvatarSize st = new strAvatarSize();
505 st.size = size;
506 st.offset = feetOffset;
507 AddChange(changes.AvatarSize, st);
508 }
509 else
510 {
511 m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
512 }
513
514 }
515 /// <summary>
516 /// This creates the Avatar's physical Surrogate at the position supplied
517 /// </summary>
518 /// <param name="npositionX"></param>
519 /// <param name="npositionY"></param>
520 /// <param name="npositionZ"></param>
521
522 //
523 /// <summary>
524 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
525 /// This may be used in calculations in the scene/scenepresence
526 /// </summary>
527 public override float Mass
528 {
529 get
530 {
531 return m_mass;
532 }
533 }
534 public override void link(PhysicsActor obj)
535 {
536
537 }
538
539 public override void delink()
540 {
541
542 }
543
544 public override void LockAngularMotion(Vector3 axis)
545 {
546
547 }
548
549
550 public override Vector3 Force
551 {
552 get { return _target_velocity; }
553 set { return; }
554 }
555
556 public override int VehicleType
557 {
558 get { return 0; }
559 set { return; }
560 }
561
562 public override void VehicleFloatParam(int param, float value)
563 {
564
565 }
566
567 public override void VehicleVectorParam(int param, Vector3 value)
568 {
569
570 }
571
572 public override void VehicleRotationParam(int param, Quaternion rotation)
573 {
574
575 }
576
577 public override void VehicleFlags(int param, bool remove)
578 {
579
580 }
581
582 public override void SetVolumeDetect(int param)
583 {
584
585 }
586
587 public override Vector3 CenterOfMass
588 {
589 get
590 {
591 Vector3 pos = _position;
592 return pos;
593 }
594 }
595
596 public override Vector3 GeometricCenter
597 {
598 get
599 {
600 Vector3 pos = _position;
601 return pos;
602 }
603 }
604
605 public override PrimitiveBaseShape Shape
606 {
607 set { return; }
608 }
609
610 public override Vector3 Velocity
611 {
612 get
613 {
614 return _velocity;
615 }
616 set
617 {
618 if (value.IsFinite())
619 {
620 AddChange(changes.Velocity, value);
621 }
622 else
623 {
624 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
625 }
626 }
627 }
628
629 public override Vector3 Torque
630 {
631 get { return Vector3.Zero; }
632 set { return; }
633 }
634
635 public override float CollisionScore
636 {
637 get { return 0f; }
638 set { }
639 }
640
641 public override bool Kinematic
642 {
643 get { return false; }
644 set { }
645 }
646
647 public override Quaternion Orientation
648 {
649 get { return m_orientation; }
650 set
651 {
652// fakeori = value;
653// givefakeori++;
654 value.Normalize();
655 AddChange(changes.Orientation, value);
656 }
657 }
658
659 public override Vector3 Acceleration
660 {
661 get { return _acceleration; }
662 set { }
663 }
664
665 public void SetAcceleration(Vector3 accel)
666 {
667 m_pidControllerActive = true;
668 _acceleration = accel;
669 }
670
671 /// <summary>
672 /// Adds the force supplied to the Target Velocity
673 /// The PID controller takes this target velocity and tries to make it a reality
674 /// </summary>
675 /// <param name="force"></param>
676 public override void AddForce(Vector3 force, bool pushforce)
677 {
678 if (force.IsFinite())
679 {
680 if (pushforce)
681 {
682 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
683 }
684 else
685 {
686 AddChange(changes.Velocity, force);
687 }
688 }
689 else
690 {
691 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
692 }
693 //m_lastUpdateSent = false;
694 }
695
696 public override void AddAngularForce(Vector3 force, bool pushforce)
697 {
698
699 }
700
701 public override void SetMomentum(Vector3 momentum)
702 {
703 if (momentum.IsFinite())
704 AddChange(changes.Momentum, momentum);
705 }
706
707 private void ajustCollider()
708 {
709 float vq = _velocity.LengthSquared();
710 if (m_lastVelocitySqr != vq)
711 {
712 m_lastVelocitySqr = vq;
713 if (vq > 100.0f)
714 {
715 Vector3 off = _velocity;
716 float t = 0.5f * timeStep;
717 off = off * t;
718 d.Quaternion qtmp;
719 d.GeomCopyQuaternion(bbox, out qtmp);
720 Quaternion q;
721 q.X = qtmp.X;
722 q.Y = qtmp.Y;
723 q.Z = qtmp.Z;
724 q.W = qtmp.W;
725 off *= Quaternion.Conjugate(q);
726
727 d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
728
729 off.X = 2.0f * (m_size.X + Math.Abs(off.X));
730 off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
731 off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
732 d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
733
734 d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
735 d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
736 d.GeomSetCategoryBits(topbox, 0);
737 d.GeomSetCollideBits(topbox, 0);
738 d.GeomSetCategoryBits(midbox, 0);
739 d.GeomSetCollideBits(midbox, 0);
740 d.GeomSetCategoryBits(feetbox, 0);
741 d.GeomSetCollideBits(feetbox, 0);
742 }
743 else
744 {
745 d.GeomSetCategoryBits(bbox, 0);
746 d.GeomSetCollideBits(bbox, 0);
747 d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
748 d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
749 d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
750 d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
751 d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
752 d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
753 }
754 uint cat1 = d.GeomGetCategoryBits(bbox);
755 uint col1 = d.GeomGetCollideBits(bbox);
756
757 }
758 }
759
760 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
761 {
762 // sizes one day should came from visual parameters
763 float sx = m_size.X;
764 float sy = m_size.Y;
765 float sz = m_size.Z;
766
767 float topsx = sx * 0.9f;
768 float midsx = sx;
769 float feetsx = sx * feetScale;
770 float bonesx = sx * 0.2f;
771
772 float topsy = sy * 0.4f;
773 float midsy = sy;
774 float feetsy = sy * feetScale * 0.8f;
775 float bonesy = feetsy * 0.2f;
776
777 float topsz = sz * 0.15f;
778 float feetsz = sz * 0.45f;
779 if (feetsz > 0.6f)
780 feetsz = 0.6f;
781
782 float midsz = sz - topsz - feetsz;
783 float bonesz = sz;
784
785 float bot = -sz * 0.5f + m_feetOffset;
786
787 boneOff = bot + 0.3f;
788
789 float feetz = bot + feetsz * 0.5f;
790 bot += feetsz;
791
792 feetOff = bot;
793 feetSZ = feetsz;
794
795 float midz = bot + midsz * 0.5f;
796 bot += midsz;
797 float topz = bot + topsz * 0.5f;
798
799 _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
800
801 collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
802 d.HashSpaceSetLevels(collider, -4, 3);
803 d.SpaceSetSublevel(collider, 3);
804 d.SpaceSetCleanup(collider, false);
805 d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
806 d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
807
808 feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
809 midbox = d.CreateBox(collider, midsx, midsy, midsz);
810 topbox = d.CreateBox(collider, topsx, topsy, topsz);
811 bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
812
813 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
814
815 d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
816
817 PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
818 PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
819
820 Body = d.BodyCreate(_parent_scene.world);
821
822 _zeroFlag = false;
823 m_collisionException = false;
824 m_pidControllerActive = true;
825 m_freemove = false;
826
827 _velocity = Vector3.Zero;
828 m_lastVelocitySqr = 0;
829
830 d.BodySetAutoDisableFlag(Body, false);
831 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
832
833 _position.X = npositionX;
834 _position.Y = npositionY;
835 _position.Z = npositionZ;
836
837 d.BodySetMass(Body, ref ShellMass);
838 d.GeomSetBody(feetbox, Body);
839 d.GeomSetBody(midbox, Body);
840 d.GeomSetBody(topbox, Body);
841 d.GeomSetBody(bbox, Body);
842
843 d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
844 d.GeomSetOffsetPosition(midbox, 0, 0, midz);
845 d.GeomSetOffsetPosition(topbox, 0, 0, topz);
846
847 ajustCollider();
848
849
850 // The purpose of the AMotor here is to keep the avatar's physical
851 // surrogate from rotating while moving
852 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
853 d.JointAttach(Amotor, Body, IntPtr.Zero);
854
855 d.JointSetAMotorMode(Amotor, 0);
856 d.JointSetAMotorNumAxes(Amotor, 3);
857 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
858 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
859 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
860
861 d.JointSetAMotorAngle(Amotor, 0, 0);
862 d.JointSetAMotorAngle(Amotor, 1, 0);
863 d.JointSetAMotorAngle(Amotor, 2, 0);
864
865 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
866 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
867 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
868 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
869 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
870 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
871
872 // These lowstops and high stops are effectively (no wiggle room)
873 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
874 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
875 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
876 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
877 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
878 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
879
880 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
881 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
882 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
883
884 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
885 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
886 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
887 }
888
889 /// <summary>
890 /// Destroys the avatar body and geom
891
892 private void AvatarGeomAndBodyDestroy()
893 {
894 // Kill the Amotor
895 if (Amotor != IntPtr.Zero)
896 {
897 d.JointDestroy(Amotor);
898 Amotor = IntPtr.Zero;
899 }
900
901 if (Body != IntPtr.Zero)
902 {
903 //kill the body
904 d.BodyDestroy(Body);
905 Body = IntPtr.Zero;
906 }
907
908 //kill the Geoms
909 if (topbox != IntPtr.Zero)
910 {
911 _parent_scene.actor_name_map.Remove(topbox);
912 _parent_scene.waitForSpaceUnlock(collider);
913 d.GeomDestroy(topbox);
914 topbox = IntPtr.Zero;
915 }
916 if (midbox != IntPtr.Zero)
917 {
918 _parent_scene.actor_name_map.Remove(midbox);
919 _parent_scene.waitForSpaceUnlock(collider);
920 d.GeomDestroy(midbox);
921 midbox = IntPtr.Zero;
922 }
923 if (feetbox != IntPtr.Zero)
924 {
925 _parent_scene.actor_name_map.Remove(feetbox);
926 _parent_scene.waitForSpaceUnlock(collider);
927 d.GeomDestroy(feetbox);
928 feetbox = IntPtr.Zero;
929 }
930
931 if (bbox != IntPtr.Zero)
932 {
933 _parent_scene.actor_name_map.Remove(bbox);
934 _parent_scene.waitForSpaceUnlock(collider);
935 d.GeomDestroy(bbox);
936 bbox = IntPtr.Zero;
937 }
938
939 if (collider != IntPtr.Zero)
940 {
941 d.SpaceDestroy(collider);
942 collider = IntPtr.Zero;
943 }
944
945 }
946
947 //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
948 public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
949 {
950 float sin = 2.0f * rot.Z * rot.W;
951 float cos = rot.W * rot.W - rot.Z * rot.Z;
952 float tx = x;
953
954 x = tx * cos - y * sin;
955 y = tx * sin + y * cos;
956 }
957 public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
958 {
959 float tx = x;
960 x = tx * cos - y * sin;
961 y = tx * sin + y * cos;
962 }
963 public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
964 {
965 float tx = x;
966 x = tx * cos + y * sin;
967 y = -tx * sin + y * cos;
968 }
969
970 public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
971 {
972 float sin = - 2.0f * rot.Z * rot.W;
973 float cos = rot.W * rot.W - rot.Z * rot.Z;
974 float tx = x;
975
976 x = tx * cos - y * sin;
977 y = tx * sin + y * cos;
978 }
979
980
981 public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
982 {
983 feetcollision = false;
984 if (m_collisionException)
985 return false;
986
987 Vector3 offset;
988
989 if (me == bbox) // if moving fast
990 {
991 // force a full inelastic collision
992 m_collisionException = true;
993
994 offset = m_size * m_orientation2D;
995
996 offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
997 offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
998 offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
999
1000 if (reverse)
1001 {
1002 offset.X *= -contact.normal.X;
1003 offset.Y *= -contact.normal.Y;
1004 offset.Z *= -contact.normal.Z;
1005 }
1006 else
1007 {
1008 offset.X *= contact.normal.X;
1009 offset.Y *= contact.normal.Y;
1010 offset.Z *= contact.normal.Z;
1011 }
1012
1013 offset.X += contact.pos.X;
1014 offset.Y += contact.pos.Y;
1015 offset.Z += contact.pos.Z;
1016
1017 //_position = offset;
1018 //return false;
1019 }
1020
1021 offset.X = contact.pos.X - _position.X;
1022 offset.Y = contact.pos.Y - _position.Y;
1023
1024 if (me == topbox)
1025 {
1026 offset.Z = contact.pos.Z - _position.Z;
1027
1028 offset.Normalize();
1029
1030 if (reverse)
1031 {
1032 contact.normal.X = offset.X;
1033 contact.normal.Y = offset.Y;
1034 contact.normal.Z = offset.Z;
1035 }
1036 else
1037 {
1038 contact.normal.X = -offset.X;
1039 contact.normal.Y = -offset.Y;
1040 contact.normal.Z = -offset.Z;
1041 }
1042 return true;
1043 }
1044
1045 if (me == midbox)
1046 {
1047 if (Math.Abs(contact.normal.Z) > 0.95f)
1048 offset.Z = contact.pos.Z - _position.Z;
1049 else
1050 offset.Z = contact.normal.Z;
1051
1052 offset.Normalize();
1053
1054 if (reverse)
1055 {
1056 contact.normal.X = offset.X;
1057 contact.normal.Y = offset.Y;
1058 contact.normal.Z = offset.Z;
1059 }
1060 else
1061 {
1062 contact.normal.X = -offset.X;
1063 contact.normal.Y = -offset.Y;
1064 contact.normal.Z = -offset.Z;
1065 }
1066
1067 return true;
1068 }
1069
1070 else if (me == feetbox)
1071 {
1072 float h = contact.pos.Z - _position.Z;
1073
1074 if (Math.Abs(contact.normal.Z) > 0.95f)
1075 {
1076 if (contact.normal.Z > 0)
1077 contact.normal.Z = 1.0f;
1078 else
1079 contact.normal.Z = -1.0f;
1080 contact.normal.X = 0.0f;
1081 contact.normal.Y = 0.0f;
1082 feetcollision = true;
1083 if (h < boneOff)
1084 IsColliding = true;
1085 return true;
1086 }
1087
1088 offset.Z = h - feetOff; // distance from top of feetbox
1089
1090 if (offset.Z > 0)
1091 return false;
1092
1093 if (offset.Z > -0.01)
1094 {
1095 offset.X = 0;
1096 offset.Y = 0;
1097 offset.Z = -1.0f;
1098 }
1099 else
1100 {
1101 offset.Normalize();
1102 }
1103
1104 if (reverse)
1105 {
1106 contact.normal.X = offset.X;
1107 contact.normal.Y = offset.Y;
1108 contact.normal.Z = offset.Z;
1109 }
1110 else
1111 {
1112 contact.normal.X = -offset.X;
1113 contact.normal.Y = -offset.Y;
1114 contact.normal.Z = -offset.Z;
1115 }
1116 feetcollision = true;
1117 if (h < boneOff)
1118 IsColliding = true;
1119 }
1120 else
1121 return false;
1122
1123 return true;
1124 }
1125
1126 /// <summary>
1127 /// Called from Simulate
1128 /// This is the avatar's movement control + PID Controller
1129 /// </summary>
1130 /// <param name="timeStep"></param>
1131 public void Move(List<OdeCharacter> defects)
1132 {
1133 if (Body == IntPtr.Zero)
1134 return;
1135
1136 if (m_collisionException)
1137 {
1138 d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
1139 d.BodySetLinearVel(Body, 0, 0, 0);
1140
1141 float v = _velocity.Length();
1142 if (v != 0)
1143 {
1144 v = 5.0f / v;
1145 _velocity = _velocity * v;
1146 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1147 }
1148 ajustCollider();
1149 m_collisionException = false;
1150 return;
1151 }
1152
1153 d.Vector3 dtmp = d.BodyGetPosition(Body);
1154 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1155
1156 // the Amotor still lets avatar rotation to drift during colisions
1157 // so force it back to identity
1158
1159 d.Quaternion qtmp;
1160 qtmp.W = m_orientation2D.W;
1161 qtmp.X = m_orientation2D.X;
1162 qtmp.Y = m_orientation2D.Y;
1163 qtmp.Z = m_orientation2D.Z;
1164 d.BodySetQuaternion(Body, ref qtmp);
1165
1166 if (m_pidControllerActive == false)
1167 {
1168 _zeroPosition = localpos;
1169 }
1170
1171 if (!localpos.IsFinite())
1172 {
1173 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1174 defects.Add(this);
1175 // _parent_scene.RemoveCharacter(this);
1176
1177 // destroy avatar capsule and related ODE data
1178 AvatarGeomAndBodyDestroy();
1179 return;
1180 }
1181
1182 // check outbounds forcing to be in world
1183 bool fixbody = false;
1184 if (localpos.X < 0.0f)
1185 {
1186 fixbody = true;
1187 localpos.X = 0.1f;
1188 }
1189 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
1190 {
1191 fixbody = true;
1192 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
1193 }
1194 if (localpos.Y < 0.0f)
1195 {
1196 fixbody = true;
1197 localpos.Y = 0.1f;
1198 }
1199 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
1200 {
1201 fixbody = true;
1202 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
1203 }
1204 if (fixbody)
1205 {
1206 m_freemove = false;
1207 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
1208 }
1209
1210 float breakfactor;
1211
1212 Vector3 vec = Vector3.Zero;
1213 dtmp = d.BodyGetLinearVel(Body);
1214 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1215 float velLengthSquared = vel.LengthSquared();
1216
1217 float movementdivisor = 1f;
1218 //Ubit change divisions into multiplications below
1219 if (!m_alwaysRun)
1220 movementdivisor = 1 / walkDivisor;
1221 else
1222 movementdivisor = 1 / runDivisor;
1223
1224 //******************************************
1225 // colide with land
1226
1227 d.AABB aabb;
1228 d.GeomGetAABB(feetbox, out aabb);
1229 float chrminZ = aabb.MinZ; ; // move up a bit
1230 Vector3 posch = localpos;
1231
1232 float ftmp;
1233
1234 if (flying)
1235 {
1236 ftmp = timeStep;
1237 posch.X += vel.X * ftmp;
1238 posch.Y += vel.Y * ftmp;
1239 }
1240
1241 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
1242 if (chrminZ < terrainheight)
1243 {
1244 float depth = terrainheight - chrminZ;
1245 if (!flying)
1246 {
1247 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
1248 }
1249 else
1250 vec.Z = depth * PID_P * 50;
1251
1252 if (depth < 0.1f)
1253 {
1254 m_colliderGroundfilter++;
1255 if (m_colliderGroundfilter > 2)
1256 {
1257 m_iscolliding = true;
1258 m_colliderfilter = 2;
1259
1260 if (m_colliderGroundfilter > 10)
1261 {
1262 m_colliderGroundfilter = 10;
1263 m_freemove = false;
1264 }
1265
1266 m_iscollidingGround = true;
1267
1268 ContactPoint contact = new ContactPoint();
1269 contact.PenetrationDepth = depth;
1270 contact.Position.X = localpos.X;
1271 contact.Position.Y = localpos.Y;
1272 contact.Position.Z = terrainheight;
1273 contact.SurfaceNormal.X = 0.0f;
1274 contact.SurfaceNormal.Y = 0.0f;
1275 contact.SurfaceNormal.Z = -1f;
1276 contact.RelativeSpeed = -vel.Z;
1277 contact.CharacterFeet = true;
1278 AddCollisionEvent(0, contact);
1279
1280 vec.Z *= 0.5f;
1281 }
1282 }
1283
1284 else
1285 {
1286 m_colliderGroundfilter = 0;
1287 m_iscollidingGround = false;
1288 }
1289 }
1290 else
1291 {
1292 m_colliderGroundfilter = 0;
1293 m_iscollidingGround = false;
1294 }
1295
1296
1297 //******************************************
1298
1299 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
1300
1301 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
1302 if (!tviszero)
1303 m_freemove = false;
1304
1305 if (!m_freemove)
1306 {
1307
1308 // if velocity is zero, use position control; otherwise, velocity control
1309 if (tviszero && m_iscolliding)
1310 {
1311 // keep track of where we stopped. No more slippin' & slidin'
1312 if (!_zeroFlag)
1313 {
1314 _zeroFlag = true;
1315 _zeroPosition = localpos;
1316 }
1317 if (m_pidControllerActive)
1318 {
1319 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1320 // react to the physics scene by moving it's position.
1321 // Avatar to Avatar collisions
1322 // Prim to avatar collisions
1323
1324 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
1325 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
1326 if (flying)
1327 {
1328 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1329 }
1330 }
1331 //PidStatus = true;
1332 }
1333 else
1334 {
1335 m_pidControllerActive = true;
1336 _zeroFlag = false;
1337
1338 if (m_iscolliding)
1339 {
1340 if (!flying)
1341 {
1342 if (_target_velocity.Z > 0.0f)
1343 {
1344 // We're colliding with something and we're not flying but we're moving
1345 // This means we're walking or running. JUMPING
1346 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
1347 }
1348 // We're standing on something
1349 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
1350 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
1351 }
1352 else
1353 {
1354 // We're flying and colliding with something
1355 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
1356 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
1357 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1358 }
1359 }
1360 else // ie not colliding
1361 {
1362 if (flying) //(!m_iscolliding && flying)
1363 {
1364 // we're in mid air suspended
1365 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
1366 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
1367 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1368 }
1369
1370 else
1371 {
1372 // we're not colliding and we're not flying so that means we're falling!
1373 // m_iscolliding includes collisions with the ground.
1374
1375 // d.Vector3 pos = d.BodyGetPosition(Body);
1376 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
1377 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
1378 }
1379 }
1380 }
1381
1382 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
1383 {
1384 breakfactor = 0.16f * m_mass;
1385 vec.X -= breakfactor * vel.X;
1386 vec.Y -= breakfactor * vel.Y;
1387 vec.Z -= breakfactor * vel.Z;
1388 }
1389 }
1390 else
1391 {
1392 breakfactor = m_mass;
1393 vec.X -= breakfactor * vel.X;
1394 vec.Y -= breakfactor * vel.Y;
1395 if (flying)
1396 vec.Z -= breakfactor * vel.Z;
1397 else
1398 vec.Z -= .5f* m_mass * vel.Z;
1399 }
1400
1401 if (flying)
1402 {
1403 vec.Z -= _parent_scene.gravityz * m_mass;
1404
1405 //Added for auto fly height. Kitto Flora
1406 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1407
1408 if (localpos.Z < target_altitude)
1409 {
1410 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1411 }
1412 // end add Kitto Flora
1413 }
1414
1415 if (vec.IsFinite())
1416 {
1417 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1418 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1419 }
1420 else
1421 {
1422 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1423 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1424 defects.Add(this);
1425 // _parent_scene.RemoveCharacter(this);
1426 // destroy avatar capsule and related ODE data
1427 AvatarGeomAndBodyDestroy();
1428 return;
1429 }
1430
1431 // update our local ideia of position velocity and aceleration
1432 // _position = localpos;
1433 _position = localpos;
1434
1435 if (_zeroFlag)
1436 {
1437 _velocity = Vector3.Zero;
1438 _acceleration = Vector3.Zero;
1439 m_rotationalVelocity = Vector3.Zero;
1440 }
1441 else
1442 {
1443 Vector3 a =_velocity; // previus velocity
1444 SetSmooth(ref _velocity, ref vel, 2);
1445 a = (_velocity - a) * invtimeStep;
1446 SetSmooth(ref _acceleration, ref a, 2);
1447
1448 dtmp = d.BodyGetAngularVel(Body);
1449 m_rotationalVelocity.X = 0f;
1450 m_rotationalVelocity.Y = 0f;
1451 m_rotationalVelocity.Z = dtmp.Z;
1452 Math.Round(m_rotationalVelocity.Z,3);
1453 }
1454 ajustCollider();
1455 }
1456
1457 public void round(ref Vector3 v, int digits)
1458 {
1459 v.X = (float)Math.Round(v.X, digits);
1460 v.Y = (float)Math.Round(v.Y, digits);
1461 v.Z = (float)Math.Round(v.Z, digits);
1462 }
1463
1464 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1465 {
1466 dst.X = 0.1f * dst.X + 0.9f * value.X;
1467 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1468 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1469 }
1470
1471 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1472 {
1473 dst.X = 0.4f * dst.X + 0.6f * value.X;
1474 dst.X = (float)Math.Round(dst.X, rounddigits);
1475
1476 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1477 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1478
1479 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1480 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1481 }
1482
1483
1484 /// <summary>
1485 /// Updates the reported position and velocity.
1486 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1487 /// also outbounds checking
1488 /// copy and outbounds now done in move(..) at ode rate
1489 ///
1490 /// </summary>
1491 public void UpdatePositionAndVelocity()
1492 {
1493 return;
1494
1495// if (Body == IntPtr.Zero)
1496// return;
1497
1498 }
1499
1500 /// <summary>
1501 /// Cleanup the things we use in the scene.
1502 /// </summary>
1503 public void Destroy()
1504 {
1505 AddChange(changes.Remove, null);
1506 }
1507
1508 public override void CrossingFailure()
1509 {
1510 }
1511
1512 public override Vector3 PIDTarget { set { return; } }
1513 public override bool PIDActive { set { return; } }
1514 public override float PIDTau { set { return; } }
1515
1516 public override float PIDHoverHeight { set { return; } }
1517 public override bool PIDHoverActive { set { return; } }
1518 public override PIDHoverType PIDHoverType { set { return; } }
1519 public override float PIDHoverTau { set { return; } }
1520
1521 public override Quaternion APIDTarget { set { return; } }
1522
1523 public override bool APIDActive { set { return; } }
1524
1525 public override float APIDStrength { set { return; } }
1526
1527 public override float APIDDamping { set { return; } }
1528
1529
1530 public override void SubscribeEvents(int ms)
1531 {
1532 m_eventsubscription = ms;
1533 m_cureventsubscription = 0;
1534 if (CollisionEventsThisFrame == null)
1535 CollisionEventsThisFrame = new CollisionEventUpdate();
1536 SentEmptyCollisionsEvent = false;
1537 }
1538
1539 public override void UnSubscribeEvents()
1540 {
1541 if (CollisionEventsThisFrame != null)
1542 {
1543 lock (CollisionEventsThisFrame)
1544 {
1545 CollisionEventsThisFrame.Clear();
1546 CollisionEventsThisFrame = null;
1547 }
1548 }
1549 m_eventsubscription = 0;
1550 }
1551
1552 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1553 {
1554 if (CollisionEventsThisFrame == null)
1555 CollisionEventsThisFrame = new CollisionEventUpdate();
1556 lock (CollisionEventsThisFrame)
1557 {
1558 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1559 _parent_scene.AddCollisionEventReporting(this);
1560 }
1561 }
1562
1563 public void SendCollisions()
1564 {
1565 if (CollisionEventsThisFrame == null)
1566 return;
1567
1568 lock (CollisionEventsThisFrame)
1569 {
1570 if (m_cureventsubscription < m_eventsubscription)
1571 return;
1572
1573 m_cureventsubscription = 0;
1574
1575 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1576
1577 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1578 {
1579 base.SendCollisionUpdate(CollisionEventsThisFrame);
1580
1581 if (ncolisions == 0)
1582 {
1583 SentEmptyCollisionsEvent = true;
1584 _parent_scene.RemoveCollisionEventReporting(this);
1585 }
1586 else
1587 {
1588 SentEmptyCollisionsEvent = false;
1589 CollisionEventsThisFrame.Clear();
1590 }
1591 }
1592 }
1593 }
1594
1595 internal void AddCollisionFrameTime(int t)
1596 {
1597 // protect it from overflow crashing
1598 if (m_cureventsubscription < 50000)
1599 m_cureventsubscription += t;
1600 }
1601
1602 public override bool SubscribedEvents()
1603 {
1604 if (m_eventsubscription > 0)
1605 return true;
1606 return false;
1607 }
1608
1609 private void changePhysicsStatus(bool NewStatus)
1610 {
1611 if (NewStatus != m_isPhysical)
1612 {
1613 if (NewStatus)
1614 {
1615 AvatarGeomAndBodyDestroy();
1616
1617 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1618
1619 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1620 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1621 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1622 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1623 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1624 _parent_scene.AddCharacter(this);
1625 }
1626 else
1627 {
1628 _parent_scene.RemoveCollisionEventReporting(this);
1629 _parent_scene.RemoveCharacter(this);
1630 // destroy avatar capsule and related ODE data
1631 AvatarGeomAndBodyDestroy();
1632 }
1633 m_freemove = false;
1634 m_isPhysical = NewStatus;
1635 }
1636 }
1637
1638 private void changeAdd()
1639 {
1640 changePhysicsStatus(true);
1641 }
1642
1643 private void changeRemove()
1644 {
1645 changePhysicsStatus(false);
1646 }
1647
1648 private void changeShape(PrimitiveBaseShape arg)
1649 {
1650 }
1651
1652 private void changeAvatarSize(strAvatarSize st)
1653 {
1654 m_feetOffset = st.offset;
1655 changeSize(st.size);
1656 }
1657
1658 private void changeSize(Vector3 pSize)
1659 {
1660 if (pSize.IsFinite())
1661 {
1662 // for now only look to Z changes since viewers also don't change X and Y
1663 if (pSize.Z != m_size.Z)
1664 {
1665 AvatarGeomAndBodyDestroy();
1666
1667
1668 float oldsz = m_size.Z;
1669 m_size = pSize;
1670
1671
1672 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1673 _position.Z + (m_size.Z - oldsz) * 0.5f);
1674
1675 Velocity = Vector3.Zero;
1676
1677
1678 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1679 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1680 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1681 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1682 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1683 }
1684 m_freemove = false;
1685 m_collisionException = false;
1686 m_pidControllerActive = true;
1687 }
1688 else
1689 {
1690 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1691 }
1692 }
1693
1694 private void changePosition( Vector3 newPos)
1695 {
1696 if (Body != IntPtr.Zero)
1697 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1698 _position = newPos;
1699 m_freemove = false;
1700 m_pidControllerActive = true;
1701 }
1702
1703 private void changeOrientation(Quaternion newOri)
1704 {
1705 if (m_orientation != newOri)
1706 {
1707 m_orientation = newOri; // keep a copy for core use
1708 // but only use rotations around Z
1709
1710 m_orientation2D.W = newOri.W;
1711 m_orientation2D.Z = newOri.Z;
1712
1713 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1714 if (t > 0)
1715 {
1716 t = 1.0f / (float)Math.Sqrt(t);
1717 m_orientation2D.W *= t;
1718 m_orientation2D.Z *= t;
1719 }
1720 else
1721 {
1722 m_orientation2D.W = 1.0f;
1723 m_orientation2D.Z = 0f;
1724 }
1725 m_orientation2D.Y = 0f;
1726 m_orientation2D.X = 0f;
1727
1728 d.Quaternion myrot = new d.Quaternion();
1729 myrot.X = m_orientation2D.X;
1730 myrot.Y = m_orientation2D.Y;
1731 myrot.Z = m_orientation2D.Z;
1732 myrot.W = m_orientation2D.W;
1733 d.BodySetQuaternion(Body, ref myrot);
1734 }
1735 }
1736
1737 private void changeVelocity(Vector3 newVel)
1738 {
1739 m_pidControllerActive = true;
1740 m_freemove = false;
1741 _target_velocity = newVel;
1742 }
1743
1744 private void changeSetTorque(Vector3 newTorque)
1745 {
1746 }
1747
1748 private void changeAddForce(Vector3 newForce)
1749 {
1750 }
1751
1752 private void changeAddAngularForce(Vector3 arg)
1753 {
1754 }
1755
1756 private void changeAngularLock(Vector3 arg)
1757 {
1758 }
1759
1760 private void changeFloatOnWater(bool arg)
1761 {
1762 }
1763
1764 private void changeVolumedetetion(bool arg)
1765 {
1766 }
1767
1768 private void changeSelectedStatus(bool arg)
1769 {
1770 }
1771
1772 private void changeDisable(bool arg)
1773 {
1774 }
1775
1776 private void changeBuilding(bool arg)
1777 {
1778 }
1779
1780 private void setFreeMove()
1781 {
1782 m_pidControllerActive = true;
1783 _zeroFlag = false;
1784 _target_velocity = Vector3.Zero;
1785 m_freemove = true;
1786 m_colliderfilter = -2;
1787 m_colliderObjectfilter = -2;
1788 m_colliderGroundfilter = -2;
1789
1790 m_iscolliding = false;
1791 m_iscollidingGround = false;
1792 m_iscollidingObj = false;
1793
1794 CollisionEventsThisFrame.Clear();
1795 }
1796
1797 private void changeForce(Vector3 newForce)
1798 {
1799 setFreeMove();
1800
1801 if (Body != IntPtr.Zero)
1802 {
1803 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1804 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1805 }
1806 }
1807
1808 // for now momentum is actually velocity
1809 private void changeMomentum(Vector3 newmomentum)
1810 {
1811 _velocity = newmomentum;
1812 setFreeMove();
1813
1814 if (Body != IntPtr.Zero)
1815 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1816 ajustCollider();
1817 }
1818
1819 private void donullchange()
1820 {
1821 }
1822
1823 public bool DoAChange(changes what, object arg)
1824 {
1825 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1826 {
1827 return false;
1828 }
1829
1830 // nasty switch
1831 switch (what)
1832 {
1833 case changes.Add:
1834 changeAdd();
1835 break;
1836 case changes.Remove:
1837 changeRemove();
1838 break;
1839
1840 case changes.Position:
1841 changePosition((Vector3)arg);
1842 break;
1843
1844 case changes.Orientation:
1845 changeOrientation((Quaternion)arg);
1846 break;
1847
1848 case changes.PosOffset:
1849 donullchange();
1850 break;
1851
1852 case changes.OriOffset:
1853 donullchange();
1854 break;
1855
1856 case changes.Velocity:
1857 changeVelocity((Vector3)arg);
1858 break;
1859
1860 // case changes.Acceleration:
1861 // changeacceleration((Vector3)arg);
1862 // break;
1863 // case changes.AngVelocity:
1864 // changeangvelocity((Vector3)arg);
1865 // break;
1866
1867 case changes.Force:
1868 changeForce((Vector3)arg);
1869 break;
1870
1871 case changes.Torque:
1872 changeSetTorque((Vector3)arg);
1873 break;
1874
1875 case changes.AddForce:
1876 changeAddForce((Vector3)arg);
1877 break;
1878
1879 case changes.AddAngForce:
1880 changeAddAngularForce((Vector3)arg);
1881 break;
1882
1883 case changes.AngLock:
1884 changeAngularLock((Vector3)arg);
1885 break;
1886
1887 case changes.Size:
1888 changeSize((Vector3)arg);
1889 break;
1890
1891 case changes.AvatarSize:
1892 changeAvatarSize((strAvatarSize)arg);
1893 break;
1894
1895 case changes.Momentum:
1896 changeMomentum((Vector3)arg);
1897 break;
1898/* not in use for now
1899 case changes.Shape:
1900 changeShape((PrimitiveBaseShape)arg);
1901 break;
1902
1903 case changes.CollidesWater:
1904 changeFloatOnWater((bool)arg);
1905 break;
1906
1907 case changes.VolumeDtc:
1908 changeVolumedetetion((bool)arg);
1909 break;
1910
1911 case changes.Physical:
1912 changePhysicsStatus((bool)arg);
1913 break;
1914
1915 case changes.Selected:
1916 changeSelectedStatus((bool)arg);
1917 break;
1918
1919 case changes.disabled:
1920 changeDisable((bool)arg);
1921 break;
1922
1923 case changes.building:
1924 changeBuilding((bool)arg);
1925 break;
1926*/
1927 case changes.Null:
1928 donullchange();
1929 break;
1930
1931 default:
1932 donullchange();
1933 break;
1934 }
1935 return false;
1936 }
1937
1938 public void AddChange(changes what, object arg)
1939 {
1940 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1941 }
1942
1943 private struct strAvatarSize
1944 {
1945 public Vector3 size;
1946 public float offset;
1947 }
1948
1949 }
1950}
generated by cgit v1.1 at 2024-11-18 22:38:36 +0000