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.cs1451
1 files changed, 1451 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..793e281
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1451 @@
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 public ContactData AvatarContactData = new ContactData(10f, 0.3f);
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 AvatarContactData.mu = parent_scene.AvatarFriction;
172 AvatarContactData.bounce = parent_scene.AvatarBounce;
173
174 walkDivisor = walk_divisor;
175 runDivisor = rundivisor;
176
177 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
178 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
179
180 m_isPhysical = false; // current status: no ODE information exists
181
182 m_name = avName;
183
184 AddChange(changes.Add, null);
185 }
186
187 public override int PhysicsActorType
188 {
189 get { return (int)ActorTypes.Agent; }
190 set { return; }
191 }
192
193 public override ContactData ContactData
194 {
195 get { return AvatarContactData; }
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
358 /// <summary>
359 /// turn the PID controller on or off.
360 /// The PID Controller will turn on all by itself in many situations
361 /// </summary>
362 /// <param name="status"></param>
363 public void SetPidStatus(bool status)
364 {
365 m_pidControllerActive = status;
366 }
367
368 public override bool Stopped
369 {
370 get { return _zeroFlag; }
371 }
372
373 /// <summary>
374 /// This 'puts' an avatar somewhere in the physics space.
375 /// Not really a good choice unless you 'know' it's a good
376 /// spot otherwise you're likely to orbit the avatar.
377 /// </summary>
378 public override Vector3 Position
379 {
380 get { return _position; }
381 set
382 {
383 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
384 {
385 if (value.IsFinite())
386 {
387 if (value.Z > 9999999f)
388 {
389 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
390 }
391 if (value.Z < -100f)
392 {
393 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
394 }
395 AddChange(changes.Position, value);
396 }
397 else
398 {
399 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
400 }
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 }
642
643
644 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
645 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
646 // place that is safe to call this routine AvatarGeomAndBodyCreation.
647 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
648 {
649 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
650 if (CAPSULE_LENGTH <= 0)
651 {
652 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
653 CAPSULE_LENGTH = 0.01f;
654
655 }
656
657 if (CAPSULE_RADIUS <= 0)
658 {
659 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
660 CAPSULE_RADIUS = 0.01f;
661
662 }
663 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
664
665 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
666 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
667
668 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
669
670 m_mass = ShellMass.mass; // update mass
671
672 // rescale PID parameters
673 PID_D = _parent_scene.avPIDD;
674 PID_P = _parent_scene.avPIDP;
675
676 // rescale PID parameters so that this aren't affected by mass
677 // and so don't get unstable for some masses
678 // also scale by ode time step so you don't need to refix them
679
680 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
681 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
682 PID_P /= 50 * 80;
683 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
684
685 Body = d.BodyCreate(_parent_scene.world);
686
687 d.BodySetAutoDisableFlag(Body, false);
688 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
689
690 _position.X = npositionX;
691 _position.Y = npositionY;
692 _position.Z = npositionZ;
693
694 d.BodySetMass(Body, ref ShellMass);
695 d.GeomSetBody(Shell, Body);
696
697 // The purpose of the AMotor here is to keep the avatar's physical
698 // surrogate from rotating while moving
699 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
700 d.JointAttach(Amotor, Body, IntPtr.Zero);
701
702 d.JointSetAMotorMode(Amotor, 0);
703 d.JointSetAMotorNumAxes(Amotor, 3);
704 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
705 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
706 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
707
708 d.JointSetAMotorAngle(Amotor, 0, 0);
709 d.JointSetAMotorAngle(Amotor, 1, 0);
710 d.JointSetAMotorAngle(Amotor, 2, 0);
711
712 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
713 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
714 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
715 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
716 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
717 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
718
719 // These lowstops and high stops are effectively (no wiggle room)
720 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
721 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
722 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
723 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
724 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
725 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
726
727 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
728 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
729 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
730
731 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f);
732 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f);
733 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f);
734 }
735
736 /// <summary>
737 /// Destroys the avatar body and geom
738
739 private void AvatarGeomAndBodyDestroy()
740 {
741 // Kill the Amotor
742 if (Amotor != IntPtr.Zero)
743 {
744 d.JointDestroy(Amotor);
745 Amotor = IntPtr.Zero;
746 }
747
748 if (Body != IntPtr.Zero)
749 {
750 //kill the body
751 d.BodyDestroy(Body);
752 Body = IntPtr.Zero;
753 }
754
755 //kill the Geometry
756 if (Shell != IntPtr.Zero)
757 {
758 _parent_scene.geom_name_map.Remove(Shell);
759 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
760 d.GeomDestroy(Shell);
761 _parent_scene.geom_name_map.Remove(Shell);
762 Shell = IntPtr.Zero;
763 }
764 }
765
766 /// <summary>
767 /// Called from Simulate
768 /// This is the avatar's movement control + PID Controller
769 /// </summary>
770 /// <param name="timeStep"></param>
771 public void Move(float timeStep, List<OdeCharacter> defects)
772 {
773 // no lock; for now it's only called from within Simulate()
774
775 // If the PID Controller isn't active then we set our force
776 // calculating base velocity to the current position
777
778 if (Body == IntPtr.Zero)
779 return;
780
781 d.Vector3 dtmp;
782 d.BodyCopyPosition(Body, out dtmp);
783 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
784
785 // the Amotor still lets avatar rotation to drift during colisions
786 // so force it back to identity
787
788 d.Quaternion qtmp;
789 qtmp.W = 1;
790 qtmp.X = 0;
791 qtmp.Y = 0;
792 qtmp.Z = 0;
793 d.BodySetQuaternion(Body, ref qtmp);
794
795 if (m_pidControllerActive == false)
796 {
797 _zeroPosition = localpos;
798 }
799 //PidStatus = true;
800
801
802 if (!localpos.IsFinite())
803 {
804
805 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
806 defects.Add(this);
807 // _parent_scene.RemoveCharacter(this);
808
809 // destroy avatar capsule and related ODE data
810 AvatarGeomAndBodyDestroy();
811
812 return;
813 }
814
815 Vector3 vec = Vector3.Zero;
816 dtmp = d.BodyGetLinearVel(Body);
817 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
818
819 float movementdivisor = 1f;
820 //Ubit change divisions into multiplications below
821 if (!m_alwaysRun)
822 {
823 movementdivisor = 1 / walkDivisor;
824 }
825 else
826 {
827 movementdivisor = 1 / runDivisor;
828 }
829
830 // colide with land
831
832 d.AABB aabb;
833 d.GeomGetAABB(Shell, out aabb);
834 float chrminZ = aabb.MinZ;
835
836 Vector3 posch = localpos;
837
838 float ftmp;
839
840 if (flying)
841 {
842 ftmp = timeStep;
843 posch.X += vel.X * ftmp;
844 posch.Y += vel.Y * ftmp;
845 }
846
847 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
848 if (chrminZ < terrainheight)
849 {
850 float depth = terrainheight - chrminZ;
851 if (!flying)
852 {
853 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
854 }
855 else
856 vec.Z = depth * PID_P * 50;
857
858 /*
859 Vector3 vtmp;
860 vtmp.X = _target_velocity.X * timeStep;
861 vtmp.Y = _target_velocity.Y * timeStep;
862 // fake and avoid squares
863 float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
864 if (k > 0)
865 {
866 posch.X += vtmp.X;
867 posch.Y += vtmp.Y;
868 terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
869 k = 1 + Math.Abs(terrainheight) / k;
870 movementdivisor /= k;
871
872 if (k < 1)
873 k = 1;
874 }
875 */
876
877
878 if (depth < 0.1f)
879 {
880 m_iscolliding = true;
881 m_colliderfilter = 2;
882 m_iscollidingGround = true;
883
884 ContactPoint contact = new ContactPoint();
885 contact.PenetrationDepth = depth;
886 contact.Position.X = localpos.X;
887 contact.Position.Y = localpos.Y;
888 contact.Position.Z = chrminZ;
889 contact.SurfaceNormal.X = 0f;
890 contact.SurfaceNormal.Y = 0f;
891 contact.SurfaceNormal.Z = -1f;
892 AddCollisionEvent(0, contact);
893
894 vec.Z *= 0.5f;
895 }
896
897 else
898 m_iscollidingGround = false;
899 }
900 else
901 m_iscollidingGround = false;
902
903
904 // if velocity is zero, use position control; otherwise, velocity control
905 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
906 && m_iscolliding)
907 {
908 // keep track of where we stopped. No more slippin' & slidin'
909 if (!_zeroFlag)
910 {
911 _zeroFlag = true;
912 _zeroPosition = localpos;
913 }
914 if (m_pidControllerActive)
915 {
916 // We only want to deactivate the PID Controller if we think we want to have our surrogate
917 // react to the physics scene by moving it's position.
918 // Avatar to Avatar collisions
919 // Prim to avatar collisions
920
921 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
922 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
923 if (flying)
924 {
925 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
926 }
927 }
928 //PidStatus = true;
929 }
930 else
931 {
932 m_pidControllerActive = true;
933 _zeroFlag = false;
934
935 if (m_iscolliding)
936 {
937 if (!flying)
938 {
939 if (_target_velocity.Z > 0.0f)
940 {
941 // We're colliding with something and we're not flying but we're moving
942 // This means we're walking or running. JUMPING
943 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
944 }
945 // We're standing on something
946 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
947 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
948 }
949 else
950 {
951 // We're flying and colliding with something
952 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
953 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
954 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
955 }
956 }
957 else // ie not colliding
958 {
959 if (flying) //(!m_iscolliding && flying)
960 {
961 // we're in mid air suspended
962 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
963 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
964 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
965 }
966
967 else
968 {
969 // we're not colliding and we're not flying so that means we're falling!
970 // m_iscolliding includes collisions with the ground.
971
972 // d.Vector3 pos = d.BodyGetPosition(Body);
973 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
974 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
975 }
976 }
977 }
978
979 if (flying)
980 {
981 vec.Z -= _parent_scene.gravityz * m_mass;
982
983 //Added for auto fly height. Kitto Flora
984 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
985
986 if (localpos.Z < target_altitude)
987 {
988 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
989 }
990 // end add Kitto Flora
991 }
992
993 if (vec.IsFinite())
994 {
995 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
996 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
997 }
998 else
999 {
1000 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1001 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1002 defects.Add(this);
1003 // _parent_scene.RemoveCharacter(this);
1004 // destroy avatar capsule and related ODE data
1005 AvatarGeomAndBodyDestroy();
1006 }
1007 }
1008
1009 /// <summary>
1010 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1011 /// </summary>
1012 public void UpdatePositionAndVelocity()
1013 {
1014 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1015 if (Body == IntPtr.Zero)
1016 return;
1017
1018 d.Vector3 vec;
1019 try
1020 {
1021 d.BodyCopyPosition(Body, out vec);
1022 }
1023 catch (NullReferenceException)
1024 {
1025 bad = true;
1026 _parent_scene.BadCharacter(this);
1027 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1028 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1029 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1030 }
1031
1032 _position.X = vec.X;
1033 _position.Y = vec.Y;
1034 _position.Z = vec.Z;
1035
1036 bool fixbody = false;
1037
1038 if (_position.X < 0.0f)
1039 {
1040 fixbody = true;
1041 _position.X = 0.1f;
1042 }
1043 else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
1044 {
1045 fixbody = true;
1046 _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
1047 }
1048
1049 if (_position.Y < 0.0f)
1050 {
1051 fixbody = true;
1052 _position.Y = 0.1f;
1053 }
1054 else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
1055 {
1056 fixbody = true;
1057 _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
1058 }
1059
1060 if (fixbody)
1061 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1062
1063 // Did we move last? = zeroflag
1064 // This helps keep us from sliding all over
1065/*
1066 if (_zeroFlag)
1067 {
1068 _velocity.X = 0.0f;
1069 _velocity.Y = 0.0f;
1070 _velocity.Z = 0.0f;
1071
1072 // Did we send out the 'stopped' message?
1073 if (!m_lastUpdateSent)
1074 {
1075 m_lastUpdateSent = true;
1076 base.RequestPhysicsterseUpdate();
1077 }
1078 }
1079 else
1080 {
1081 m_lastUpdateSent = false;
1082 */
1083 try
1084 {
1085 vec = d.BodyGetLinearVel(Body);
1086 }
1087 catch (NullReferenceException)
1088 {
1089 vec.X = _velocity.X;
1090 vec.Y = _velocity.Y;
1091 vec.Z = _velocity.Z;
1092 }
1093 _velocity.X = (vec.X);
1094 _velocity.Y = (vec.Y);
1095 _velocity.Z = (vec.Z);
1096 // }
1097 }
1098
1099 /// <summary>
1100 /// Cleanup the things we use in the scene.
1101 /// </summary>
1102 public void Destroy()
1103 {
1104 AddChange(changes.Remove, null);
1105 }
1106
1107 public override void CrossingFailure()
1108 {
1109 }
1110
1111 public override Vector3 PIDTarget { set { return; } }
1112 public override bool PIDActive { set { return; } }
1113 public override float PIDTau { set { return; } }
1114
1115 public override float PIDHoverHeight { set { return; } }
1116 public override bool PIDHoverActive { set { return; } }
1117 public override PIDHoverType PIDHoverType { set { return; } }
1118 public override float PIDHoverTau { set { return; } }
1119
1120 public override Quaternion APIDTarget { set { return; } }
1121
1122 public override bool APIDActive { set { return; } }
1123
1124 public override float APIDStrength { set { return; } }
1125
1126 public override float APIDDamping { set { return; } }
1127
1128
1129 public override void SubscribeEvents(int ms)
1130 {
1131 m_requestedUpdateFrequency = ms;
1132 m_eventsubscription = ms;
1133 _parent_scene.AddCollisionEventReporting(this);
1134 m_haseventsubscription = true;
1135 }
1136
1137 public override void UnSubscribeEvents()
1138 {
1139 m_haseventsubscription = false;
1140 _parent_scene.RemoveCollisionEventReporting(this);
1141 m_requestedUpdateFrequency = 0;
1142 m_eventsubscription = 0;
1143 }
1144
1145 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1146 {
1147 if (m_haseventsubscription)
1148 {
1149 // m_log.DebugFormat(
1150 // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
1151
1152 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1153 }
1154 }
1155
1156 public void SendCollisions()
1157 {
1158 if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency)
1159 {
1160 if (CollisionEventsThisFrame != null)
1161 {
1162 base.SendCollisionUpdate(CollisionEventsThisFrame);
1163 }
1164 CollisionEventsThisFrame = new CollisionEventUpdate();
1165 m_eventsubscription = 0;
1166 }
1167 }
1168
1169 public override bool SubscribedEvents()
1170 {
1171 return m_haseventsubscription;
1172 }
1173
1174 private void changePhysicsStatus(bool NewStatus)
1175 {
1176 if (NewStatus != m_isPhysical)
1177 {
1178 if (NewStatus)
1179 {
1180 // Create avatar capsule and related ODE data
1181 if ((Shell != IntPtr.Zero))
1182 {
1183 // a lost shell ?
1184 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1185 + (Shell != IntPtr.Zero ? "Shell " : "")
1186 + (Body != IntPtr.Zero ? "Body " : "")
1187 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1188 AvatarGeomAndBodyDestroy();
1189 }
1190
1191 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1192 _parent_scene.geom_name_map[Shell] = m_name;
1193 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1194 _parent_scene.AddCharacter(this);
1195 }
1196 else
1197 {
1198 _parent_scene.RemoveCharacter(this);
1199 // destroy avatar capsule and related ODE data
1200 AvatarGeomAndBodyDestroy();
1201 }
1202
1203 m_isPhysical = NewStatus;
1204 }
1205 }
1206
1207 private void changeAdd()
1208 {
1209 changePhysicsStatus(true);
1210 }
1211
1212 private void changeRemove()
1213 {
1214 changePhysicsStatus(false);
1215 }
1216
1217 private void changeShape(PrimitiveBaseShape arg)
1218 {
1219 }
1220
1221 private void changeSize(Vector3 Size)
1222 {
1223 if (Size.IsFinite())
1224 {
1225 float caplen = Size.Z;
1226
1227 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1228
1229 if (caplen != CAPSULE_LENGTH)
1230 {
1231 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1232 {
1233 AvatarGeomAndBodyDestroy();
1234
1235 float prevCapsule = CAPSULE_LENGTH;
1236 CAPSULE_LENGTH = caplen;
1237
1238 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1239 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
1240
1241 Velocity = Vector3.Zero;
1242
1243 _parent_scene.geom_name_map[Shell] = m_name;
1244 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1245 }
1246 else
1247 {
1248 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1249 + (Shell == IntPtr.Zero ? "Shell " : "")
1250 + (Body == IntPtr.Zero ? "Body " : "")
1251 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1252 }
1253 }
1254
1255 m_pidControllerActive = true;
1256 }
1257 else
1258 {
1259 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1260 }
1261 }
1262
1263 private void changePosition( Vector3 newPos)
1264 {
1265 if (Body != IntPtr.Zero)
1266 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1267 _position = newPos;
1268 }
1269
1270 private void changeOrientation(Quaternion newOri)
1271 {
1272 }
1273
1274 private void changeVelocity(Vector3 newVel)
1275 {
1276 m_pidControllerActive = true;
1277 _target_velocity = newVel;
1278 }
1279
1280 private void changeSetTorque(Vector3 newTorque)
1281 {
1282 }
1283
1284 private void changeAddForce(Vector3 newForce)
1285 {
1286 }
1287
1288 private void changeAddAngularForce(Vector3 arg)
1289 {
1290 }
1291
1292 private void changeAngularLock(Vector3 arg)
1293 {
1294 }
1295
1296 private void changeFloatOnWater(bool arg)
1297 {
1298 }
1299
1300 private void changeVolumedetetion(bool arg)
1301 {
1302 }
1303
1304 private void changeSelectedStatus(bool arg)
1305 {
1306 }
1307
1308 private void changeDisable(bool arg)
1309 {
1310 }
1311
1312 private void changeBuilding(bool arg)
1313 {
1314 }
1315
1316 private void changeForce(Vector3 newForce)
1317 {
1318 m_pidControllerActive = false;
1319 if (Body != IntPtr.Zero)
1320 {
1321 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1322 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1323 }
1324 }
1325
1326 private void donullchange()
1327 {
1328 }
1329
1330 public bool DoAChange(changes what, object arg)
1331 {
1332 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1333 {
1334 return false;
1335 }
1336
1337 // nasty switch
1338 switch (what)
1339 {
1340 case changes.Add:
1341 changeAdd();
1342 break;
1343 case changes.Remove:
1344 changeRemove();
1345 break;
1346
1347 case changes.Position:
1348 changePosition((Vector3)arg);
1349 break;
1350
1351 case changes.Orientation:
1352 changeOrientation((Quaternion)arg);
1353 break;
1354
1355 case changes.PosOffset:
1356 donullchange();
1357 break;
1358
1359 case changes.OriOffset:
1360 donullchange();
1361 break;
1362
1363 case changes.Velocity:
1364 changeVelocity((Vector3)arg);
1365 break;
1366
1367 // case changes.Acceleration:
1368 // changeacceleration((Vector3)arg);
1369 // break;
1370 // case changes.AngVelocity:
1371 // changeangvelocity((Vector3)arg);
1372 // break;
1373
1374 case changes.Force:
1375 changeForce((Vector3)arg);
1376 break;
1377
1378 case changes.Torque:
1379 changeSetTorque((Vector3)arg);
1380 break;
1381
1382 case changes.AddForce:
1383 changeAddForce((Vector3)arg);
1384 break;
1385
1386 case changes.AddAngForce:
1387 changeAddAngularForce((Vector3)arg);
1388 break;
1389
1390 case changes.AngLock:
1391 changeAngularLock((Vector3)arg);
1392 break;
1393
1394 case changes.Size:
1395 changeSize((Vector3)arg);
1396 break;
1397/* not in use for now
1398 case changes.Shape:
1399 changeShape((PrimitiveBaseShape)arg);
1400 break;
1401
1402 case changes.CollidesWater:
1403 changeFloatOnWater((bool)arg);
1404 break;
1405
1406 case changes.VolumeDtc:
1407 changeVolumedetetion((bool)arg);
1408 break;
1409
1410 case changes.Physical:
1411 changePhysicsStatus((bool)arg);
1412 break;
1413
1414 case changes.Selected:
1415 changeSelectedStatus((bool)arg);
1416 break;
1417
1418 case changes.disabled:
1419 changeDisable((bool)arg);
1420 break;
1421
1422 case changes.building:
1423 changeBuilding((bool)arg);
1424 break;
1425*/
1426 case changes.Null:
1427 donullchange();
1428 break;
1429
1430 default:
1431 donullchange();
1432 break;
1433 }
1434 return false;
1435 }
1436
1437 public void AddChange(changes what, object arg)
1438 {
1439 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1440 }
1441
1442
1443 internal void AddCollisionFrameTime(int p)
1444 {
1445 // protect it from overflow crashing
1446 if (m_eventsubscription + p >= int.MaxValue)
1447 m_eventsubscription = 0;
1448 m_eventsubscription += p;
1449 }
1450 }
1451}
generated by cgit v1.1 at 2024-11-09 01:32:01 +0000