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