aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs1847
1 files changed, 1847 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs
new file mode 100644
index 0000000..e461dcd
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs
@@ -0,0 +1,1847 @@
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.PhysicsModules.SharedBase;
38using log4net;
39
40namespace OpenSim.Region.PhysicsModules.UbitOde
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 Vector3 _velocity;
78 private Vector3 _target_velocity;
79 private Vector3 _acceleration;
80 private Vector3 m_rotationalVelocity;
81 private Vector3 m_size;
82 private Vector3 m_collideNormal;
83 private Quaternion m_orientation;
84 private Quaternion m_orientation2D;
85 private float m_mass = 80f;
86 public float m_density = 60f;
87 private bool m_pidControllerActive = true;
88
89 const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80))
90 const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80))
91 public float PID_D;
92 public float PID_P;
93
94 private float timeStep;
95 private float invtimeStep;
96
97 private float m_feetOffset = 0;
98 private float feetOff = 0;
99 private float boneOff = 0;
100 private float AvaAvaSizeXsq = 0.3f;
101 private float AvaAvaSizeYsq = 0.2f;
102
103 public float walkDivisor = 1.3f;
104 public float runDivisor = 0.8f;
105 private bool flying = false;
106 private bool m_iscolliding = false;
107 private bool m_iscollidingGround = false;
108 private bool m_iscollidingObj = false;
109 private bool m_alwaysRun = false;
110
111 private bool _zeroFlag = false;
112
113
114 private uint m_localID = 0;
115 public bool m_returnCollisions = false;
116 // taints and their non-tainted counterparts
117 public bool m_isPhysical = false; // the current physical status
118 public float MinimumGroundFlightOffset = 3f;
119
120 private float m_buoyancy = 0f;
121
122 private bool m_freemove = false;
123 // private CollisionLocker ode;
124
125// private string m_name = String.Empty;
126 // other filter control
127 int m_colliderfilter = 0;
128 int m_colliderGroundfilter = 0;
129 int m_colliderObjectfilter = 0;
130
131 // Default we're a Character
132 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
133
134 // Default, Collide with Other Geometries, spaces, bodies and characters.
135 private CollisionCategories m_collisionFlags = (CollisionCategories.Character
136 | CollisionCategories.Geom
137 | CollisionCategories.VolumeDtc
138 );
139 // we do land collisions not ode | CollisionCategories.Land);
140 public IntPtr Body = IntPtr.Zero;
141 private OdeScene _parent_scene;
142 private IntPtr capsule = IntPtr.Zero;
143 public IntPtr collider = IntPtr.Zero;
144
145 public IntPtr Amotor = IntPtr.Zero;
146
147 public d.Mass ShellMass;
148
149 public int m_eventsubscription = 0;
150 private int m_cureventsubscription = 0;
151 private CollisionEventUpdate CollisionEventsThisFrame = null;
152 private bool SentEmptyCollisionsEvent;
153
154 // unique UUID of this character object
155 public UUID m_uuid;
156 public bool bad = false;
157
158 float mu;
159
160 public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor)
161 {
162 m_uuid = UUID.Random();
163 m_localID = localID;
164
165 timeStep = parent_scene.ODE_STEPSIZE;
166 invtimeStep = 1 / timeStep;
167
168 if (pos.IsFinite())
169 {
170 if (pos.Z > 99999f)
171 {
172 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
173 }
174 if (pos.Z < -100f) // shouldn't this be 0 ?
175 {
176 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
177 }
178 _position = pos;
179 }
180 else
181 {
182 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
183 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
184 }
185
186 _parent_scene = parent_scene;
187
188
189 m_size.X = pSize.X;
190 m_size.Y = pSize.Y;
191 m_size.Z = pSize.Z;
192
193 if(m_size.X <0.01f)
194 m_size.X = 0.01f;
195 if(m_size.Y <0.01f)
196 m_size.Y = 0.01f;
197 if(m_size.Z <0.01f)
198 m_size.Z = 0.01f;
199
200 m_feetOffset = pfeetOffset;
201 m_orientation = Quaternion.Identity;
202 m_orientation2D = Quaternion.Identity;
203 m_density = density;
204
205 // force lower density for testing
206 m_density = 3.0f;
207
208 mu = parent_scene.AvatarFriction;
209
210 walkDivisor = walk_divisor;
211 runDivisor = rundivisor;
212
213 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default
214
215 PID_D = basePID_D * m_mass * invtimeStep;
216 PID_P = basePID_P * m_mass * invtimeStep;
217
218 m_isPhysical = false; // current status: no ODE information exists
219
220 Name = avName;
221
222 AddChange(changes.Add, null);
223 }
224
225 public override int PhysicsActorType
226 {
227 get { return (int)ActorTypes.Agent; }
228 set { return; }
229 }
230
231 public override void getContactData(ref ContactData cdata)
232 {
233 cdata.mu = mu;
234 cdata.bounce = 0;
235 cdata.softcolide = false;
236 }
237
238 public override bool Building { get; set; }
239
240 /// <summary>
241 /// If this is set, the avatar will move faster
242 /// </summary>
243 public override bool SetAlwaysRun
244 {
245 get { return m_alwaysRun; }
246 set { m_alwaysRun = value; }
247 }
248
249 public override uint LocalID
250 {
251 get { return m_localID; }
252 set { m_localID = value; }
253 }
254
255 public override PhysicsActor ParentActor
256 {
257 get { return (PhysicsActor)this; }
258 }
259
260 public override bool Grabbed
261 {
262 set { return; }
263 }
264
265 public override bool Selected
266 {
267 set { return; }
268 }
269
270 public override float Buoyancy
271 {
272 get { return m_buoyancy; }
273 set { m_buoyancy = value; }
274 }
275
276 public override bool FloatOnWater
277 {
278 set { return; }
279 }
280
281 public override bool IsPhysical
282 {
283 get { return m_isPhysical; }
284 set { return; }
285 }
286
287 public override bool ThrottleUpdates
288 {
289 get { return false; }
290 set { return; }
291 }
292
293 public override bool Flying
294 {
295 get { return flying; }
296 set
297 {
298 flying = value;
299// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
300 }
301 }
302
303 /// <summary>
304 /// Returns if the avatar is colliding in general.
305 /// This includes the ground and objects and avatar.
306 /// </summary>
307 public override bool IsColliding
308 {
309 get { return (m_iscolliding || m_iscollidingGround); }
310 set
311 {
312 if (value)
313 {
314 m_colliderfilter += 3;
315 if (m_colliderfilter > 3)
316 m_colliderfilter = 3;
317 }
318 else
319 {
320 m_colliderfilter--;
321 if (m_colliderfilter < 0)
322 m_colliderfilter = 0;
323 }
324
325 if (m_colliderfilter == 0)
326 m_iscolliding = false;
327 else
328 {
329 m_pidControllerActive = true;
330 m_iscolliding = true;
331 m_freemove = false;
332 }
333 }
334 }
335
336 /// <summary>
337 /// Returns if an avatar is colliding with the ground
338 /// </summary>
339 public override bool CollidingGround
340 {
341 get { return m_iscollidingGround; }
342 set
343 {
344/* we now control this
345 if (value)
346 {
347 m_colliderGroundfilter += 2;
348 if (m_colliderGroundfilter > 2)
349 m_colliderGroundfilter = 2;
350 }
351 else
352 {
353 m_colliderGroundfilter--;
354 if (m_colliderGroundfilter < 0)
355 m_colliderGroundfilter = 0;
356 }
357
358 if (m_colliderGroundfilter == 0)
359 m_iscollidingGround = false;
360 else
361 m_iscollidingGround = true;
362 */
363 }
364
365 }
366
367 /// <summary>
368 /// Returns if the avatar is colliding with an object
369 /// </summary>
370 public override bool CollidingObj
371 {
372 get { return m_iscollidingObj; }
373 set
374 {
375 // Ubit filter this also
376 if (value)
377 {
378 m_colliderObjectfilter += 2;
379 if (m_colliderObjectfilter > 2)
380 m_colliderObjectfilter = 2;
381 }
382 else
383 {
384 m_colliderObjectfilter--;
385 if (m_colliderObjectfilter < 0)
386 m_colliderObjectfilter = 0;
387 }
388
389 if (m_colliderObjectfilter == 0)
390 m_iscollidingObj = false;
391 else
392 m_iscollidingObj = true;
393
394// m_iscollidingObj = value;
395
396 if (m_iscollidingObj)
397 m_pidControllerActive = false;
398 else
399 m_pidControllerActive = true;
400 }
401 }
402
403 /// <summary>
404 /// turn the PID controller on or off.
405 /// The PID Controller will turn on all by itself in many situations
406 /// </summary>
407 /// <param name="status"></param>
408 public void SetPidStatus(bool status)
409 {
410 m_pidControllerActive = status;
411 }
412
413 public override bool Stopped
414 {
415 get { return _zeroFlag; }
416 }
417
418 /// <summary>
419 /// This 'puts' an avatar somewhere in the physics space.
420 /// Not really a good choice unless you 'know' it's a good
421 /// spot otherwise you're likely to orbit the avatar.
422 /// </summary>
423 public override Vector3 Position
424 {
425 get { return _position; }
426 set
427 {
428 if (value.IsFinite())
429 {
430 if (value.Z > 9999999f)
431 {
432 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
433 }
434 if (value.Z < -100f)
435 {
436 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
437 }
438 AddChange(changes.Position, value);
439 }
440 else
441 {
442 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
443 }
444 }
445 }
446
447 public override Vector3 RotationalVelocity
448 {
449 get { return m_rotationalVelocity; }
450 set { m_rotationalVelocity = value; }
451 }
452
453 /// <summary>
454 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
455 /// and use it to offset landings properly
456 /// </summary>
457 public override Vector3 Size
458 {
459 get
460 {
461 return m_size;
462 }
463 set
464 {
465 if (value.IsFinite())
466 {
467 if(value.X <0.01f)
468 value.X = 0.01f;
469 if(value.Y <0.01f)
470 value.Y = 0.01f;
471 if(value.Z <0.01f)
472 value.Z = 0.01f;
473
474 AddChange(changes.Size, value);
475 }
476 else
477 {
478 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
479 }
480 }
481 }
482
483 public override void setAvatarSize(Vector3 size, float feetOffset)
484 {
485 if (size.IsFinite())
486 {
487 if (size.X < 0.01f)
488 size.X = 0.01f;
489 if (size.Y < 0.01f)
490 size.Y = 0.01f;
491 if (size.Z < 0.01f)
492 size.Z = 0.01f;
493
494 strAvatarSize st = new strAvatarSize();
495 st.size = size;
496 st.offset = feetOffset;
497 AddChange(changes.AvatarSize, st);
498 }
499 else
500 {
501 m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character");
502 }
503
504 }
505 /// <summary>
506 /// This creates the Avatar's physical Surrogate at the position supplied
507 /// </summary>
508 /// <param name="npositionX"></param>
509 /// <param name="npositionY"></param>
510 /// <param name="npositionZ"></param>
511
512 //
513 /// <summary>
514 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
515 /// This may be used in calculations in the scene/scenepresence
516 /// </summary>
517 public override float Mass
518 {
519 get
520 {
521 return m_mass;
522 }
523 }
524 public override void link(PhysicsActor obj)
525 {
526
527 }
528
529 public override void delink()
530 {
531
532 }
533
534 public override void LockAngularMotion(Vector3 axis)
535 {
536
537 }
538
539
540 public override Vector3 Force
541 {
542 get { return _target_velocity; }
543 set { return; }
544 }
545
546 public override int VehicleType
547 {
548 get { return 0; }
549 set { return; }
550 }
551
552 public override void VehicleFloatParam(int param, float value)
553 {
554
555 }
556
557 public override void VehicleVectorParam(int param, Vector3 value)
558 {
559
560 }
561
562 public override void VehicleRotationParam(int param, Quaternion rotation)
563 {
564
565 }
566
567 public override void VehicleFlags(int param, bool remove)
568 {
569
570 }
571
572 public override void SetVolumeDetect(int param)
573 {
574
575 }
576
577 public override Vector3 CenterOfMass
578 {
579 get
580 {
581 Vector3 pos = _position;
582 return pos;
583 }
584 }
585
586 public override Vector3 GeometricCenter
587 {
588 get
589 {
590 Vector3 pos = _position;
591 return pos;
592 }
593 }
594
595 public override PrimitiveBaseShape Shape
596 {
597 set { return; }
598 }
599
600 public override Vector3 Velocity
601 {
602 get
603 {
604 return _velocity;
605 }
606 set
607 {
608 if (value.IsFinite())
609 {
610 AddChange(changes.Velocity, value);
611 }
612 else
613 {
614 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
615 }
616 }
617 }
618
619 public override Vector3 Torque
620 {
621 get { return Vector3.Zero; }
622 set { return; }
623 }
624
625 public override float CollisionScore
626 {
627 get { return 0f; }
628 set { }
629 }
630
631 public override bool Kinematic
632 {
633 get { return false; }
634 set { }
635 }
636
637 public override Quaternion Orientation
638 {
639 get { return m_orientation; }
640 set
641 {
642// fakeori = value;
643// givefakeori++;
644 value.Normalize();
645 AddChange(changes.Orientation, value);
646 }
647 }
648
649 public override Vector3 Acceleration
650 {
651 get { return _acceleration; }
652 set { }
653 }
654
655 public void SetAcceleration(Vector3 accel)
656 {
657 m_pidControllerActive = true;
658 _acceleration = accel;
659 }
660
661 /// <summary>
662 /// Adds the force supplied to the Target Velocity
663 /// The PID controller takes this target velocity and tries to make it a reality
664 /// </summary>
665 /// <param name="force"></param>
666 public override void AddForce(Vector3 force, bool pushforce)
667 {
668 if (force.IsFinite())
669 {
670 if (pushforce)
671 {
672 AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f));
673 }
674 else
675 {
676 AddChange(changes.Velocity, force);
677 }
678 }
679 else
680 {
681 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
682 }
683 //m_lastUpdateSent = false;
684 }
685
686 public override void AddAngularForce(Vector3 force, bool pushforce)
687 {
688
689 }
690
691 public override void SetMomentum(Vector3 momentum)
692 {
693 if (momentum.IsFinite())
694 AddChange(changes.Momentum, momentum);
695 }
696
697
698 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
699 {
700 // sizes one day should came from visual parameters
701 float sx = m_size.X;
702 float sy = m_size.Y;
703 float sz = m_size.Z;
704
705 float bot = -sz * 0.5f + m_feetOffset;
706 boneOff = bot + 0.3f;
707
708 float feetsz = sz * 0.45f;
709 if (feetsz > 0.6f)
710 feetsz = 0.6f;
711
712 feetOff = bot + feetsz;
713
714 AvaAvaSizeXsq = 0.4f * sx;
715 AvaAvaSizeXsq *= AvaAvaSizeXsq;
716 AvaAvaSizeYsq = 0.5f * sy;
717 AvaAvaSizeYsq *= AvaAvaSizeYsq;
718
719 _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace);
720
721 collider = d.HashSpaceCreate(_parent_scene.CharsSpace);
722 d.HashSpaceSetLevels(collider, -4, 3);
723 d.SpaceSetSublevel(collider, 3);
724 d.SpaceSetCleanup(collider, false);
725 d.GeomSetCategoryBits(collider, (uint)m_collisionCategories);
726 d.GeomSetCollideBits(collider, (uint)m_collisionFlags);
727
728 float r = m_size.X;
729 if (m_size.Y > r)
730 r = m_size.Y;
731 float l = m_size.Z - r;
732 r *= 0.5f;
733
734 capsule = d.CreateCapsule(collider, r, l);
735
736 m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass
737
738 d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z);
739
740 PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE;
741 PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE;
742
743 Body = d.BodyCreate(_parent_scene.world);
744
745 _zeroFlag = false;
746 m_pidControllerActive = true;
747 m_freemove = false;
748
749 _velocity = Vector3.Zero;
750
751 d.BodySetAutoDisableFlag(Body, false);
752 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
753
754 _position.X = npositionX;
755 _position.Y = npositionY;
756 _position.Z = npositionZ;
757
758 d.BodySetMass(Body, ref ShellMass);
759 d.GeomSetBody(capsule, Body);
760
761 // The purpose of the AMotor here is to keep the avatar's physical
762 // surrogate from rotating while moving
763 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
764 d.JointAttach(Amotor, Body, IntPtr.Zero);
765
766 d.JointSetAMotorMode(Amotor, 0);
767 d.JointSetAMotorNumAxes(Amotor, 3);
768 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
769 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
770 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
771
772 d.JointSetAMotorAngle(Amotor, 0, 0);
773 d.JointSetAMotorAngle(Amotor, 1, 0);
774 d.JointSetAMotorAngle(Amotor, 2, 0);
775
776 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
777 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
778 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
779 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
780 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
781 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
782
783 // These lowstops and high stops are effectively (no wiggle room)
784 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
785 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
786 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
787 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
788 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
789 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
790
791 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
792 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
793 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
794
795 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f);
796 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f);
797 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f);
798 }
799
800 /// <summary>
801 /// Destroys the avatar body and geom
802
803 private void AvatarGeomAndBodyDestroy()
804 {
805 // Kill the Amotor
806 if (Amotor != IntPtr.Zero)
807 {
808 d.JointDestroy(Amotor);
809 Amotor = IntPtr.Zero;
810 }
811
812 if (Body != IntPtr.Zero)
813 {
814 //kill the body
815 d.BodyDestroy(Body);
816 Body = IntPtr.Zero;
817 }
818
819 //kill the Geoms
820 if (capsule != IntPtr.Zero)
821 {
822 _parent_scene.actor_name_map.Remove(capsule);
823 _parent_scene.waitForSpaceUnlock(collider);
824 d.GeomDestroy(capsule);
825 capsule = IntPtr.Zero;
826 }
827
828 if (collider != IntPtr.Zero)
829 {
830 d.SpaceDestroy(collider);
831 collider = IntPtr.Zero;
832 }
833
834 }
835
836 //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z
837 public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot)
838 {
839 float sin = 2.0f * rot.Z * rot.W;
840 float cos = rot.W * rot.W - rot.Z * rot.Z;
841 float tx = x;
842
843 x = tx * cos - y * sin;
844 y = tx * sin + y * cos;
845 }
846 public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
847 {
848 float tx = x;
849 x = tx * cos - y * sin;
850 y = tx * sin + y * cos;
851 }
852 public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos)
853 {
854 float tx = x;
855 x = tx * cos + y * sin;
856 y = -tx * sin + y * cos;
857 }
858
859 public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot)
860 {
861 float sin = - 2.0f * rot.Z * rot.W;
862 float cos = rot.W * rot.W - rot.Z * rot.Z;
863 float tx = x;
864
865 x = tx * cos - y * sin;
866 y = tx * sin + y * cos;
867 }
868
869 public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact,
870 ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision)
871 {
872 feetcollision = false;
873 useAltcontact = false;
874
875 if (me == capsule)
876 {
877 Vector3 offset;
878
879 float h = contact.pos.Z - _position.Z;
880 offset.Z = h - feetOff;
881
882 offset.X = contact.pos.X - _position.X;
883 offset.Y = contact.pos.Y - _position.Y;
884
885 d.GeomClassID gtype = d.GeomGetClass(other);
886 if (gtype == d.GeomClassID.CapsuleClass)
887 {
888 Vector3 roff = offset * Quaternion.Inverse(m_orientation2D);
889 float r = roff.X *roff.X / AvaAvaSizeXsq;
890 r += (roff.Y * roff.Y) / AvaAvaSizeYsq;
891 if (r > 1.0f)
892 return false;
893
894 float dp = 1.0f -(float)Math.Sqrt((double)r);
895 if (dp > 0.05f)
896 dp = 0.05f;
897
898 contact.depth = dp;
899
900 if (offset.Z < 0)
901 {
902 feetcollision = true;
903 if (h < boneOff)
904 {
905 m_collideNormal.X = contact.normal.X;
906 m_collideNormal.Y = contact.normal.Y;
907 m_collideNormal.Z = contact.normal.Z;
908 IsColliding = true;
909 }
910 }
911 return true;
912 }
913/*
914 d.AABB aabb;
915 d.GeomGetAABB(other,out aabb);
916 float othertop = aabb.MaxZ - _position.Z;
917*/
918// if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f)
919 if (offset.Z > 0 || contact.normal.Z > 0.35f)
920 {
921 if (offset.Z <= 0)
922 {
923 feetcollision = true;
924 if (h < boneOff)
925 {
926 m_collideNormal.X = contact.normal.X;
927 m_collideNormal.Y = contact.normal.Y;
928 m_collideNormal.Z = contact.normal.Z;
929 IsColliding = true;
930 }
931 }
932 return true;
933 }
934
935 altContact = contact;
936 useAltcontact = true;
937
938 offset.Z -= 0.2f;
939
940 offset.Normalize();
941
942 if (contact.depth > 0.1f)
943 contact.depth = 0.1f;
944
945 if (reverse)
946 {
947 altContact.normal.X = offset.X;
948 altContact.normal.Y = offset.Y;
949 altContact.normal.Z = offset.Z;
950 }
951 else
952 {
953 altContact.normal.X = -offset.X;
954 altContact.normal.Y = -offset.Y;
955 altContact.normal.Z = -offset.Z;
956 }
957
958 feetcollision = true;
959 if (h < boneOff)
960 {
961 m_collideNormal.X = contact.normal.X;
962 m_collideNormal.Y = contact.normal.Y;
963 m_collideNormal.Z = contact.normal.Z;
964 IsColliding = true;
965 }
966 return true;
967 }
968 return false;
969 }
970
971 /// <summary>
972 /// Called from Simulate
973 /// This is the avatar's movement control + PID Controller
974 /// </summary>
975 /// <param name="timeStep"></param>
976 public void Move(List<OdeCharacter> defects)
977 {
978 if (Body == IntPtr.Zero)
979 return;
980
981 d.Vector3 dtmp = d.BodyGetPosition(Body);
982 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
983
984 // the Amotor still lets avatar rotation to drift during colisions
985 // so force it back to identity
986
987 d.Quaternion qtmp;
988 qtmp.W = m_orientation2D.W;
989 qtmp.X = m_orientation2D.X;
990 qtmp.Y = m_orientation2D.Y;
991 qtmp.Z = m_orientation2D.Z;
992 d.BodySetQuaternion(Body, ref qtmp);
993
994 if (m_pidControllerActive == false)
995 {
996 _zeroPosition = localpos;
997 }
998
999 if (!localpos.IsFinite())
1000 {
1001 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1002 defects.Add(this);
1003 // _parent_scene.RemoveCharacter(this);
1004
1005 // destroy avatar capsule and related ODE data
1006 AvatarGeomAndBodyDestroy();
1007 return;
1008 }
1009
1010 // check outbounds forcing to be in world
1011 bool fixbody = false;
1012 if (localpos.X < 0.0f)
1013 {
1014 fixbody = true;
1015 localpos.X = 0.1f;
1016 }
1017 else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f)
1018 {
1019 fixbody = true;
1020 localpos.X = _parent_scene.WorldExtents.X - 0.1f;
1021 }
1022 if (localpos.Y < 0.0f)
1023 {
1024 fixbody = true;
1025 localpos.Y = 0.1f;
1026 }
1027 else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1)
1028 {
1029 fixbody = true;
1030 localpos.Y = _parent_scene.WorldExtents.Y - 0.1f;
1031 }
1032 if (fixbody)
1033 {
1034 m_freemove = false;
1035 d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z);
1036 }
1037
1038 float breakfactor;
1039
1040 Vector3 vec = Vector3.Zero;
1041 dtmp = d.BodyGetLinearVel(Body);
1042 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
1043 float velLengthSquared = vel.LengthSquared();
1044
1045
1046 Vector3 ctz = _target_velocity;
1047
1048 float movementdivisor = 1f;
1049 //Ubit change divisions into multiplications below
1050 if (!m_alwaysRun)
1051 movementdivisor = 1 / walkDivisor;
1052 else
1053 movementdivisor = 1 / runDivisor;
1054
1055 ctz.X *= movementdivisor;
1056 ctz.Y *= movementdivisor;
1057
1058 //******************************************
1059 // colide with land
1060
1061 d.AABB aabb;
1062// d.GeomGetAABB(feetbox, out aabb);
1063 d.GeomGetAABB(capsule, out aabb);
1064 float chrminZ = aabb.MinZ; // move up a bit
1065 Vector3 posch = localpos;
1066
1067 float ftmp;
1068
1069 if (flying)
1070 {
1071 ftmp = timeStep;
1072 posch.X += vel.X * ftmp;
1073 posch.Y += vel.Y * ftmp;
1074 }
1075
1076 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
1077 if (chrminZ < terrainheight)
1078 {
1079 if (ctz.Z < 0)
1080 ctz.Z = 0;
1081
1082 Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y);
1083 float depth = terrainheight - chrminZ;
1084
1085 vec.Z = depth * PID_P * 50;
1086
1087 if (!flying)
1088 vec.Z += -vel.Z * PID_D;
1089
1090 if (depth < 0.2f)
1091 {
1092 m_colliderGroundfilter++;
1093 if (m_colliderGroundfilter > 2)
1094 {
1095 m_iscolliding = true;
1096 m_colliderfilter = 2;
1097
1098 if (m_colliderGroundfilter > 10)
1099 {
1100 m_colliderGroundfilter = 10;
1101 m_freemove = false;
1102 }
1103
1104 m_collideNormal.X = n.X;
1105 m_collideNormal.Y = n.Y;
1106 m_collideNormal.Z = n.Z;
1107
1108 m_iscollidingGround = true;
1109
1110
1111 ContactPoint contact = new ContactPoint();
1112 contact.PenetrationDepth = depth;
1113 contact.Position.X = localpos.X;
1114 contact.Position.Y = localpos.Y;
1115 contact.Position.Z = terrainheight;
1116 contact.SurfaceNormal.X = -n.X;
1117 contact.SurfaceNormal.Y = -n.Y;
1118 contact.SurfaceNormal.Z = -n.Z;
1119 contact.RelativeSpeed = -vel.Z;
1120 contact.CharacterFeet = true;
1121 AddCollisionEvent(0, contact);
1122
1123// vec.Z *= 0.5f;
1124 }
1125 }
1126
1127 else
1128 {
1129 m_colliderGroundfilter -= 5;
1130 if (m_colliderGroundfilter <= 0)
1131 {
1132 m_colliderGroundfilter = 0;
1133 m_iscollidingGround = false;
1134 }
1135 }
1136 }
1137 else
1138 {
1139 m_colliderGroundfilter -= 5;
1140 if (m_colliderGroundfilter <= 0)
1141 {
1142 m_colliderGroundfilter = 0;
1143 m_iscollidingGround = false;
1144 }
1145 }
1146
1147
1148 //******************************************
1149 if (!m_iscolliding)
1150 m_collideNormal.Z = 0;
1151
1152 bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f);
1153
1154
1155
1156 if (!tviszero)
1157 {
1158 m_freemove = false;
1159
1160 // movement relative to surface if moving on it
1161 // dont disturbe vertical movement, ie jumps
1162 if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f)
1163 {
1164 float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y;
1165 ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X);
1166 ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y);
1167 ctz.Z -= p;
1168 if (ctz.Z < 0)
1169 ctz.Z *= 2;
1170
1171 }
1172
1173 }
1174
1175
1176 if (!m_freemove)
1177 {
1178
1179 // if velocity is zero, use position control; otherwise, velocity control
1180 if (tviszero && m_iscolliding && !flying)
1181 {
1182 // keep track of where we stopped. No more slippin' & slidin'
1183 if (!_zeroFlag)
1184 {
1185 _zeroFlag = true;
1186 _zeroPosition = localpos;
1187 }
1188 if (m_pidControllerActive)
1189 {
1190 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1191 // react to the physics scene by moving it's position.
1192 // Avatar to Avatar collisions
1193 // Prim to avatar collisions
1194
1195 vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5);
1196 vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5);
1197 if(vel.Z > 0)
1198 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1199 else
1200 vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f;
1201/*
1202 if (flying)
1203 {
1204 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
1205 }
1206*/
1207 }
1208 //PidStatus = true;
1209 }
1210 else
1211 {
1212 m_pidControllerActive = true;
1213 _zeroFlag = false;
1214
1215 if (m_iscolliding)
1216 {
1217 if (!flying)
1218 {
1219 // we are on a surface
1220 if (ctz.Z > 0f)
1221 {
1222 // moving up or JUMPING
1223 vec.Z += (ctz.Z - vel.Z) * PID_D * 2f;
1224 vec.X += (ctz.X - vel.X) * (PID_D);
1225 vec.Y += (ctz.Y - vel.Y) * (PID_D);
1226 }
1227 else
1228 {
1229 // we are moving down on a surface
1230 if (ctz.Z == 0)
1231 {
1232 if (vel.Z > 0)
1233 vec.Z -= vel.Z * PID_D * 2f;
1234 vec.X += (ctz.X - vel.X) * (PID_D);
1235 vec.Y += (ctz.Y - vel.Y) * (PID_D);
1236 }
1237 // intencionally going down
1238 else
1239 {
1240 if (ctz.Z < vel.Z)
1241 vec.Z += (ctz.Z - vel.Z) * PID_D;
1242 else
1243 {
1244 }
1245
1246 if (Math.Abs(ctz.X) > Math.Abs(vel.X))
1247 vec.X += (ctz.X - vel.X) * (PID_D);
1248 if (Math.Abs(ctz.Y) > Math.Abs(vel.Y))
1249 vec.Y += (ctz.Y - vel.Y) * (PID_D);
1250 }
1251 }
1252
1253 // We're standing on something
1254 }
1255 else
1256 {
1257 // We're flying and colliding with something
1258 vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f);
1259 vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f);
1260 vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f);
1261 }
1262 }
1263 else // ie not colliding
1264 {
1265 if (flying) //(!m_iscolliding && flying)
1266 {
1267 // we're in mid air suspended
1268 vec.X += (ctz.X - vel.X) * (PID_D);
1269 vec.Y += (ctz.Y - vel.Y) * (PID_D);
1270 vec.Z += (ctz.Z - vel.Z) * (PID_D);
1271 }
1272
1273 else
1274 {
1275 // we're not colliding and we're not flying so that means we're falling!
1276 // m_iscolliding includes collisions with the ground.
1277
1278 // d.Vector3 pos = d.BodyGetPosition(Body);
1279 vec.X += (ctz.X - vel.X) * PID_D * 0.833f;
1280 vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f;
1281 // hack for breaking on fall
1282 if (ctz.Z == -9999f)
1283 vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass;
1284 }
1285 }
1286 }
1287
1288 if (velLengthSquared > 2500.0f) // 50m/s apply breaks
1289 {
1290 breakfactor = 0.16f * m_mass;
1291 vec.X -= breakfactor * vel.X;
1292 vec.Y -= breakfactor * vel.Y;
1293 vec.Z -= breakfactor * vel.Z;
1294 }
1295 }
1296 else
1297 {
1298 breakfactor = m_mass;
1299 vec.X -= breakfactor * vel.X;
1300 vec.Y -= breakfactor * vel.Y;
1301 if (flying)
1302 vec.Z -= 0.5f * breakfactor * vel.Z;
1303 else
1304 vec.Z -= .16f* m_mass * vel.Z;
1305 }
1306
1307 if (flying)
1308 {
1309 vec.Z -= _parent_scene.gravityz * m_mass;
1310
1311 //Added for auto fly height. Kitto Flora
1312 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
1313
1314 if (localpos.Z < target_altitude)
1315 {
1316 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
1317 }
1318 // end add Kitto Flora
1319 }
1320
1321 if (vec.IsFinite())
1322 {
1323 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
1324 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
1325 }
1326 else
1327 {
1328 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1329 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1330 defects.Add(this);
1331 // _parent_scene.RemoveCharacter(this);
1332 // destroy avatar capsule and related ODE data
1333 AvatarGeomAndBodyDestroy();
1334 return;
1335 }
1336
1337 // update our local ideia of position velocity and aceleration
1338 // _position = localpos;
1339 _position = localpos;
1340
1341 if (_zeroFlag)
1342 {
1343 _velocity = Vector3.Zero;
1344 _acceleration = Vector3.Zero;
1345 m_rotationalVelocity = Vector3.Zero;
1346 }
1347 else
1348 {
1349 Vector3 a =_velocity; // previus velocity
1350 SetSmooth(ref _velocity, ref vel, 2);
1351 a = (_velocity - a) * invtimeStep;
1352 SetSmooth(ref _acceleration, ref a, 2);
1353
1354 dtmp = d.BodyGetAngularVel(Body);
1355 m_rotationalVelocity.X = 0f;
1356 m_rotationalVelocity.Y = 0f;
1357 m_rotationalVelocity.Z = dtmp.Z;
1358 Math.Round(m_rotationalVelocity.Z,3);
1359 }
1360 }
1361
1362 public void round(ref Vector3 v, int digits)
1363 {
1364 v.X = (float)Math.Round(v.X, digits);
1365 v.Y = (float)Math.Round(v.Y, digits);
1366 v.Z = (float)Math.Round(v.Z, digits);
1367 }
1368
1369 public void SetSmooth(ref Vector3 dst, ref Vector3 value)
1370 {
1371 dst.X = 0.1f * dst.X + 0.9f * value.X;
1372 dst.Y = 0.1f * dst.Y + 0.9f * value.Y;
1373 dst.Z = 0.1f * dst.Z + 0.9f * value.Z;
1374 }
1375
1376 public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits)
1377 {
1378 dst.X = 0.4f * dst.X + 0.6f * value.X;
1379 dst.X = (float)Math.Round(dst.X, rounddigits);
1380
1381 dst.Y = 0.4f * dst.Y + 0.6f * value.Y;
1382 dst.Y = (float)Math.Round(dst.Y, rounddigits);
1383
1384 dst.Z = 0.4f * dst.Z + 0.6f * value.Z;
1385 dst.Z = (float)Math.Round(dst.Z, rounddigits);
1386 }
1387
1388
1389 /// <summary>
1390 /// Updates the reported position and velocity.
1391 /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording
1392 /// also outbounds checking
1393 /// copy and outbounds now done in move(..) at ode rate
1394 ///
1395 /// </summary>
1396 public void UpdatePositionAndVelocity()
1397 {
1398 return;
1399
1400// if (Body == IntPtr.Zero)
1401// return;
1402
1403 }
1404
1405 /// <summary>
1406 /// Cleanup the things we use in the scene.
1407 /// </summary>
1408 public void Destroy()
1409 {
1410 AddChange(changes.Remove, null);
1411 }
1412
1413 public override void CrossingFailure()
1414 {
1415 }
1416
1417 public override Vector3 PIDTarget { set { return; } }
1418 public override bool PIDActive {get {return m_pidControllerActive;} set { return; } }
1419 public override float PIDTau { set { return; } }
1420
1421 public override float PIDHoverHeight { set { return; } }
1422 public override bool PIDHoverActive { set { return; } }
1423 public override PIDHoverType PIDHoverType { set { return; } }
1424 public override float PIDHoverTau { set { return; } }
1425
1426 public override Quaternion APIDTarget { set { return; } }
1427
1428 public override bool APIDActive { set { return; } }
1429
1430 public override float APIDStrength { set { return; } }
1431
1432 public override float APIDDamping { set { return; } }
1433
1434
1435 public override void SubscribeEvents(int ms)
1436 {
1437 m_eventsubscription = ms;
1438 m_cureventsubscription = 0;
1439 if (CollisionEventsThisFrame == null)
1440 CollisionEventsThisFrame = new CollisionEventUpdate();
1441 SentEmptyCollisionsEvent = false;
1442 }
1443
1444 public override void UnSubscribeEvents()
1445 {
1446 if (CollisionEventsThisFrame != null)
1447 {
1448 lock (CollisionEventsThisFrame)
1449 {
1450 CollisionEventsThisFrame.Clear();
1451 CollisionEventsThisFrame = null;
1452 }
1453 }
1454 m_eventsubscription = 0;
1455 }
1456
1457 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1458 {
1459 if (CollisionEventsThisFrame == null)
1460 CollisionEventsThisFrame = new CollisionEventUpdate();
1461 lock (CollisionEventsThisFrame)
1462 {
1463 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1464 _parent_scene.AddCollisionEventReporting(this);
1465 }
1466 }
1467
1468 public void SendCollisions()
1469 {
1470 if (CollisionEventsThisFrame == null)
1471 return;
1472
1473 lock (CollisionEventsThisFrame)
1474 {
1475 if (m_cureventsubscription < m_eventsubscription)
1476 return;
1477
1478 m_cureventsubscription = 0;
1479
1480 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
1481
1482 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1483 {
1484 base.SendCollisionUpdate(CollisionEventsThisFrame);
1485
1486 if (ncolisions == 0)
1487 {
1488 SentEmptyCollisionsEvent = true;
1489 _parent_scene.RemoveCollisionEventReporting(this);
1490 }
1491 else
1492 {
1493 SentEmptyCollisionsEvent = false;
1494 CollisionEventsThisFrame.Clear();
1495 }
1496 }
1497 }
1498 }
1499
1500 internal void AddCollisionFrameTime(int t)
1501 {
1502 // protect it from overflow crashing
1503 if (m_cureventsubscription < 50000)
1504 m_cureventsubscription += t;
1505 }
1506
1507 public override bool SubscribedEvents()
1508 {
1509 if (m_eventsubscription > 0)
1510 return true;
1511 return false;
1512 }
1513
1514 private void changePhysicsStatus(bool NewStatus)
1515 {
1516 if (NewStatus != m_isPhysical)
1517 {
1518 if (NewStatus)
1519 {
1520 AvatarGeomAndBodyDestroy();
1521
1522 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1523
1524 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1525 _parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
1526 _parent_scene.AddCharacter(this);
1527 }
1528 else
1529 {
1530 _parent_scene.RemoveCollisionEventReporting(this);
1531 _parent_scene.RemoveCharacter(this);
1532 // destroy avatar capsule and related ODE data
1533 AvatarGeomAndBodyDestroy();
1534 }
1535 m_freemove = false;
1536 m_isPhysical = NewStatus;
1537 }
1538 }
1539
1540 private void changeAdd()
1541 {
1542 changePhysicsStatus(true);
1543 }
1544
1545 private void changeRemove()
1546 {
1547 changePhysicsStatus(false);
1548 }
1549
1550 private void changeShape(PrimitiveBaseShape arg)
1551 {
1552 }
1553
1554 private void changeAvatarSize(strAvatarSize st)
1555 {
1556 m_feetOffset = st.offset;
1557 changeSize(st.size);
1558 }
1559
1560 private void changeSize(Vector3 pSize)
1561 {
1562 if (pSize.IsFinite())
1563 {
1564 // for now only look to Z changes since viewers also don't change X and Y
1565 if (pSize.Z != m_size.Z)
1566 {
1567 AvatarGeomAndBodyDestroy();
1568
1569
1570 float oldsz = m_size.Z;
1571 m_size = pSize;
1572
1573
1574 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1575 _position.Z + (m_size.Z - oldsz) * 0.5f);
1576
1577 Velocity = Vector3.Zero;
1578
1579
1580 _parent_scene.actor_name_map[collider] = (PhysicsActor)this;
1581 _parent_scene.actor_name_map[capsule] = (PhysicsActor)this;
1582 }
1583 m_freemove = false;
1584 m_pidControllerActive = true;
1585 }
1586 else
1587 {
1588 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1589 }
1590 }
1591
1592 private void changePosition( Vector3 newPos)
1593 {
1594 if (Body != IntPtr.Zero)
1595 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1596 _position = newPos;
1597 m_freemove = false;
1598 m_pidControllerActive = true;
1599 }
1600
1601 private void changeOrientation(Quaternion newOri)
1602 {
1603 if (m_orientation != newOri)
1604 {
1605 m_orientation = newOri; // keep a copy for core use
1606 // but only use rotations around Z
1607
1608 m_orientation2D.W = newOri.W;
1609 m_orientation2D.Z = newOri.Z;
1610
1611 float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z;
1612 if (t > 0)
1613 {
1614 t = 1.0f / (float)Math.Sqrt(t);
1615 m_orientation2D.W *= t;
1616 m_orientation2D.Z *= t;
1617 }
1618 else
1619 {
1620 m_orientation2D.W = 1.0f;
1621 m_orientation2D.Z = 0f;
1622 }
1623 m_orientation2D.Y = 0f;
1624 m_orientation2D.X = 0f;
1625
1626 d.Quaternion myrot = new d.Quaternion();
1627 myrot.X = m_orientation2D.X;
1628 myrot.Y = m_orientation2D.Y;
1629 myrot.Z = m_orientation2D.Z;
1630 myrot.W = m_orientation2D.W;
1631 d.BodySetQuaternion(Body, ref myrot);
1632 }
1633 }
1634
1635 private void changeVelocity(Vector3 newVel)
1636 {
1637 m_pidControllerActive = true;
1638 m_freemove = false;
1639 _target_velocity = newVel;
1640 }
1641
1642 private void changeSetTorque(Vector3 newTorque)
1643 {
1644 }
1645
1646 private void changeAddForce(Vector3 newForce)
1647 {
1648 }
1649
1650 private void changeAddAngularForce(Vector3 arg)
1651 {
1652 }
1653
1654 private void changeAngularLock(Vector3 arg)
1655 {
1656 }
1657
1658 private void changeFloatOnWater(bool arg)
1659 {
1660 }
1661
1662 private void changeVolumedetetion(bool arg)
1663 {
1664 }
1665
1666 private void changeSelectedStatus(bool arg)
1667 {
1668 }
1669
1670 private void changeDisable(bool arg)
1671 {
1672 }
1673
1674 private void changeBuilding(bool arg)
1675 {
1676 }
1677
1678 private void setFreeMove()
1679 {
1680 m_pidControllerActive = true;
1681 _zeroFlag = false;
1682 _target_velocity = Vector3.Zero;
1683 m_freemove = true;
1684 m_colliderfilter = -1;
1685 m_colliderObjectfilter = -1;
1686 m_colliderGroundfilter = -1;
1687
1688 m_iscolliding = false;
1689 m_iscollidingGround = false;
1690 m_iscollidingObj = false;
1691
1692 CollisionEventsThisFrame.Clear();
1693 }
1694
1695 private void changeForce(Vector3 newForce)
1696 {
1697 setFreeMove();
1698
1699 if (Body != IntPtr.Zero)
1700 {
1701 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1702 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1703 }
1704 }
1705
1706 // for now momentum is actually velocity
1707 private void changeMomentum(Vector3 newmomentum)
1708 {
1709 _velocity = newmomentum;
1710 setFreeMove();
1711
1712 if (Body != IntPtr.Zero)
1713 d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z);
1714 }
1715
1716 private void donullchange()
1717 {
1718 }
1719
1720 public bool DoAChange(changes what, object arg)
1721 {
1722 if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1723 {
1724 return false;
1725 }
1726
1727 // nasty switch
1728 switch (what)
1729 {
1730 case changes.Add:
1731 changeAdd();
1732 break;
1733 case changes.Remove:
1734 changeRemove();
1735 break;
1736
1737 case changes.Position:
1738 changePosition((Vector3)arg);
1739 break;
1740
1741 case changes.Orientation:
1742 changeOrientation((Quaternion)arg);
1743 break;
1744
1745 case changes.PosOffset:
1746 donullchange();
1747 break;
1748
1749 case changes.OriOffset:
1750 donullchange();
1751 break;
1752
1753 case changes.Velocity:
1754 changeVelocity((Vector3)arg);
1755 break;
1756
1757 // case changes.Acceleration:
1758 // changeacceleration((Vector3)arg);
1759 // break;
1760 // case changes.AngVelocity:
1761 // changeangvelocity((Vector3)arg);
1762 // break;
1763
1764 case changes.Force:
1765 changeForce((Vector3)arg);
1766 break;
1767
1768 case changes.Torque:
1769 changeSetTorque((Vector3)arg);
1770 break;
1771
1772 case changes.AddForce:
1773 changeAddForce((Vector3)arg);
1774 break;
1775
1776 case changes.AddAngForce:
1777 changeAddAngularForce((Vector3)arg);
1778 break;
1779
1780 case changes.AngLock:
1781 changeAngularLock((Vector3)arg);
1782 break;
1783
1784 case changes.Size:
1785 changeSize((Vector3)arg);
1786 break;
1787
1788 case changes.AvatarSize:
1789 changeAvatarSize((strAvatarSize)arg);
1790 break;
1791
1792 case changes.Momentum:
1793 changeMomentum((Vector3)arg);
1794 break;
1795/* not in use for now
1796 case changes.Shape:
1797 changeShape((PrimitiveBaseShape)arg);
1798 break;
1799
1800 case changes.CollidesWater:
1801 changeFloatOnWater((bool)arg);
1802 break;
1803
1804 case changes.VolumeDtc:
1805 changeVolumedetetion((bool)arg);
1806 break;
1807
1808 case changes.Physical:
1809 changePhysicsStatus((bool)arg);
1810 break;
1811
1812 case changes.Selected:
1813 changeSelectedStatus((bool)arg);
1814 break;
1815
1816 case changes.disabled:
1817 changeDisable((bool)arg);
1818 break;
1819
1820 case changes.building:
1821 changeBuilding((bool)arg);
1822 break;
1823*/
1824 case changes.Null:
1825 donullchange();
1826 break;
1827
1828 default:
1829 donullchange();
1830 break;
1831 }
1832 return false;
1833 }
1834
1835 public void AddChange(changes what, object arg)
1836 {
1837 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1838 }
1839
1840 private struct strAvatarSize
1841 {
1842 public Vector3 size;
1843 public float offset;
1844 }
1845
1846 }
1847}
generated by cgit v1.1 at 2024-12-16 22:11:55 +0000