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