aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1988
1 files changed, 1988 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..e912997
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1988 @@
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 {
1049 offset.Z = contact.pos.Z - _position.Z;
1050 offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
1051 offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
1052 offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
1053
1054 if (reverse)
1055 {
1056 offset.X *= -contact.normal.X;
1057 offset.Y *= -contact.normal.Y;
1058 offset.Z *= -contact.normal.Z;
1059 }
1060 else
1061 {
1062 offset.X *= contact.normal.X;
1063 offset.Y *= contact.normal.Y;
1064 offset.Z *= contact.normal.Z;
1065 }
1066
1067 offset.X += contact.pos.X;
1068 offset.Y += contact.pos.Y;
1069 offset.Z += contact.pos.Z;
1070 _position = offset;
1071 return true;
1072 }
1073 else
1074 offset.Z = contact.normal.Z;
1075
1076 offset.Normalize();
1077
1078 /*
1079 if (reverse)
1080 {
1081 contact.normal.X = offset.X;
1082 contact.normal.Y = offset.Y;
1083 contact.normal.Z = offset.Z;
1084 }
1085 else
1086 {
1087 contact.normal.X = -offset.X;
1088 contact.normal.Y = -offset.Y;
1089 contact.normal.Z = -offset.Z;
1090 }
1091 */
1092 //_position.Z = offset.Z;
1093 return true;
1094 }
1095
1096 else if (me == feetbox)
1097 {
1098 float h = contact.pos.Z - _position.Z;
1099
1100 // Only do this if the normal is sufficiently pointing in the 'up' direction
1101 if (Math.Abs(contact.normal.Z) > 0.95f)
1102 {
1103 // We Only want to do this if we're sunk into the object a bit and we're stuck and we're trying to move and feetcollision is false
1104 if ((contact.depth > 0.0010f && _velocity.X == 0f && _velocity.Y == 0 && _velocity.Z == 0)
1105 && (_target_velocity.X > 0 || _target_velocity.Y > 0 || _target_velocity.Z > 0)
1106 && (!feetcollision) )
1107 {
1108 m_collisionException = true; // Stop looping, do this only once not X times Contacts
1109 _position.Z += contact.depth + 0.01f; // Move us Up the amount that we sank in, and add 0.01 meters to gently lift avatar up.
1110
1111 return true;
1112 }
1113
1114 if (contact.normal.Z > 0)
1115 contact.normal.Z = 1.0f;
1116 else
1117 contact.normal.Z = -1.0f;
1118 contact.normal.X = 0.0f;
1119 contact.normal.Y = 0.0f;
1120 feetcollision = true;
1121 if (h < boneOff)
1122 IsColliding = true;
1123 return true;
1124 }
1125
1126 offset.Z = h - feetOff; // distance from top of feetbox
1127
1128 if (offset.Z > 0)
1129 return false;
1130
1131 if (offset.Z > -0.01)
1132 {
1133 offset.X = 0;
1134 offset.Y = 0;
1135 offset.Z = -1.0f;
1136 }
1137 else
1138 {
1139 offset.Normalize();
1140 }
1141
1142 if (reverse)
1143 {
1144 contact.normal.X = offset.X;
1145 contact.normal.Y = offset.Y;
1146 contact.normal.Z = offset.Z;
1147 }
1148 else
1149 {
1150 contact.normal.X = -offset.X;
1151 contact.normal.Y = -offset.Y;
1152 contact.normal.Z = -offset.Z;
1153 }
1154 feetcollision = true;
1155 if (h < boneOff)
1156 IsColliding = true;
1157 }
1158 else
1159 return false;
1160
1161 return true;
1162 }
1163
1164 /// <summary>
1165 /// Called from Simulate
1166 /// This is the avatar's movement control + PID Controller
1167 /// </summary>
1168 /// <param name="timeStep"></param>
1169 public void Move(List<OdeCharacter> defects)
1170 {
1171 if (Body == IntPtr.Zero)
1172 return;
1173
1174 if (m_collisionException)
1175 {
1176 d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
1177 d.BodySetLinearVel(Body, 0, 0, 0);
1178
1179 float v = _velocity.Length();
1180 if (v != 0)
1181 {
1182 v = 5.0f / v;
1183 _velocity = _velocity * v;
1184 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1185 }
1186 ajustCollider();
1187 m_collisionException = false;
1188 return;
1189 }
1190
1191 d.Vector3 dtmp = d.BodyGetPosition(Body);
1192 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1193
1194 // the Amotor still lets avatar rotation to drift during colisions
1195 // so force it back to identity
1196
1197 d.Quaternion qtmp;
1198 qtmp.W = m_orientation2D.W;
1199 qtmp.X = m_orientation2D.X;
1200 qtmp.Y = m_orientation2D.Y;
1201 qtmp.Z = m_orientation2D.Z;
1202 d.BodySetQuaternion(Body, ref qtmp);
1203
1204 if (m_pidControllerActive == false)
1205 {
1206 _zeroPosition = localpos;
1207 }
1208
1209 if (!localpos.IsFinite())
1210 {
1211 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1212 defects.Add(this);
1213 // _parent_scene.RemoveCharacter(this);
1214
1215 // destroy avatar capsule and related ODE data
1216 AvatarGeomAndBodyDestroy();
1217 return;
1218 }
1219
1220 // check outbounds forcing to be in world
1221 bool fixbody = false;
1222 if (localpos.X < 0.0f)
1223 {
1224 fixbody = true;
1225 localpos.X = 0.1f;
1226 }
1227 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
1228 {
1229 fixbody = true;
1230 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
1231 }
1232 if (localpos.Y < 0.0f)
1233 {
1234 fixbody = true;
1235 localpos.Y = 0.1f;
1236 }
1237 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
1238 {
1239 fixbody = true;
1240 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
1241 }
1242 if (fixbody)
1243 {
1244 m_freemove = false;
1245 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
1246 }
1247
1248 float breakfactor;
1249
1250 Vector3 vec = Vector3.Zero;
1251 dtmp = d.BodyGetLinearVel(Body);
1252 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1253 float velLengthSquared = vel.LengthSquared();
1254
1255 float movementdivisor = 1f;
1256 //Ubit change divisions into multiplications below
1257 if (!m_alwaysRun)
1258 movementdivisor = 1 / walkDivisor;
1259 else
1260 movementdivisor = 1 / runDivisor;
1261
1262 //******************************************
1263 // colide with land
1264
1265 d.AABB aabb;
1266 d.GeomGetAABB(feetbox, out aabb);
1267 float chrminZ = aabb.MinZ; ; // move up a bit
1268 Vector3 posch = localpos;
1269
1270 float ftmp;
1271
1272 if (flying)
1273 {
1274 ftmp = timeStep;
1275 posch.X += vel.X * ftmp;
1276 posch.Y += vel.Y * ftmp;
1277 }
1278
1279 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
1280 if (chrminZ < terrainheight)
1281 {
1282 float depth = terrainheight - chrminZ;
1283 if (!flying)
1284 {
1285 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
1286 }
1287 else
1288 vec.Z = depth * PID_P * 50;
1289
1290 if (depth < 0.1f)
1291 {
1292 m_colliderGroundfilter++;
1293 if (m_colliderGroundfilter > 2)
1294 {
1295 m_iscolliding = true;
1296 m_colliderfilter = 2;
1297
1298 if (m_colliderGroundfilter > 10)
1299 {
1300 m_colliderGroundfilter = 10;
1301 m_freemove = false;
1302 }
1303
1304 m_iscollidingGround = true;
1305
1306 ContactPoint contact = new ContactPoint();
1307 contact.PenetrationDepth = depth;
1308 contact.Position.X = localpos.X;
1309 contact.Position.Y = localpos.Y;
1310 contact.Position.Z = terrainheight;
1311 contact.SurfaceNormal.X = 0.0f;
1312 contact.SurfaceNormal.Y = 0.0f;
1313 contact.SurfaceNormal.Z = -1f;
1314 contact.RelativeSpeed = -vel.Z;
1315 contact.CharacterFeet = true;
1316 AddCollisionEvent(0, contact);
1317
1318 vec.Z *= 0.5f;
1319 }
1320 }
1321
1322 else
1323 {
1324 m_colliderGroundfilter = 0;
1325 m_iscollidingGround = false;
1326 }
1327 }
1328 else
1329 {
1330 m_colliderGroundfilter = 0;
1331 m_iscollidingGround = false;
1332 }
1333
1334
1335 //******************************************
1336
1337 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
1338
1339 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
1340 if (!tviszero)
1341 m_freemove = false;
1342
1343 if (!m_freemove)
1344 {
1345
1346 // if velocity is zero, use position control; otherwise, velocity control
1347 if (tviszero && m_iscolliding)
1348 {
1349 // keep track of where we stopped. No more slippin' & slidin'
1350 if (!_zeroFlag)
1351 {
1352 _zeroFlag = true;
1353 _zeroPosition = localpos;
1354 }
1355 if (m_pidControllerActive)
1356 {
1357 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1358 // react to the physics scene by moving it's position.
1359 // Avatar to Avatar collisions
1360 // Prim to avatar collisions
1361
1362 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
1363 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
1364 if (flying)
1365 {
1366 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1367 }
1368 }
1369 //PidStatus = true;
1370 }
1371 else
1372 {
1373 m_pidControllerActive = true;
1374 _zeroFlag = false;
1375
1376 if (m_iscolliding)
1377 {
1378 if (!flying)
1379 {
1380 if (_target_velocity.Z > 0.0f)
1381 {
1382 // We're colliding with something and we're not flying but we're moving
1383 // This means we're walking or running. JUMPING
1384 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
1385 }
1386 // We're standing on something
1387 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
1388 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
1389 }
1390 else
1391 {
1392 // We're flying and colliding with something
1393 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
1394 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
1395 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1396 }
1397 }
1398 else // ie not colliding
1399 {
1400 if (flying) //(!m_iscolliding && flying)
1401 {
1402 // we're in mid air suspended
1403 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
1404 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
1405 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1406 }
1407
1408 else
1409 {
1410 // we're not colliding and we're not flying so that means we're falling!
1411 // m_iscolliding includes collisions with the ground.
1412
1413 // d.Vector3 pos = d.BodyGetPosition(Body);
1414 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
1415 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
1416 }
1417 }
1418 }
1419
1420 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
1421 {
1422 breakfactor = 0.16f * m_mass;
1423 vec.X -= breakfactor * vel.X;
1424 vec.Y -= breakfactor * vel.Y;
1425 vec.Z -= breakfactor * vel.Z;
1426 }
1427 }
1428 else
1429 {
1430 breakfactor = m_mass;
1431 vec.X -= breakfactor * vel.X;
1432 vec.Y -= breakfactor * vel.Y;
1433 if (flying)
1434 vec.Z -= breakfactor * vel.Z;
1435 else
1436 vec.Z -= .5f* m_mass * vel.Z;
1437 }
1438
1439 if (flying)
1440 {
1441 vec.Z -= _parent_scene.gravityz * m_mass;
1442
1443 //Added for auto fly height. Kitto Flora
1444 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1445
1446 if (localpos.Z < target_altitude)
1447 {
1448 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1449 }
1450 // end add Kitto Flora
1451 }
1452
1453 if (vec.IsFinite())
1454 {
1455 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1456 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1457 }
1458 else
1459 {
1460 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1461 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1462 defects.Add(this);
1463 // _parent_scene.RemoveCharacter(this);
1464 // destroy avatar capsule and related ODE data
1465 AvatarGeomAndBodyDestroy();
1466 return;
1467 }
1468
1469 // update our local ideia of position velocity and aceleration
1470 // _position = localpos;
1471 _position = localpos;
1472
1473 if (_zeroFlag)
1474 {
1475 _velocity = Vector3.Zero;
1476 _acceleration = Vector3.Zero;
1477 m_rotationalVelocity = Vector3.Zero;
1478 }
1479 else
1480 {
1481 Vector3 a =_velocity; // previus velocity
1482 SetSmooth(ref _velocity, ref vel, 2);
1483 a = (_velocity - a) * invtimeStep;
1484 SetSmooth(ref _acceleration, ref a, 2);
1485
1486 dtmp = d.BodyGetAngularVel(Body);
1487 m_rotationalVelocity.X = 0f;
1488 m_rotationalVelocity.Y = 0f;
1489 m_rotationalVelocity.Z = dtmp.Z;
1490 Math.Round(m_rotationalVelocity.Z,3);
1491 }
1492 ajustCollider();
1493 }
1494
1495 public void round(ref Vector3 v, int digits)
1496 {
1497 v.X = (float)Math.Round(v.X, digits);
1498 v.Y = (float)Math.Round(v.Y, digits);
1499 v.Z = (float)Math.Round(v.Z, digits);
1500 }
1501
1502 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1503 {
1504 dst.X = 0.1f * dst.X + 0.9f * value.X;
1505 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1506 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1507 }
1508
1509 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1510 {
1511 dst.X = 0.4f * dst.X + 0.6f * value.X;
1512 dst.X = (float)Math.Round(dst.X, rounddigits);
1513
1514 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1515 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1516
1517 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1518 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1519 }
1520
1521
1522 /// <summary>
1523 /// Updates the reported position and velocity.
1524 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1525 /// also outbounds checking
1526 /// copy and outbounds now done in move(..) at ode rate
1527 ///
1528 /// </summary>
1529 public void UpdatePositionAndVelocity()
1530 {
1531 return;
1532
1533// if (Body == IntPtr.Zero)
1534// return;
1535
1536 }
1537
1538 /// <summary>
1539 /// Cleanup the things we use in the scene.
1540 /// </summary>
1541 public void Destroy()
1542 {
1543 AddChange(changes.Remove, null);
1544 }
1545
1546 public override void CrossingFailure()
1547 {
1548 }
1549
1550 public override Vector3 PIDTarget { set { return; } }
1551 public override bool PIDActive { set { return; } }
1552 public override float PIDTau { set { return; } }
1553
1554 public override float PIDHoverHeight { set { return; } }
1555 public override bool PIDHoverActive { set { return; } }
1556 public override PIDHoverType PIDHoverType { set { return; } }
1557 public override float PIDHoverTau { set { return; } }
1558
1559 public override Quaternion APIDTarget { set { return; } }
1560
1561 public override bool APIDActive { set { return; } }
1562
1563 public override float APIDStrength { set { return; } }
1564
1565 public override float APIDDamping { set { return; } }
1566
1567
1568 public override void SubscribeEvents(int ms)
1569 {
1570 m_eventsubscription = ms;
1571 m_cureventsubscription = 0;
1572 if (CollisionEventsThisFrame == null)
1573 CollisionEventsThisFrame = new CollisionEventUpdate();
1574 SentEmptyCollisionsEvent = false;
1575 }
1576
1577 public override void UnSubscribeEvents()
1578 {
1579 if (CollisionEventsThisFrame != null)
1580 {
1581 lock (CollisionEventsThisFrame)
1582 {
1583 CollisionEventsThisFrame.Clear();
1584 CollisionEventsThisFrame = null;
1585 }
1586 }
1587 m_eventsubscription = 0;
1588 }
1589
1590 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1591 {
1592 if (CollisionEventsThisFrame == null)
1593 CollisionEventsThisFrame = new CollisionEventUpdate();
1594 lock (CollisionEventsThisFrame)
1595 {
1596 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1597 _parent_scene.AddCollisionEventReporting(this);
1598 }
1599 }
1600
1601 public void SendCollisions()
1602 {
1603 if (CollisionEventsThisFrame == null)
1604 return;
1605
1606 lock (CollisionEventsThisFrame)
1607 {
1608 if (m_cureventsubscription < m_eventsubscription)
1609 return;
1610
1611 m_cureventsubscription = 0;
1612
1613 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1614
1615 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1616 {
1617 base.SendCollisionUpdate(CollisionEventsThisFrame);
1618
1619 if (ncolisions == 0)
1620 {
1621 SentEmptyCollisionsEvent = true;
1622 _parent_scene.RemoveCollisionEventReporting(this);
1623 }
1624 else
1625 {
1626 SentEmptyCollisionsEvent = false;
1627 CollisionEventsThisFrame.Clear();
1628 }
1629 }
1630 }
1631 }
1632
1633 internal void AddCollisionFrameTime(int t)
1634 {
1635 // protect it from overflow crashing
1636 if (m_cureventsubscription < 50000)
1637 m_cureventsubscription += t;
1638 }
1639
1640 public override bool SubscribedEvents()
1641 {
1642 if (m_eventsubscription > 0)
1643 return true;
1644 return false;
1645 }
1646
1647 private void changePhysicsStatus(bool NewStatus)
1648 {
1649 if (NewStatus != m_isPhysical)
1650 {
1651 if (NewStatus)
1652 {
1653 AvatarGeomAndBodyDestroy();
1654
1655 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1656
1657 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1658 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1659 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1660 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1661 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1662 _parent_scene.AddCharacter(this);
1663 }
1664 else
1665 {
1666 _parent_scene.RemoveCollisionEventReporting(this);
1667 _parent_scene.RemoveCharacter(this);
1668 // destroy avatar capsule and related ODE data
1669 AvatarGeomAndBodyDestroy();
1670 }
1671 m_freemove = false;
1672 m_isPhysical = NewStatus;
1673 }
1674 }
1675
1676 private void changeAdd()
1677 {
1678 changePhysicsStatus(true);
1679 }
1680
1681 private void changeRemove()
1682 {
1683 changePhysicsStatus(false);
1684 }
1685
1686 private void changeShape(PrimitiveBaseShape arg)
1687 {
1688 }
1689
1690 private void changeAvatarSize(strAvatarSize st)
1691 {
1692 m_feetOffset = st.offset;
1693 changeSize(st.size);
1694 }
1695
1696 private void changeSize(Vector3 pSize)
1697 {
1698 if (pSize.IsFinite())
1699 {
1700 // for now only look to Z changes since viewers also don't change X and Y
1701 if (pSize.Z != m_size.Z)
1702 {
1703 AvatarGeomAndBodyDestroy();
1704
1705
1706 float oldsz = m_size.Z;
1707 m_size = pSize;
1708
1709
1710 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1711 _position.Z + (m_size.Z - oldsz) * 0.5f);
1712
1713 Velocity = Vector3.Zero;
1714
1715
1716 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1717 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1718 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1719 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1720 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1721 }
1722 m_freemove = false;
1723 m_collisionException = false;
1724 m_pidControllerActive = true;
1725 }
1726 else
1727 {
1728 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1729 }
1730 }
1731
1732 private void changePosition( Vector3 newPos)
1733 {
1734 if (Body != IntPtr.Zero)
1735 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1736 _position = newPos;
1737 m_freemove = false;
1738 m_pidControllerActive = true;
1739 }
1740
1741 private void changeOrientation(Quaternion newOri)
1742 {
1743 if (m_orientation != newOri)
1744 {
1745 m_orientation = newOri; // keep a copy for core use
1746 // but only use rotations around Z
1747
1748 m_orientation2D.W = newOri.W;
1749 m_orientation2D.Z = newOri.Z;
1750
1751 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1752 if (t > 0)
1753 {
1754 t = 1.0f / (float)Math.Sqrt(t);
1755 m_orientation2D.W *= t;
1756 m_orientation2D.Z *= t;
1757 }
1758 else
1759 {
1760 m_orientation2D.W = 1.0f;
1761 m_orientation2D.Z = 0f;
1762 }
1763 m_orientation2D.Y = 0f;
1764 m_orientation2D.X = 0f;
1765
1766 d.Quaternion myrot = new d.Quaternion();
1767 myrot.X = m_orientation2D.X;
1768 myrot.Y = m_orientation2D.Y;
1769 myrot.Z = m_orientation2D.Z;
1770 myrot.W = m_orientation2D.W;
1771 d.BodySetQuaternion(Body, ref myrot);
1772 }
1773 }
1774
1775 private void changeVelocity(Vector3 newVel)
1776 {
1777 m_pidControllerActive = true;
1778 m_freemove = false;
1779 _target_velocity = newVel;
1780 }
1781
1782 private void changeSetTorque(Vector3 newTorque)
1783 {
1784 }
1785
1786 private void changeAddForce(Vector3 newForce)
1787 {
1788 }
1789
1790 private void changeAddAngularForce(Vector3 arg)
1791 {
1792 }
1793
1794 private void changeAngularLock(Vector3 arg)
1795 {
1796 }
1797
1798 private void changeFloatOnWater(bool arg)
1799 {
1800 }
1801
1802 private void changeVolumedetetion(bool arg)
1803 {
1804 }
1805
1806 private void changeSelectedStatus(bool arg)
1807 {
1808 }
1809
1810 private void changeDisable(bool arg)
1811 {
1812 }
1813
1814 private void changeBuilding(bool arg)
1815 {
1816 }
1817
1818 private void setFreeMove()
1819 {
1820 m_pidControllerActive = true;
1821 _zeroFlag = false;
1822 _target_velocity = Vector3.Zero;
1823 m_freemove = true;
1824 m_colliderfilter = -2;
1825 m_colliderObjectfilter = -2;
1826 m_colliderGroundfilter = -2;
1827
1828 m_iscolliding = false;
1829 m_iscollidingGround = false;
1830 m_iscollidingObj = false;
1831
1832 CollisionEventsThisFrame.Clear();
1833 }
1834
1835 private void changeForce(Vector3 newForce)
1836 {
1837 setFreeMove();
1838
1839 if (Body != IntPtr.Zero)
1840 {
1841 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1842 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1843 }
1844 }
1845
1846 // for now momentum is actually velocity
1847 private void changeMomentum(Vector3 newmomentum)
1848 {
1849 _velocity = newmomentum;
1850 setFreeMove();
1851
1852 if (Body != IntPtr.Zero)
1853 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1854 ajustCollider();
1855 }
1856
1857 private void donullchange()
1858 {
1859 }
1860
1861 public bool DoAChange(changes what, object arg)
1862 {
1863 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1864 {
1865 return false;
1866 }
1867
1868 // nasty switch
1869 switch (what)
1870 {
1871 case changes.Add:
1872 changeAdd();
1873 break;
1874 case changes.Remove:
1875 changeRemove();
1876 break;
1877
1878 case changes.Position:
1879 changePosition((Vector3)arg);
1880 break;
1881
1882 case changes.Orientation:
1883 changeOrientation((Quaternion)arg);
1884 break;
1885
1886 case changes.PosOffset:
1887 donullchange();
1888 break;
1889
1890 case changes.OriOffset:
1891 donullchange();
1892 break;
1893
1894 case changes.Velocity:
1895 changeVelocity((Vector3)arg);
1896 break;
1897
1898 // case changes.Acceleration:
1899 // changeacceleration((Vector3)arg);
1900 // break;
1901 // case changes.AngVelocity:
1902 // changeangvelocity((Vector3)arg);
1903 // break;
1904
1905 case changes.Force:
1906 changeForce((Vector3)arg);
1907 break;
1908
1909 case changes.Torque:
1910 changeSetTorque((Vector3)arg);
1911 break;
1912
1913 case changes.AddForce:
1914 changeAddForce((Vector3)arg);
1915 break;
1916
1917 case changes.AddAngForce:
1918 changeAddAngularForce((Vector3)arg);
1919 break;
1920
1921 case changes.AngLock:
1922 changeAngularLock((Vector3)arg);
1923 break;
1924
1925 case changes.Size:
1926 changeSize((Vector3)arg);
1927 break;
1928
1929 case changes.AvatarSize:
1930 changeAvatarSize((strAvatarSize)arg);
1931 break;
1932
1933 case changes.Momentum:
1934 changeMomentum((Vector3)arg);
1935 break;
1936/* not in use for now
1937 case changes.Shape:
1938 changeShape((PrimitiveBaseShape)arg);
1939 break;
1940
1941 case changes.CollidesWater:
1942 changeFloatOnWater((bool)arg);
1943 break;
1944
1945 case changes.VolumeDtc:
1946 changeVolumedetetion((bool)arg);
1947 break;
1948
1949 case changes.Physical:
1950 changePhysicsStatus((bool)arg);
1951 break;
1952
1953 case changes.Selected:
1954 changeSelectedStatus((bool)arg);
1955 break;
1956
1957 case changes.disabled:
1958 changeDisable((bool)arg);
1959 break;
1960
1961 case changes.building:
1962 changeBuilding((bool)arg);
1963 break;
1964*/
1965 case changes.Null:
1966 donullchange();
1967 break;
1968
1969 default:
1970 donullchange();
1971 break;
1972 }
1973 return false;
1974 }
1975
1976 public void AddChange(changes what, object arg)
1977 {
1978 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1979 }
1980
1981 private struct strAvatarSize
1982 {
1983 public Vector3 size;
1984 public float offset;
1985 }
1986
1987 }
1988}
generated by cgit v1.1 at 2024-12-20 04:21:35 +0000