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