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