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.cs1928
1 files changed, 1928 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..bb04ea7
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1928 @@
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.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z);
719 off.X = 2.0f * (m_size.X + Math.Abs(off.X));
720 off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y));
721 off.Z = m_size.Z + 2.0f * Math.Abs(off.Z);
722 d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z);
723
724 d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories);
725 d.GeomSetCollideBits(bbox, (uint)m_collisionFlags);
726 d.GeomSetCategoryBits(topbox, 0);
727 d.GeomSetCollideBits(topbox, 0);
728 d.GeomSetCategoryBits(midbox, 0);
729 d.GeomSetCollideBits(midbox, 0);
730 d.GeomSetCategoryBits(feetbox, 0);
731 d.GeomSetCollideBits(feetbox, 0);
732 }
733 else
734 {
735 d.GeomSetCategoryBits(bbox, 0);
736 d.GeomSetCollideBits(bbox, 0);
737 d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories);
738 d.GeomSetCollideBits(topbox, (uint)m_collisionFlags);
739 d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories);
740 d.GeomSetCollideBits(midbox, (uint)m_collisionFlags);
741 d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories);
742 d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags);
743 }
744 }
745 }
746
747 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
748 {
749 // sizes one day should came from visual parameters
750 float sx = m_size.X;
751 float sy = m_size.Y;
752 float sz = m_size.Z;
753
754 float topsx = sx * 0.9f;
755 float midsx = sx;
756 float feetsx = sx * feetScale;
757 float bonesx = sx * 0.2f;
758
759 float topsy = sy * 0.4f;
760 float midsy = sy;
761 float feetsy = sy * feetScale * 0.8f;
762 float bonesy = feetsy * 0.2f;
763
764 float topsz = sz * 0.15f;
765 float feetsz = sz * 0.45f;
766 if (feetsz > 0.6f)
767 feetsz = 0.6f;
768
769 float midsz = sz - topsz - feetsz;
770 float bonesz = sz;
771
772 float bot = -sz * 0.5f + m_feetOffset;
773
774 boneOff = bot + 0.3f;
775
776 float feetz = bot + feetsz * 0.5f;
777 bot += feetsz;
778
779 feetOff = bot;
780 feetSZ = feetsz;
781
782 float midz = bot + midsz * 0.5f;
783 bot += midsz;
784 float topz = bot + topsz * 0.5f;
785
786 _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
787
788 collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
789 d.HashSpaceSetLevels(collider, -4, 3);
790 d.SpaceSetSublevel(collider, 3);
791 d.SpaceSetCleanup(collider, false);
792 d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
793 d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
794
795 feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz);
796 midbox = d.CreateBox(collider, midsx, midsy, midsz);
797 topbox = d.CreateBox(collider, topsx, topsy, topsz);
798 bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z);
799
800 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
801
802 d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
803
804 PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
805 PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
806
807 Body = d.BodyCreate(_parent_scene.world);
808
809 _zeroFlag = false;
810 m_collisionException = false;
811 m_pidControllerActive = true;
812 m_freemove = false;
813
814 _velocity = Vector3.Zero;
815 m_lastVelocitySqr = 0;
816
817 d.BodySetAutoDisableFlag(Body, false);
818 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
819
820 _position.X = npositionX;
821 _position.Y = npositionY;
822 _position.Z = npositionZ;
823
824 d.BodySetMass(Body, ref ShellMass);
825 d.GeomSetBody(feetbox, Body);
826 d.GeomSetBody(midbox, Body);
827 d.GeomSetBody(topbox, Body);
828 d.GeomSetBody(bbox, Body);
829
830 d.GeomSetOffsetPosition(feetbox, 0, 0, feetz);
831 d.GeomSetOffsetPosition(midbox, 0, 0, midz);
832 d.GeomSetOffsetPosition(topbox, 0, 0, topz);
833
834 ajustCollider();
835
836
837 // The purpose of the AMotor here is to keep the avatar's physical
838 // surrogate from rotating while moving
839 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
840 d.JointAttach(Amotor, Body, IntPtr.Zero);
841
842 d.JointSetAMotorMode(Amotor, 0);
843 d.JointSetAMotorNumAxes(Amotor, 3);
844 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
845 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
846 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
847
848 d.JointSetAMotorAngle(Amotor, 0, 0);
849 d.JointSetAMotorAngle(Amotor, 1, 0);
850 d.JointSetAMotorAngle(Amotor, 2, 0);
851
852 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
853 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
854 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
855 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
856 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
857 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
858
859 // These lowstops and high stops are effectively (no wiggle room)
860 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
861 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
862 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
863 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
864 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
865 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
866
867 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
868 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
869 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
870
871 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
872 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
873 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
874 }
875
876 /// <summary>
877 /// Destroys the avatar body and geom
878
879 private void AvatarGeomAndBodyDestroy()
880 {
881 // Kill the Amotor
882 if (Amotor != IntPtr.Zero)
883 {
884 d.JointDestroy(Amotor);
885 Amotor = IntPtr.Zero;
886 }
887
888 if (Body != IntPtr.Zero)
889 {
890 //kill the body
891 d.BodyDestroy(Body);
892 Body = IntPtr.Zero;
893 }
894
895 //kill the Geoms
896 if (topbox != IntPtr.Zero)
897 {
898 _parent_scene.actor_name_map.Remove(topbox);
899 _parent_scene.waitForSpaceUnlock(collider);
900 d.GeomDestroy(topbox);
901 topbox = IntPtr.Zero;
902 }
903 if (midbox != IntPtr.Zero)
904 {
905 _parent_scene.actor_name_map.Remove(midbox);
906 _parent_scene.waitForSpaceUnlock(collider);
907 d.GeomDestroy(midbox);
908 midbox = IntPtr.Zero;
909 }
910 if (feetbox != IntPtr.Zero)
911 {
912 _parent_scene.actor_name_map.Remove(feetbox);
913 _parent_scene.waitForSpaceUnlock(collider);
914 d.GeomDestroy(feetbox);
915 feetbox = IntPtr.Zero;
916 }
917
918 if (bbox != IntPtr.Zero)
919 {
920 _parent_scene.actor_name_map.Remove(bbox);
921 _parent_scene.waitForSpaceUnlock(collider);
922 d.GeomDestroy(bbox);
923 bbox = IntPtr.Zero;
924 }
925
926 if (collider != IntPtr.Zero)
927 {
928 d.SpaceDestroy(collider);
929 collider = IntPtr.Zero;
930 }
931
932 }
933
934 //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
935 public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
936 {
937 float sin = 2.0f * rot.Z * rot.W;
938 float cos = rot.W * rot.W - rot.Z * rot.Z;
939 float tx = x;
940
941 x = tx * cos - y * sin;
942 y = tx * sin + y * cos;
943 }
944 public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
945 {
946 float tx = x;
947 x = tx * cos - y * sin;
948 y = tx * sin + y * cos;
949 }
950 public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
951 {
952 float tx = x;
953 x = tx * cos + y * sin;
954 y = -tx * sin + y * cos;
955 }
956
957 public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
958 {
959 float sin = - 2.0f * rot.Z * rot.W;
960 float cos = rot.W * rot.W - rot.Z * rot.Z;
961 float tx = x;
962
963 x = tx * cos - y * sin;
964 y = tx * sin + y * cos;
965 }
966
967
968 public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision)
969 {
970 feetcollision = false;
971 if (m_collisionException)
972 return false;
973
974 Vector3 offset;
975
976 if (me == bbox) // if moving fast
977 {
978 // force a full inelastic collision
979 m_collisionException = true;
980
981 offset = m_size * m_orientation2D;
982
983 offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth;
984 offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth;
985 offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth;
986
987 if (reverse)
988 {
989 offset.X *= -contact.normal.X;
990 offset.Y *= -contact.normal.Y;
991 offset.Z *= -contact.normal.Z;
992 }
993 else
994 {
995 offset.X *= contact.normal.X;
996 offset.Y *= contact.normal.Y;
997 offset.Z *= contact.normal.Z;
998 }
999
1000 offset.X += contact.pos.X;
1001 offset.Y += contact.pos.Y;
1002 offset.Z += contact.pos.Z;
1003
1004 _position = offset;
1005 return false;
1006 }
1007
1008 offset.X = contact.pos.X - _position.X;
1009 offset.Y = contact.pos.Y - _position.Y;
1010
1011 if (me == topbox)
1012 {
1013 offset.Z = contact.pos.Z - _position.Z;
1014
1015 offset.Normalize();
1016
1017 if (reverse)
1018 {
1019 contact.normal.X = offset.X;
1020 contact.normal.Y = offset.Y;
1021 contact.normal.Z = offset.Z;
1022 }
1023 else
1024 {
1025 contact.normal.X = -offset.X;
1026 contact.normal.Y = -offset.Y;
1027 contact.normal.Z = -offset.Z;
1028 }
1029 return true;
1030 }
1031
1032 if (me == midbox)
1033 {
1034 if (Math.Abs(contact.normal.Z) > 0.95f)
1035 offset.Z = contact.pos.Z - _position.Z;
1036 else
1037 offset.Z = contact.normal.Z;
1038
1039 offset.Normalize();
1040
1041 if (reverse)
1042 {
1043 contact.normal.X = offset.X;
1044 contact.normal.Y = offset.Y;
1045 contact.normal.Z = offset.Z;
1046 }
1047 else
1048 {
1049 contact.normal.X = -offset.X;
1050 contact.normal.Y = -offset.Y;
1051 contact.normal.Z = -offset.Z;
1052 }
1053
1054 return true;
1055 }
1056
1057 else if (me == feetbox)
1058 {
1059 float h = contact.pos.Z - _position.Z;
1060
1061 if (Math.Abs(contact.normal.Z) > 0.95f)
1062 {
1063 if (contact.normal.Z > 0)
1064 contact.normal.Z = 1.0f;
1065 else
1066 contact.normal.Z = -1.0f;
1067 contact.normal.X = 0.0f;
1068 contact.normal.Y = 0.0f;
1069 feetcollision = true;
1070 if (h < boneOff)
1071 IsColliding = true;
1072 return true;
1073 }
1074
1075 offset.Z = h - feetOff; // distance from top of feetbox
1076
1077 if (offset.Z > 0)
1078 return false;
1079
1080 if (offset.Z > -0.01)
1081 {
1082 offset.X = 0;
1083 offset.Y = 0;
1084 offset.Z = -1.0f;
1085 }
1086 else
1087 {
1088 offset.Normalize();
1089 }
1090
1091 if (reverse)
1092 {
1093 contact.normal.X = offset.X;
1094 contact.normal.Y = offset.Y;
1095 contact.normal.Z = offset.Z;
1096 }
1097 else
1098 {
1099 contact.normal.X = -offset.X;
1100 contact.normal.Y = -offset.Y;
1101 contact.normal.Z = -offset.Z;
1102 }
1103 feetcollision = true;
1104 if (h < boneOff)
1105 IsColliding = true;
1106 }
1107 else
1108 return false;
1109
1110 return true;
1111 }
1112
1113 /// <summary>
1114 /// Called from Simulate
1115 /// This is the avatar's movement control + PID Controller
1116 /// </summary>
1117 /// <param name="timeStep"></param>
1118 public void Move(List<OdeCharacter> defects)
1119 {
1120 if (Body == IntPtr.Zero)
1121 return;
1122
1123 if (m_collisionException)
1124 {
1125 d.BodySetPosition(Body,_position.X, _position.Y, _position.Z);
1126 d.BodySetLinearVel(Body, 0, 0, 0);
1127
1128 float v = _velocity.Length();
1129 if (v != 0)
1130 {
1131 v = 5.0f / v;
1132 _velocity = _velocity * v;
1133 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1134 }
1135 ajustCollider();
1136 m_collisionException = false;
1137 return;
1138 }
1139
1140 d.Vector3 dtmp = d.BodyGetPosition(Body);
1141 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1142
1143 // the Amotor still lets avatar rotation to drift during colisions
1144 // so force it back to identity
1145
1146 d.Quaternion qtmp;
1147 qtmp.W = m_orientation2D.W;
1148 qtmp.X = m_orientation2D.X;
1149 qtmp.Y = m_orientation2D.Y;
1150 qtmp.Z = m_orientation2D.Z;
1151 d.BodySetQuaternion(Body, ref qtmp);
1152
1153 if (m_pidControllerActive == false)
1154 {
1155 _zeroPosition = localpos;
1156 }
1157
1158 if (!localpos.IsFinite())
1159 {
1160 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1161 defects.Add(this);
1162 // _parent_scene.RemoveCharacter(this);
1163
1164 // destroy avatar capsule and related ODE data
1165 AvatarGeomAndBodyDestroy();
1166 return;
1167 }
1168
1169 // check outbounds forcing to be in world
1170 bool fixbody = false;
1171 if (localpos.X < 0.0f)
1172 {
1173 fixbody = true;
1174 localpos.X = 0.1f;
1175 }
1176 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
1177 {
1178 fixbody = true;
1179 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
1180 }
1181 if (localpos.Y < 0.0f)
1182 {
1183 fixbody = true;
1184 localpos.Y = 0.1f;
1185 }
1186 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
1187 {
1188 fixbody = true;
1189 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
1190 }
1191 if (fixbody)
1192 {
1193 m_freemove = false;
1194 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
1195 }
1196
1197 float breakfactor;
1198
1199 Vector3 vec = Vector3.Zero;
1200 dtmp = d.BodyGetLinearVel(Body);
1201 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1202 float velLengthSquared = vel.LengthSquared();
1203
1204 float movementdivisor = 1f;
1205 //Ubit change divisions into multiplications below
1206 if (!m_alwaysRun)
1207 movementdivisor = 1 / walkDivisor;
1208 else
1209 movementdivisor = 1 / runDivisor;
1210
1211 //******************************************
1212 // colide with land
1213
1214 d.AABB aabb;
1215 d.GeomGetAABB(feetbox, out aabb);
1216 float chrminZ = aabb.MinZ; ; // move up a bit
1217 Vector3 posch = localpos;
1218
1219 float ftmp;
1220
1221 if (flying)
1222 {
1223 ftmp = timeStep;
1224 posch.X += vel.X * ftmp;
1225 posch.Y += vel.Y * ftmp;
1226 }
1227
1228 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
1229 if (chrminZ < terrainheight)
1230 {
1231 float depth = terrainheight - chrminZ;
1232 if (!flying)
1233 {
1234 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
1235 }
1236 else
1237 vec.Z = depth * PID_P * 50;
1238
1239 if (depth < 0.1f)
1240 {
1241 m_colliderGroundfilter++;
1242 if (m_colliderGroundfilter > 2)
1243 {
1244 m_iscolliding = true;
1245 m_colliderfilter = 2;
1246
1247 if (m_colliderGroundfilter > 10)
1248 {
1249 m_colliderGroundfilter = 10;
1250 m_freemove = false;
1251 }
1252
1253 m_iscollidingGround = true;
1254
1255 ContactPoint contact = new ContactPoint();
1256 contact.PenetrationDepth = depth;
1257 contact.Position.X = localpos.X;
1258 contact.Position.Y = localpos.Y;
1259 contact.Position.Z = terrainheight;
1260 contact.SurfaceNormal.X = 0.0f;
1261 contact.SurfaceNormal.Y = 0.0f;
1262 contact.SurfaceNormal.Z = -1f;
1263 contact.RelativeSpeed = -vel.Z;
1264 contact.CharacterFeet = true;
1265 AddCollisionEvent(0, contact);
1266
1267 vec.Z *= 0.5f;
1268 }
1269 }
1270
1271 else
1272 {
1273 m_colliderGroundfilter = 0;
1274 m_iscollidingGround = false;
1275 }
1276 }
1277 else
1278 {
1279 m_colliderGroundfilter = 0;
1280 m_iscollidingGround = false;
1281 }
1282
1283
1284 //******************************************
1285
1286 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
1287
1288 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
1289 if (!tviszero)
1290 m_freemove = false;
1291
1292 if (!m_freemove)
1293 {
1294
1295 // if velocity is zero, use position control; otherwise, velocity control
1296 if (tviszero && m_iscolliding)
1297 {
1298 // keep track of where we stopped. No more slippin' & slidin'
1299 if (!_zeroFlag)
1300 {
1301 _zeroFlag = true;
1302 _zeroPosition = localpos;
1303 }
1304 if (m_pidControllerActive)
1305 {
1306 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1307 // react to the physics scene by moving it's position.
1308 // Avatar to Avatar collisions
1309 // Prim to avatar collisions
1310
1311 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
1312 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
1313 if (flying)
1314 {
1315 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1316 }
1317 }
1318 //PidStatus = true;
1319 }
1320 else
1321 {
1322 m_pidControllerActive = true;
1323 _zeroFlag = false;
1324
1325 if (m_iscolliding)
1326 {
1327 if (!flying)
1328 {
1329 if (_target_velocity.Z > 0.0f)
1330 {
1331 // We're colliding with something and we're not flying but we're moving
1332 // This means we're walking or running. JUMPING
1333 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
1334 }
1335 // We're standing on something
1336 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
1337 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
1338 }
1339 else
1340 {
1341 // We're flying and colliding with something
1342 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
1343 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
1344 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1345 }
1346 }
1347 else // ie not colliding
1348 {
1349 if (flying) //(!m_iscolliding && flying)
1350 {
1351 // we're in mid air suspended
1352 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
1353 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
1354 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
1355 }
1356
1357 else
1358 {
1359 // we're not colliding and we're not flying so that means we're falling!
1360 // m_iscolliding includes collisions with the ground.
1361
1362 // d.Vector3 pos = d.BodyGetPosition(Body);
1363 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
1364 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
1365 }
1366 }
1367 }
1368
1369 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
1370 {
1371 breakfactor = 0.16f * m_mass;
1372 vec.X -= breakfactor * vel.X;
1373 vec.Y -= breakfactor * vel.Y;
1374 vec.Z -= breakfactor * vel.Z;
1375 }
1376 }
1377 else
1378 {
1379 breakfactor = m_mass;
1380 vec.X -= breakfactor * vel.X;
1381 vec.Y -= breakfactor * vel.Y;
1382 if (flying)
1383 vec.Z -= breakfactor * vel.Z;
1384 else
1385 vec.Z -= .5f* m_mass * vel.Z;
1386 }
1387
1388 if (flying)
1389 {
1390 vec.Z -= _parent_scene.gravityz * m_mass;
1391
1392 //Added for auto fly height. Kitto Flora
1393 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1394
1395 if (localpos.Z < target_altitude)
1396 {
1397 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1398 }
1399 // end add Kitto Flora
1400 }
1401
1402 if (vec.IsFinite())
1403 {
1404 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1405 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1406 }
1407 else
1408 {
1409 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1410 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1411 defects.Add(this);
1412 // _parent_scene.RemoveCharacter(this);
1413 // destroy avatar capsule and related ODE data
1414 AvatarGeomAndBodyDestroy();
1415 return;
1416 }
1417
1418 // update our local ideia of position velocity and aceleration
1419 // _position = localpos;
1420 _position = localpos;
1421
1422 if (_zeroFlag)
1423 {
1424 _velocity = Vector3.Zero;
1425 _acceleration = Vector3.Zero;
1426 m_rotationalVelocity = Vector3.Zero;
1427 }
1428 else
1429 {
1430 Vector3 a =_velocity; // previus velocity
1431 SetSmooth(ref _velocity, ref vel, 2);
1432 a = (_velocity - a) * invtimeStep;
1433 SetSmooth(ref _acceleration, ref a, 2);
1434
1435 dtmp = d.BodyGetAngularVel(Body);
1436 m_rotationalVelocity.X = 0f;
1437 m_rotationalVelocity.Y = 0f;
1438 m_rotationalVelocity.Z = dtmp.Z;
1439 Math.Round(m_rotationalVelocity.Z,3);
1440 }
1441 ajustCollider();
1442 }
1443
1444 public void round(ref Vector3 v, int digits)
1445 {
1446 v.X = (float)Math.Round(v.X, digits);
1447 v.Y = (float)Math.Round(v.Y, digits);
1448 v.Z = (float)Math.Round(v.Z, digits);
1449 }
1450
1451 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1452 {
1453 dst.X = 0.1f * dst.X + 0.9f * value.X;
1454 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1455 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1456 }
1457
1458 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1459 {
1460 dst.X = 0.4f * dst.X + 0.6f * value.X;
1461 dst.X = (float)Math.Round(dst.X, rounddigits);
1462
1463 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1464 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1465
1466 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1467 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1468 }
1469
1470
1471 /// <summary>
1472 /// Updates the reported position and velocity.
1473 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1474 /// also outbounds checking
1475 /// copy and outbounds now done in move(..) at ode rate
1476 ///
1477 /// </summary>
1478 public void UpdatePositionAndVelocity()
1479 {
1480 return;
1481
1482// if (Body == IntPtr.Zero)
1483// return;
1484
1485 }
1486
1487 /// <summary>
1488 /// Cleanup the things we use in the scene.
1489 /// </summary>
1490 public void Destroy()
1491 {
1492 AddChange(changes.Remove, null);
1493 }
1494
1495 public override void CrossingFailure()
1496 {
1497 }
1498
1499 public override Vector3 PIDTarget { set { return; } }
1500 public override bool PIDActive { set { return; } }
1501 public override float PIDTau { set { return; } }
1502
1503 public override float PIDHoverHeight { set { return; } }
1504 public override bool PIDHoverActive { set { return; } }
1505 public override PIDHoverType PIDHoverType { set { return; } }
1506 public override float PIDHoverTau { set { return; } }
1507
1508 public override Quaternion APIDTarget { set { return; } }
1509
1510 public override bool APIDActive { set { return; } }
1511
1512 public override float APIDStrength { set { return; } }
1513
1514 public override float APIDDamping { set { return; } }
1515
1516
1517 public override void SubscribeEvents(int ms)
1518 {
1519 m_eventsubscription = ms;
1520 m_cureventsubscription = 0;
1521 if (CollisionEventsThisFrame == null)
1522 CollisionEventsThisFrame = new CollisionEventUpdate();
1523 SentEmptyCollisionsEvent = false;
1524 }
1525
1526 public override void UnSubscribeEvents()
1527 {
1528 if (CollisionEventsThisFrame != null)
1529 {
1530 CollisionEventsThisFrame.Clear();
1531 CollisionEventsThisFrame = null;
1532 }
1533 m_eventsubscription = 0;
1534 }
1535
1536 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1537 {
1538 if (CollisionEventsThisFrame == null)
1539 CollisionEventsThisFrame = new CollisionEventUpdate();
1540 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1541 _parent_scene.AddCollisionEventReporting(this);
1542 }
1543
1544 public void SendCollisions()
1545 {
1546 if (CollisionEventsThisFrame == null)
1547 return;
1548
1549 if (m_cureventsubscription < m_eventsubscription)
1550 return;
1551
1552 m_cureventsubscription = 0;
1553
1554 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1555
1556 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1557 {
1558 base.SendCollisionUpdate(CollisionEventsThisFrame);
1559
1560 if (ncolisions == 0)
1561 {
1562 SentEmptyCollisionsEvent = true;
1563 _parent_scene.RemoveCollisionEventReporting(this);
1564 }
1565 else
1566 {
1567 SentEmptyCollisionsEvent = false;
1568 CollisionEventsThisFrame.Clear();
1569 }
1570 }
1571 }
1572
1573 internal void AddCollisionFrameTime(int t)
1574 {
1575 // protect it from overflow crashing
1576 if (m_cureventsubscription < 50000)
1577 m_cureventsubscription += t;
1578 }
1579
1580 public override bool SubscribedEvents()
1581 {
1582 if (m_eventsubscription > 0)
1583 return true;
1584 return false;
1585 }
1586
1587 private void changePhysicsStatus(bool NewStatus)
1588 {
1589 if (NewStatus != m_isPhysical)
1590 {
1591 if (NewStatus)
1592 {
1593 AvatarGeomAndBodyDestroy();
1594
1595 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1596
1597 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1598 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1599 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1600 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1601 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1602 _parent_scene.AddCharacter(this);
1603 }
1604 else
1605 {
1606 _parent_scene.RemoveCollisionEventReporting(this);
1607 _parent_scene.RemoveCharacter(this);
1608 // destroy avatar capsule and related ODE data
1609 AvatarGeomAndBodyDestroy();
1610 }
1611 m_freemove = false;
1612 m_isPhysical = NewStatus;
1613 }
1614 }
1615
1616 private void changeAdd()
1617 {
1618 changePhysicsStatus(true);
1619 }
1620
1621 private void changeRemove()
1622 {
1623 changePhysicsStatus(false);
1624 }
1625
1626 private void changeShape(PrimitiveBaseShape arg)
1627 {
1628 }
1629
1630 private void changeAvatarSize(strAvatarSize st)
1631 {
1632 m_feetOffset = st.offset;
1633 changeSize(st.size);
1634 }
1635
1636 private void changeSize(Vector3 pSize)
1637 {
1638 if (pSize.IsFinite())
1639 {
1640 // for now only look to Z changes since viewers also don't change X and Y
1641 if (pSize.Z != m_size.Z)
1642 {
1643 AvatarGeomAndBodyDestroy();
1644
1645
1646 float oldsz = m_size.Z;
1647 m_size = pSize;
1648
1649
1650 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1651 _position.Z + (m_size.Z - oldsz) * 0.5f);
1652
1653 Velocity = Vector3.Zero;
1654
1655
1656 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1657 _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this;
1658 _parent_scene.actor_name_map[midbox] = (PhysicsActor)this;
1659 _parent_scene.actor_name_map[topbox] = (PhysicsActor)this;
1660 _parent_scene.actor_name_map[bbox] = (PhysicsActor)this;
1661 }
1662 m_freemove = false;
1663 m_collisionException = false;
1664 m_pidControllerActive = true;
1665 }
1666 else
1667 {
1668 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1669 }
1670 }
1671
1672 private void changePosition( Vector3 newPos)
1673 {
1674 if (Body != IntPtr.Zero)
1675 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1676 _position = newPos;
1677 m_freemove = false;
1678 m_pidControllerActive = true;
1679 }
1680
1681 private void changeOrientation(Quaternion newOri)
1682 {
1683 if (m_orientation != newOri)
1684 {
1685 m_orientation = newOri; // keep a copy for core use
1686 // but only use rotations around Z
1687
1688 m_orientation2D.W = newOri.W;
1689 m_orientation2D.Z = newOri.Z;
1690
1691 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1692 if (t > 0)
1693 {
1694 t = 1.0f / (float)Math.Sqrt(t);
1695 m_orientation2D.W *= t;
1696 m_orientation2D.Z *= t;
1697 }
1698 else
1699 {
1700 m_orientation2D.W = 1.0f;
1701 m_orientation2D.Z = 0f;
1702 }
1703 m_orientation2D.Y = 0f;
1704 m_orientation2D.X = 0f;
1705
1706 d.Quaternion myrot = new d.Quaternion();
1707 myrot.X = m_orientation2D.X;
1708 myrot.Y = m_orientation2D.Y;
1709 myrot.Z = m_orientation2D.Z;
1710 myrot.W = m_orientation2D.W;
1711 d.BodySetQuaternion(Body, ref myrot);
1712 }
1713 }
1714
1715 private void changeVelocity(Vector3 newVel)
1716 {
1717 m_pidControllerActive = true;
1718 m_freemove = false;
1719 _target_velocity = newVel;
1720 }
1721
1722 private void changeSetTorque(Vector3 newTorque)
1723 {
1724 }
1725
1726 private void changeAddForce(Vector3 newForce)
1727 {
1728 }
1729
1730 private void changeAddAngularForce(Vector3 arg)
1731 {
1732 }
1733
1734 private void changeAngularLock(Vector3 arg)
1735 {
1736 }
1737
1738 private void changeFloatOnWater(bool arg)
1739 {
1740 }
1741
1742 private void changeVolumedetetion(bool arg)
1743 {
1744 }
1745
1746 private void changeSelectedStatus(bool arg)
1747 {
1748 }
1749
1750 private void changeDisable(bool arg)
1751 {
1752 }
1753
1754 private void changeBuilding(bool arg)
1755 {
1756 }
1757
1758 private void setFreeMove()
1759 {
1760 m_pidControllerActive = true;
1761 _zeroFlag = false;
1762 _target_velocity = Vector3.Zero;
1763 m_freemove = true;
1764 m_colliderfilter = -2;
1765 m_colliderObjectfilter = -2;
1766 m_colliderGroundfilter = -2;
1767
1768 m_iscolliding = false;
1769 m_iscollidingGround = false;
1770 m_iscollidingObj = false;
1771
1772 CollisionEventsThisFrame.Clear();
1773 }
1774
1775 private void changeForce(Vector3 newForce)
1776 {
1777 setFreeMove();
1778
1779 if (Body != IntPtr.Zero)
1780 {
1781 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1782 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1783 }
1784 }
1785
1786 // for now momentum is actually velocity
1787 private void changeMomentum(Vector3 newmomentum)
1788 {
1789 _velocity = newmomentum;
1790 setFreeMove();
1791
1792 if (Body != IntPtr.Zero)
1793 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1794 ajustCollider();
1795 }
1796
1797 private void donullchange()
1798 {
1799 }
1800
1801 public bool DoAChange(changes what, object arg)
1802 {
1803 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1804 {
1805 return false;
1806 }
1807
1808 // nasty switch
1809 switch (what)
1810 {
1811 case changes.Add:
1812 changeAdd();
1813 break;
1814 case changes.Remove:
1815 changeRemove();
1816 break;
1817
1818 case changes.Position:
1819 changePosition((Vector3)arg);
1820 break;
1821
1822 case changes.Orientation:
1823 changeOrientation((Quaternion)arg);
1824 break;
1825
1826 case changes.PosOffset:
1827 donullchange();
1828 break;
1829
1830 case changes.OriOffset:
1831 donullchange();
1832 break;
1833
1834 case changes.Velocity:
1835 changeVelocity((Vector3)arg);
1836 break;
1837
1838 // case changes.Acceleration:
1839 // changeacceleration((Vector3)arg);
1840 // break;
1841 // case changes.AngVelocity:
1842 // changeangvelocity((Vector3)arg);
1843 // break;
1844
1845 case changes.Force:
1846 changeForce((Vector3)arg);
1847 break;
1848
1849 case changes.Torque:
1850 changeSetTorque((Vector3)arg);
1851 break;
1852
1853 case changes.AddForce:
1854 changeAddForce((Vector3)arg);
1855 break;
1856
1857 case changes.AddAngForce:
1858 changeAddAngularForce((Vector3)arg);
1859 break;
1860
1861 case changes.AngLock:
1862 changeAngularLock((Vector3)arg);
1863 break;
1864
1865 case changes.Size:
1866 changeSize((Vector3)arg);
1867 break;
1868
1869 case changes.AvatarSize:
1870 changeAvatarSize((strAvatarSize)arg);
1871 break;
1872
1873 case changes.Momentum:
1874 changeMomentum((Vector3)arg);
1875 break;
1876/* not in use for now
1877 case changes.Shape:
1878 changeShape((PrimitiveBaseShape)arg);
1879 break;
1880
1881 case changes.CollidesWater:
1882 changeFloatOnWater((bool)arg);
1883 break;
1884
1885 case changes.VolumeDtc:
1886 changeVolumedetetion((bool)arg);
1887 break;
1888
1889 case changes.Physical:
1890 changePhysicsStatus((bool)arg);
1891 break;
1892
1893 case changes.Selected:
1894 changeSelectedStatus((bool)arg);
1895 break;
1896
1897 case changes.disabled:
1898 changeDisable((bool)arg);
1899 break;
1900
1901 case changes.building:
1902 changeBuilding((bool)arg);
1903 break;
1904*/
1905 case changes.Null:
1906 donullchange();
1907 break;
1908
1909 default:
1910 donullchange();
1911 break;
1912 }
1913 return false;
1914 }
1915
1916 public void AddChange(changes what, object arg)
1917 {
1918 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1919 }
1920
1921 private struct strAvatarSize
1922 {
1923 public Vector3 size;
1924 public float offset;
1925 }
1926
1927 }
1928}
generated by cgit v1.1 at 2024-12-17 01:30:53 +0000