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.cs1477
1 files changed, 1477 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..c363310
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1477 @@
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 bool _zeroFlag = false;
78 private Vector3 _velocity;
79 private Vector3 _target_velocity;
80 private Vector3 _acceleration;
81 private Vector3 m_rotationalVelocity;
82 private float m_mass = 80f;
83 public float m_density = 60f;
84 private bool m_pidControllerActive = true;
85 public float PID_D = 800.0f;
86 public float PID_P = 900.0f;
87 //private static float POSTURE_SERVO = 10000.0f;
88 public float CAPSULE_RADIUS = 0.37f;
89 public float CAPSULE_LENGTH = 2.140599f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool m_iscolliding = false;
94 private bool m_iscollidingGround = false;
95 private bool m_iscollidingObj = false;
96 private bool m_alwaysRun = false;
97 private int m_requestedUpdateFrequency = 0;
98 private uint m_localID = 0;
99 public bool m_returnCollisions = false;
100 // taints and their non-tainted counterparts
101 public bool m_isPhysical = false; // the current physical status
102 public float MinimumGroundFlightOffset = 3f;
103
104 private float m_buoyancy = 0f;
105
106 private bool m_freemove = false;
107 // private CollisionLocker ode;
108
109// private string m_name = String.Empty;
110 // other filter control
111 int m_colliderfilter = 0;
112 int m_colliderGroundfilter = 0;
113 int m_colliderObjectfilter = 0;
114
115 // Default we're a Character
116 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
117
118 // Default, Collide with Other Geometries, spaces, bodies and characters.
119 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
120 | CollisionCategories.Geom
121 | CollisionCategories.VolumeDtc
122 );
123 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero;
127 public IntPtr Amotor = IntPtr.Zero;
128 public d.Mass ShellMass;
129// public bool collidelock = false;
130
131 public int m_eventsubscription = 0;
132 private int m_cureventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = null;
134 private bool SentEmptyCollisionsEvent;
135
136 // unique UUID of this character object
137 public UUID m_uuid;
138 public bool bad = false;
139
140 float mu;
141
142 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
143 {
144 m_uuid = UUID.Random();
145
146 if (pos.IsFinite())
147 {
148 if (pos.Z > 99999f)
149 {
150 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
151 }
152 if (pos.Z < -100f) // shouldn't this be 0 ?
153 {
154 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
155 }
156 _position = pos;
157 }
158 else
159 {
160 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
161 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
162 }
163
164 _parent_scene = parent_scene;
165
166 PID_D = pid_d;
167 PID_P = pid_p;
168 CAPSULE_RADIUS = capsule_radius;
169 m_density = density;
170 m_mass = 80f; // sure we have a default
171
172 // force lower density for testing
173 m_density = 3.0f;
174
175
176 mu = parent_scene.AvatarFriction;
177
178 walkDivisor = walk_divisor;
179 runDivisor = rundivisor;
180
181 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
182 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
183
184 m_isPhysical = false; // current status: no ODE information exists
185
186 Name = avName;
187
188 AddChange(changes.Add, null);
189 }
190
191 public override int PhysicsActorType
192 {
193 get { return (int)ActorTypes.Agent; }
194 set { return; }
195 }
196
197 public override void getContactData(ref ContactData cdata)
198 {
199 cdata.mu = mu;
200 cdata.bounce = 0;
201 cdata.softcolide = false;
202 }
203
204 public override bool Building { get; set; }
205
206 /// <summary>
207 /// If this is set, the avatar will move faster
208 /// </summary>
209 public override bool SetAlwaysRun
210 {
211 get { return m_alwaysRun; }
212 set { m_alwaysRun = value; }
213 }
214
215 public override uint LocalID
216 {
217 get { return m_localID; }
218 set { m_localID = value; }
219 }
220
221 public override PhysicsActor ParentActor
222 {
223 get { return (PhysicsActor)this; }
224 }
225
226 public override bool Grabbed
227 {
228 set { return; }
229 }
230
231 public override bool Selected
232 {
233 set { return; }
234 }
235
236 public override float Buoyancy
237 {
238 get { return m_buoyancy; }
239 set { m_buoyancy = value; }
240 }
241
242 public override bool FloatOnWater
243 {
244 set { return; }
245 }
246
247 public override bool IsPhysical
248 {
249 get { return m_isPhysical; }
250 set { return; }
251 }
252
253 public override bool ThrottleUpdates
254 {
255 get { return false; }
256 set { return; }
257 }
258
259 public override bool Flying
260 {
261 get { return flying; }
262 set
263 {
264 flying = value;
265 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
266 }
267 }
268
269 /// <summary>
270 /// Returns if the avatar is colliding in general.
271 /// This includes the ground and objects and avatar.
272 /// </summary>
273 public override bool IsColliding
274 {
275 get { return (m_iscolliding || m_iscollidingGround); }
276 set
277 {
278 if (value)
279 {
280 m_colliderfilter += 2;
281 if (m_colliderfilter > 2)
282 m_colliderfilter = 2;
283 }
284 else
285 {
286 m_colliderfilter--;
287 if (m_colliderfilter < 0)
288 m_colliderfilter = 0;
289 }
290
291 if (m_colliderfilter == 0)
292 m_iscolliding = false;
293 else
294 {
295 m_pidControllerActive = true;
296 m_iscolliding = true;
297 }
298 }
299 }
300
301 /// <summary>
302 /// Returns if an avatar is colliding with the ground
303 /// </summary>
304 public override bool CollidingGround
305 {
306 get { return m_iscollidingGround; }
307 set
308 {
309 /* we now control this
310 if (value)
311 {
312 m_colliderGroundfilter += 2;
313 if (m_colliderGroundfilter > 2)
314 m_colliderGroundfilter = 2;
315 }
316 else
317 {
318 m_colliderGroundfilter--;
319 if (m_colliderGroundfilter < 0)
320 m_colliderGroundfilter = 0;
321 }
322
323 if (m_colliderGroundfilter == 0)
324 m_iscollidingGround = false;
325 else
326 m_iscollidingGround = true;
327 */
328 }
329
330 }
331
332 /// <summary>
333 /// Returns if the avatar is colliding with an object
334 /// </summary>
335 public override bool CollidingObj
336 {
337 get { return m_iscollidingObj; }
338 set
339 {
340 // Ubit filter this also
341 if (value)
342 {
343 m_colliderObjectfilter += 2;
344 if (m_colliderObjectfilter > 2)
345 m_colliderObjectfilter = 2;
346 }
347 else
348 {
349 m_colliderObjectfilter--;
350 if (m_colliderObjectfilter < 0)
351 m_colliderObjectfilter = 0;
352 }
353
354 if (m_colliderObjectfilter == 0)
355 m_iscollidingObj = false;
356 else
357 m_iscollidingObj = true;
358
359 // m_iscollidingObj = value;
360
361 if (m_iscollidingObj)
362 m_pidControllerActive = false;
363 else
364 m_pidControllerActive = true;
365 }
366 }
367
368 /// <summary>
369 /// turn the PID controller on or off.
370 /// The PID Controller will turn on all by itself in many situations
371 /// </summary>
372 /// <param name="status"></param>
373 public void SetPidStatus(bool status)
374 {
375 m_pidControllerActive = status;
376 }
377
378 public override bool Stopped
379 {
380 get { return _zeroFlag; }
381 }
382
383 /// <summary>
384 /// This 'puts' an avatar somewhere in the physics space.
385 /// Not really a good choice unless you 'know' it's a good
386 /// spot otherwise you're likely to orbit the avatar.
387 /// </summary>
388 public override Vector3 Position
389 {
390 get { return _position; }
391 set
392 {
393 if (value.IsFinite())
394 {
395 if (value.Z > 9999999f)
396 {
397 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
398 }
399 if (value.Z < -100f)
400 {
401 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
402 }
403 AddChange(changes.Position, value);
404 }
405 else
406 {
407 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
408 }
409 }
410 }
411
412 public override Vector3 RotationalVelocity
413 {
414 get { return m_rotationalVelocity; }
415 set { m_rotationalVelocity = value; }
416 }
417
418 /// <summary>
419 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
420 /// and use it to offset landings properly
421 /// </summary>
422 public override Vector3 Size
423 {
424 get {
425 float d = CAPSULE_RADIUS * 2;
426 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
427 set
428 {
429 if (value.IsFinite())
430 {
431 AddChange(changes.Size, value);
432 }
433 else
434 {
435 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
436 }
437 }
438 }
439
440 /// <summary>
441 /// This creates the Avatar's physical Surrogate at the position supplied
442 /// </summary>
443 /// <param name="npositionX"></param>
444 /// <param name="npositionY"></param>
445 /// <param name="npositionZ"></param>
446
447 //
448 /// <summary>
449 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
450 /// This may be used in calculations in the scene/scenepresence
451 /// </summary>
452 public override float Mass
453 {
454 get
455 {
456 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
457 return m_density * AVvolume;
458 }
459 }
460 public override void link(PhysicsActor obj)
461 {
462
463 }
464
465 public override void delink()
466 {
467
468 }
469
470 public override void LockAngularMotion(Vector3 axis)
471 {
472
473 }
474
475
476 public override Vector3 Force
477 {
478 get { return _target_velocity; }
479 set { return; }
480 }
481
482 public override int VehicleType
483 {
484 get { return 0; }
485 set { return; }
486 }
487
488 public override void VehicleFloatParam(int param, float value)
489 {
490
491 }
492
493 public override void VehicleVectorParam(int param, Vector3 value)
494 {
495
496 }
497
498 public override void VehicleRotationParam(int param, Quaternion rotation)
499 {
500
501 }
502
503 public override void VehicleFlags(int param, bool remove)
504 {
505
506 }
507
508 public override void SetVolumeDetect(int param)
509 {
510
511 }
512
513 public override Vector3 CenterOfMass
514 {
515 get
516 {
517 Vector3 pos = _position;
518 return pos;
519 }
520 }
521
522 public override Vector3 GeometricCenter
523 {
524 get
525 {
526 Vector3 pos = _position;
527 return pos;
528 }
529 }
530
531 public override PrimitiveBaseShape Shape
532 {
533 set { return; }
534 }
535
536 public override Vector3 Velocity
537 {
538 get
539 {
540 return _velocity;
541 }
542 set
543 {
544 if (value.IsFinite())
545 {
546 AddChange(changes.Velocity, value);
547 }
548 else
549 {
550 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
551 }
552 }
553 }
554
555 public override Vector3 Torque
556 {
557 get { return Vector3.Zero; }
558 set { return; }
559 }
560
561 public override float CollisionScore
562 {
563 get { return 0f; }
564 set { }
565 }
566
567 public override bool Kinematic
568 {
569 get { return false; }
570 set { }
571 }
572
573 public override Quaternion Orientation
574 {
575 get { return Quaternion.Identity; }
576 set
577 {
578 }
579 }
580
581 public override Vector3 Acceleration
582 {
583 get { return _acceleration; }
584 set { }
585 }
586
587 public void SetAcceleration(Vector3 accel)
588 {
589 m_pidControllerActive = true;
590 _acceleration = accel;
591 }
592
593 /// <summary>
594 /// Adds the force supplied to the Target Velocity
595 /// The PID controller takes this target velocity and tries to make it a reality
596 /// </summary>
597 /// <param name="force"></param>
598 public override void AddForce(Vector3 force, bool pushforce)
599 {
600 if (force.IsFinite())
601 {
602 if (pushforce)
603 {
604 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
605 }
606 else
607 {
608 AddChange(changes.Velocity, force);
609 }
610 }
611 else
612 {
613 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
614 }
615 //m_lastUpdateSent = false;
616 }
617
618 public override void AddAngularForce(Vector3 force, bool pushforce)
619 {
620
621 }
622
623 public override void SetMomentum(Vector3 momentum)
624 {
625 if (momentum.IsFinite())
626 AddChange(changes.Momentum, momentum);
627 }
628
629
630 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
631 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
632 // place that is safe to call this routine AvatarGeomAndBodyCreation.
633 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
634 {
635 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
636 if (CAPSULE_LENGTH <= 0)
637 {
638 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
639 CAPSULE_LENGTH = 0.01f;
640
641 }
642
643 if (CAPSULE_RADIUS <= 0)
644 {
645 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
646 CAPSULE_RADIUS = 0.01f;
647
648 }
649 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
650
651 d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories);
652 d.GeomSetCollideBits(Shell, (uint)m_collisionFlags);
653
654 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
655
656 m_mass = ShellMass.mass; // update mass
657
658 // rescale PID parameters
659 PID_D = _parent_scene.avPIDD;
660 PID_P = _parent_scene.avPIDP;
661
662 // rescale PID parameters so that this aren't affected by mass
663 // and so don't get unstable for some masses
664 // also scale by ode time step so you don't need to refix them
665
666 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
667 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
668 PID_P /= 50 * 80;
669 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
670
671 Body = d.BodyCreate(_parent_scene.world);
672
673 _zeroFlag = false;
674 m_pidControllerActive = true;
675 m_freemove = false;
676
677 d.BodySetAutoDisableFlag(Body, false);
678 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
679
680 _position.X = npositionX;
681 _position.Y = npositionY;
682 _position.Z = npositionZ;
683
684 d.BodySetMass(Body, ref ShellMass);
685 d.GeomSetBody(Shell, Body);
686
687 // The purpose of the AMotor here is to keep the avatar's physical
688 // surrogate from rotating while moving
689 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
690 d.JointAttach(Amotor, Body, IntPtr.Zero);
691
692 d.JointSetAMotorMode(Amotor, 0);
693 d.JointSetAMotorNumAxes(Amotor, 3);
694 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
695 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
696 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
697
698 d.JointSetAMotorAngle(Amotor, 0, 0);
699 d.JointSetAMotorAngle(Amotor, 1, 0);
700 d.JointSetAMotorAngle(Amotor, 2, 0);
701
702 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
703 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
704 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
705 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
706 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
707 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
708
709 // These lowstops and high stops are effectively (no wiggle room)
710 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
711 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
712 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
713 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
714 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
715 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
716
717 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
718 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
719 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
720
721 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
722 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
723 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
724 }
725
726 /// <summary>
727 /// Destroys the avatar body and geom
728
729 private void AvatarGeomAndBodyDestroy()
730 {
731 // Kill the Amotor
732 if (Amotor != IntPtr.Zero)
733 {
734 d.JointDestroy(Amotor);
735 Amotor = IntPtr.Zero;
736 }
737
738 if (Body != IntPtr.Zero)
739 {
740 //kill the body
741 d.BodyDestroy(Body);
742 Body = IntPtr.Zero;
743 }
744
745 //kill the Geometry
746 if (Shell != IntPtr.Zero)
747 {
748// _parent_scene.geom_name_map.Remove(Shell);
749 _parent_scene.actor_name_map.Remove(Shell);
750 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
751 d.GeomDestroy(Shell);
752 Shell = IntPtr.Zero;
753 }
754 }
755
756 /// <summary>
757 /// Called from Simulate
758 /// This is the avatar's movement control + PID Controller
759 /// </summary>
760 /// <param name="timeStep"></param>
761 public void Move(float timeStep, List<OdeCharacter> defects)
762 {
763 if (Body == IntPtr.Zero)
764 return;
765
766 d.Vector3 dtmp = d.BodyGetPosition(Body);
767 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
768
769 // the Amotor still lets avatar rotation to drift during colisions
770 // so force it back to identity
771
772 d.Quaternion qtmp;
773 qtmp.W = 1;
774 qtmp.X = 0;
775 qtmp.Y = 0;
776 qtmp.Z = 0;
777 d.BodySetQuaternion(Body, ref qtmp);
778
779 if (m_pidControllerActive == false)
780 {
781 _zeroPosition = localpos;
782 }
783
784 if (!localpos.IsFinite())
785 {
786 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
787 defects.Add(this);
788 // _parent_scene.RemoveCharacter(this);
789
790 // destroy avatar capsule and related ODE data
791 AvatarGeomAndBodyDestroy();
792 return;
793 }
794
795 // check outbounds forcing to be in world
796 bool fixbody = false;
797 if (localpos.X < 0.0f)
798 {
799 fixbody = true;
800 localpos.X = 0.1f;
801 }
802 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
803 {
804 fixbody = true;
805 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
806 }
807 if (localpos.Y < 0.0f)
808 {
809 fixbody = true;
810 localpos.Y = 0.1f;
811 }
812 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
813 {
814 fixbody = true;
815 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
816 }
817 if (fixbody)
818 {
819 m_freemove = false;
820 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
821 }
822
823 float breakfactor;
824
825 Vector3 vec = Vector3.Zero;
826 dtmp = d.BodyGetLinearVel(Body);
827 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
828 float velLengthSquared = vel.LengthSquared();
829
830 float movementdivisor = 1f;
831 //Ubit change divisions into multiplications below
832 if (!m_alwaysRun)
833 movementdivisor = 1 / walkDivisor;
834 else
835 movementdivisor = 1 / runDivisor;
836
837 //******************************************
838 // colide with land
839 d.AABB aabb;
840 d.GeomGetAABB(Shell, out aabb);
841 float chrminZ = aabb.MinZ;
842
843 Vector3 posch = localpos;
844
845 float ftmp;
846
847 if (flying)
848 {
849 ftmp = timeStep;
850 posch.X += vel.X * ftmp;
851 posch.Y += vel.Y * ftmp;
852 }
853
854 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
855 if (chrminZ < terrainheight)
856 {
857 float depth = terrainheight - chrminZ;
858 if (!flying)
859 {
860 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
861 }
862 else
863 vec.Z = depth * PID_P * 50;
864
865 if (depth < 0.1f)
866 {
867 m_colliderGroundfilter++;
868 if (m_colliderGroundfilter > 2)
869 {
870 m_iscolliding = true;
871 m_colliderfilter = 2;
872
873 if (m_colliderGroundfilter > 10)
874 {
875 m_colliderGroundfilter = 10;
876 m_freemove = false;
877 }
878
879 m_iscollidingGround = true;
880
881 ContactPoint contact = new ContactPoint();
882 contact.PenetrationDepth = depth;
883 contact.Position.X = localpos.X;
884 contact.Position.Y = localpos.Y;
885 contact.Position.Z = chrminZ;
886 contact.SurfaceNormal.X = 0f;
887 contact.SurfaceNormal.Y = 0f;
888 contact.SurfaceNormal.Z = -1f;
889 contact.RelativeSpeed = -vel.Z;
890 AddCollisionEvent(0, contact);
891
892 vec.Z *= 0.5f;
893 }
894 }
895
896 else
897 {
898 m_colliderGroundfilter = 0;
899 m_iscollidingGround = false;
900 }
901 }
902 else
903 {
904 m_colliderGroundfilter = 0;
905 m_iscollidingGround = false;
906 }
907
908 //******************************************
909
910 bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f);
911
912 // if (!tviszero || m_iscolliding || velLengthSquared <0.01)
913 if (!tviszero)
914 m_freemove = false;
915
916 if (!m_freemove)
917 {
918
919 // if velocity is zero, use position control; otherwise, velocity control
920 if (tviszero && m_iscolliding)
921 {
922 // keep track of where we stopped. No more slippin' & slidin'
923 if (!_zeroFlag)
924 {
925 _zeroFlag = true;
926 _zeroPosition = localpos;
927 }
928 if (m_pidControllerActive)
929 {
930 // We only want to deactivate the PID Controller if we think we want to have our surrogate
931 // react to the physics scene by moving it's position.
932 // Avatar to Avatar collisions
933 // Prim to avatar collisions
934
935 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
936 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
937 if (flying)
938 {
939 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
940 }
941 }
942 //PidStatus = true;
943 }
944 else
945 {
946 m_pidControllerActive = true;
947 _zeroFlag = false;
948
949 if (m_iscolliding)
950 {
951 if (!flying)
952 {
953 if (_target_velocity.Z > 0.0f)
954 {
955 // We're colliding with something and we're not flying but we're moving
956 // This means we're walking or running. JUMPING
957 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
958 }
959 // We're standing on something
960 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
961 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
962 }
963 else
964 {
965 // We're flying and colliding with something
966 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
967 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
968 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
969 }
970 }
971 else // ie not colliding
972 {
973 if (flying) //(!m_iscolliding && flying)
974 {
975 // we're in mid air suspended
976 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
977 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
978 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
979 }
980
981 else
982 {
983 // we're not colliding and we're not flying so that means we're falling!
984 // m_iscolliding includes collisions with the ground.
985
986 // d.Vector3 pos = d.BodyGetPosition(Body);
987 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
988 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
989 }
990 }
991 }
992
993 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
994 {
995 breakfactor = 0.16f * m_mass;
996 vec.X -= breakfactor * vel.X;
997 vec.Y -= breakfactor * vel.Y;
998 vec.Z -= breakfactor * vel.Z;
999 }
1000 }
1001 else
1002 {
1003 breakfactor = m_mass;
1004 vec.X -= breakfactor * vel.X;
1005 vec.Y -= breakfactor * vel.Y;
1006 if (flying)
1007 vec.Z -= breakfactor * vel.Z;
1008 else
1009 vec.Z -= .5f* m_mass * vel.Z;
1010 }
1011
1012 if (flying)
1013 {
1014 vec.Z -= _parent_scene.gravityz * m_mass;
1015
1016 //Added for auto fly height. Kitto Flora
1017 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1018
1019 if (localpos.Z < target_altitude)
1020 {
1021 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1022 }
1023 // end add Kitto Flora
1024 }
1025
1026 if (vec.IsFinite())
1027 {
1028 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1029 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1030 }
1031 else
1032 {
1033 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1034 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1035 defects.Add(this);
1036 // _parent_scene.RemoveCharacter(this);
1037 // destroy avatar capsule and related ODE data
1038 AvatarGeomAndBodyDestroy();
1039 return;
1040 }
1041
1042 // update our local ideia of position velocity and aceleration
1043 _position = localpos;
1044 if (_zeroFlag)
1045 {
1046 _velocity = Vector3.Zero;
1047 _acceleration = Vector3.Zero;
1048 }
1049 else
1050 {
1051 _acceleration = _velocity; // previus velocity
1052 _velocity = vel;
1053 _acceleration = (vel - _acceleration) / timeStep;
1054 }
1055
1056 }
1057
1058 /// <summary>
1059 /// Updates the reported position and velocity.
1060 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1061 /// also outbounds checking
1062 /// copy and outbounds now done in move(..) at ode rate
1063 ///
1064 /// </summary>
1065 public void UpdatePositionAndVelocity()
1066 {
1067 return;
1068
1069// if (Body == IntPtr.Zero)
1070// return;
1071
1072 }
1073
1074 /// <summary>
1075 /// Cleanup the things we use in the scene.
1076 /// </summary>
1077 public void Destroy()
1078 {
1079 AddChange(changes.Remove, null);
1080 }
1081
1082 public override void CrossingFailure()
1083 {
1084 }
1085
1086 public override Vector3 PIDTarget { set { return; } }
1087 public override bool PIDActive { set { return; } }
1088 public override float PIDTau { set { return; } }
1089
1090 public override float PIDHoverHeight { set { return; } }
1091 public override bool PIDHoverActive { set { return; } }
1092 public override PIDHoverType PIDHoverType { set { return; } }
1093 public override float PIDHoverTau { set { return; } }
1094
1095 public override Quaternion APIDTarget { set { return; } }
1096
1097 public override bool APIDActive { set { return; } }
1098
1099 public override float APIDStrength { set { return; } }
1100
1101 public override float APIDDamping { set { return; } }
1102
1103
1104 public override void SubscribeEvents(int ms)
1105 {
1106 m_eventsubscription = ms;
1107 m_cureventsubscription = 0;
1108 if (CollisionEventsThisFrame == null)
1109 CollisionEventsThisFrame = new CollisionEventUpdate();
1110 SentEmptyCollisionsEvent = false;
1111 }
1112
1113 public override void UnSubscribeEvents()
1114 {
1115 if (CollisionEventsThisFrame != null)
1116 {
1117 CollisionEventsThisFrame.Clear();
1118 CollisionEventsThisFrame = null;
1119 }
1120 m_eventsubscription = 0;
1121 }
1122
1123 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1124 {
1125 if (CollisionEventsThisFrame == null)
1126 CollisionEventsThisFrame = new CollisionEventUpdate();
1127 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1128 _parent_scene.AddCollisionEventReporting(this);
1129 }
1130
1131 public void SendCollisions()
1132 {
1133 if (CollisionEventsThisFrame == null)
1134 return;
1135
1136 if (m_cureventsubscription < m_eventsubscription)
1137 return;
1138
1139 m_cureventsubscription = 0;
1140
1141 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1142
1143 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1144 {
1145 base.SendCollisionUpdate(CollisionEventsThisFrame);
1146
1147 if (ncolisions == 0)
1148 {
1149 SentEmptyCollisionsEvent = true;
1150 _parent_scene.RemoveCollisionEventReporting(this);
1151 }
1152 else
1153 {
1154 SentEmptyCollisionsEvent = false;
1155 CollisionEventsThisFrame.Clear();
1156 }
1157 }
1158 }
1159
1160 internal void AddCollisionFrameTime(int t)
1161 {
1162 // protect it from overflow crashing
1163 if (m_cureventsubscription < 50000)
1164 m_cureventsubscription += t;
1165 }
1166
1167 public override bool SubscribedEvents()
1168 {
1169 if (m_eventsubscription > 0)
1170 return true;
1171 return false;
1172 }
1173
1174 private void changePhysicsStatus(bool NewStatus)
1175 {
1176 if (NewStatus != m_isPhysical)
1177 {
1178 if (NewStatus)
1179 {
1180 // Create avatar capsule and related ODE data
1181 if ((Shell != IntPtr.Zero))
1182 {
1183 // a lost shell ?
1184 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1185 + (Shell != IntPtr.Zero ? "Shell " : "")
1186 + (Body != IntPtr.Zero ? "Body " : "")
1187 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1188 AvatarGeomAndBodyDestroy();
1189 }
1190
1191 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1192
1193 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1194 _parent_scene.AddCharacter(this);
1195 }
1196 else
1197 {
1198 _parent_scene.RemoveCollisionEventReporting(this);
1199 _parent_scene.RemoveCharacter(this);
1200 // destroy avatar capsule and related ODE data
1201 AvatarGeomAndBodyDestroy();
1202 }
1203 m_freemove = false;
1204 m_isPhysical = NewStatus;
1205 }
1206 }
1207
1208 private void changeAdd()
1209 {
1210 changePhysicsStatus(true);
1211 }
1212
1213 private void changeRemove()
1214 {
1215 changePhysicsStatus(false);
1216 }
1217
1218 private void changeShape(PrimitiveBaseShape arg)
1219 {
1220 }
1221
1222 private void changeSize(Vector3 Size)
1223 {
1224 if (Size.IsFinite())
1225 {
1226 float caplen = Size.Z;
1227
1228 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1229
1230 if (caplen != CAPSULE_LENGTH)
1231 {
1232 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1233 {
1234 AvatarGeomAndBodyDestroy();
1235
1236 float prevCapsule = CAPSULE_LENGTH;
1237 CAPSULE_LENGTH = caplen;
1238
1239 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1240 _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f);
1241
1242 Velocity = Vector3.Zero;
1243
1244 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1245 }
1246 else
1247 {
1248 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1249 + (Shell == IntPtr.Zero ? "Shell " : "")
1250 + (Body == IntPtr.Zero ? "Body " : "")
1251 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1252 }
1253 }
1254 m_freemove = false;
1255 m_pidControllerActive = true;
1256 }
1257 else
1258 {
1259 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1260 }
1261 }
1262
1263 private void changePosition( Vector3 newPos)
1264 {
1265 if (Body != IntPtr.Zero)
1266 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1267 _position = newPos;
1268 m_freemove = false;
1269 m_pidControllerActive = true;
1270 }
1271
1272 private void changeOrientation(Quaternion newOri)
1273 {
1274 }
1275
1276 private void changeVelocity(Vector3 newVel)
1277 {
1278 m_pidControllerActive = true;
1279 m_freemove = false;
1280 _target_velocity = newVel;
1281 }
1282
1283 private void changeSetTorque(Vector3 newTorque)
1284 {
1285 }
1286
1287 private void changeAddForce(Vector3 newForce)
1288 {
1289 }
1290
1291 private void changeAddAngularForce(Vector3 arg)
1292 {
1293 }
1294
1295 private void changeAngularLock(Vector3 arg)
1296 {
1297 }
1298
1299 private void changeFloatOnWater(bool arg)
1300 {
1301 }
1302
1303 private void changeVolumedetetion(bool arg)
1304 {
1305 }
1306
1307 private void changeSelectedStatus(bool arg)
1308 {
1309 }
1310
1311 private void changeDisable(bool arg)
1312 {
1313 }
1314
1315 private void changeBuilding(bool arg)
1316 {
1317 }
1318
1319 private void setFreeMove()
1320 {
1321 m_pidControllerActive = true;
1322 _zeroFlag = false;
1323 _target_velocity = Vector3.Zero;
1324 m_freemove = true;
1325 m_colliderfilter = -2;
1326 m_colliderObjectfilter = -2;
1327 m_colliderGroundfilter = -2;
1328
1329 m_iscolliding = false;
1330 m_iscollidingGround = false;
1331 m_iscollidingObj = false;
1332
1333 CollisionEventsThisFrame.Clear();
1334 }
1335
1336 private void changeForce(Vector3 newForce)
1337 {
1338 setFreeMove();
1339
1340 if (Body != IntPtr.Zero)
1341 {
1342 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1343 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1344 }
1345 }
1346
1347 // for now momentum is actually velocity
1348 private void changeMomentum(Vector3 newmomentum)
1349 {
1350 _velocity = newmomentum;
1351 setFreeMove();
1352
1353 if (Body != IntPtr.Zero)
1354 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1355 }
1356
1357 private void donullchange()
1358 {
1359 }
1360
1361 public bool DoAChange(changes what, object arg)
1362 {
1363 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1364 {
1365 return false;
1366 }
1367
1368 // nasty switch
1369 switch (what)
1370 {
1371 case changes.Add:
1372 changeAdd();
1373 break;
1374 case changes.Remove:
1375 changeRemove();
1376 break;
1377
1378 case changes.Position:
1379 changePosition((Vector3)arg);
1380 break;
1381
1382 case changes.Orientation:
1383 changeOrientation((Quaternion)arg);
1384 break;
1385
1386 case changes.PosOffset:
1387 donullchange();
1388 break;
1389
1390 case changes.OriOffset:
1391 donullchange();
1392 break;
1393
1394 case changes.Velocity:
1395 changeVelocity((Vector3)arg);
1396 break;
1397
1398 // case changes.Acceleration:
1399 // changeacceleration((Vector3)arg);
1400 // break;
1401 // case changes.AngVelocity:
1402 // changeangvelocity((Vector3)arg);
1403 // break;
1404
1405 case changes.Force:
1406 changeForce((Vector3)arg);
1407 break;
1408
1409 case changes.Torque:
1410 changeSetTorque((Vector3)arg);
1411 break;
1412
1413 case changes.AddForce:
1414 changeAddForce((Vector3)arg);
1415 break;
1416
1417 case changes.AddAngForce:
1418 changeAddAngularForce((Vector3)arg);
1419 break;
1420
1421 case changes.AngLock:
1422 changeAngularLock((Vector3)arg);
1423 break;
1424
1425 case changes.Size:
1426 changeSize((Vector3)arg);
1427 break;
1428
1429 case changes.Momentum:
1430 changeMomentum((Vector3)arg);
1431 break;
1432/* not in use for now
1433 case changes.Shape:
1434 changeShape((PrimitiveBaseShape)arg);
1435 break;
1436
1437 case changes.CollidesWater:
1438 changeFloatOnWater((bool)arg);
1439 break;
1440
1441 case changes.VolumeDtc:
1442 changeVolumedetetion((bool)arg);
1443 break;
1444
1445 case changes.Physical:
1446 changePhysicsStatus((bool)arg);
1447 break;
1448
1449 case changes.Selected:
1450 changeSelectedStatus((bool)arg);
1451 break;
1452
1453 case changes.disabled:
1454 changeDisable((bool)arg);
1455 break;
1456
1457 case changes.building:
1458 changeBuilding((bool)arg);
1459 break;
1460*/
1461 case changes.Null:
1462 donullchange();
1463 break;
1464
1465 default:
1466 donullchange();
1467 break;
1468 }
1469 return false;
1470 }
1471
1472 public void AddChange(changes what, object arg)
1473 {
1474 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1475 }
1476 }
1477}
generated by cgit v1.1 at 2024-08-22 12:44:17 +0000