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