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