aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
diff options
context:
space:
mode:
authorRobert Adams2015-09-08 06:15:46 -0700
committerRobert Adams2015-09-08 06:15:46 -0700
commit4dd17c4117ea413fb0c4418511956cb3abfe258c (patch)
tree6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
parentMerge of ubitworkvarnew with opensim/master as of 20150905. (diff)
downloadopensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2
opensim-SC-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz
More 'everything is a module' merging.
Have most of UbitOde converted. There are compile errors in OpenSimBase as the new modules stuff is not all there. Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1847
1 files changed, 0 insertions, 1847 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
deleted file mode 100644
index 0e2cbd2..0000000
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ /dev/null
@@ -1,1847 +0,0 @@
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 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-14 06:12:09 +0000