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