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