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