diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 347 |
1 files changed, 274 insertions, 73 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 15bdc57..1b25faf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | |||
@@ -74,7 +74,6 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
74 | 74 | ||
75 | private Vector3 _position; | 75 | private Vector3 _position; |
76 | private Vector3 _zeroPosition; | 76 | private Vector3 _zeroPosition; |
77 | private bool _zeroFlag = false; | ||
78 | private Vector3 _velocity; | 77 | private Vector3 _velocity; |
79 | private Vector3 _target_velocity; | 78 | private Vector3 _target_velocity; |
80 | private Vector3 _acceleration; | 79 | private Vector3 _acceleration; |
@@ -90,11 +89,15 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
90 | public float PID_D; | 89 | public float PID_D; |
91 | public float PID_P; | 90 | public float PID_P; |
92 | 91 | ||
92 | private float timeStep; | ||
93 | private float invtimeStep; | ||
94 | |||
93 | private float m_feetOffset = 0; | 95 | private float m_feetOffset = 0; |
94 | private float feetOff = 0; | 96 | private float feetOff = 0; |
95 | private float feetSZ = 0.5f; | 97 | private float feetSZ = 0.5f; |
96 | const float feetScale = 0.8f; | 98 | const float feetScale = 0.8f; |
97 | private float boneOff = 0; | 99 | private float boneOff = 0; |
100 | private float m_lastVelocitySqr = 0; | ||
98 | 101 | ||
99 | public float walkDivisor = 1.3f; | 102 | public float walkDivisor = 1.3f; |
100 | public float runDivisor = 0.8f; | 103 | public float runDivisor = 0.8f; |
@@ -103,6 +106,9 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
103 | private bool m_iscollidingGround = false; | 106 | private bool m_iscollidingGround = false; |
104 | private bool m_iscollidingObj = false; | 107 | private bool m_iscollidingObj = false; |
105 | private bool m_alwaysRun = false; | 108 | private bool m_alwaysRun = false; |
109 | |||
110 | private bool _zeroFlag = false; | ||
111 | |||
106 | private int m_requestedUpdateFrequency = 0; | 112 | private int m_requestedUpdateFrequency = 0; |
107 | private uint m_localID = 0; | 113 | private uint m_localID = 0; |
108 | public bool m_returnCollisions = false; | 114 | public bool m_returnCollisions = false; |
@@ -120,6 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
120 | int m_colliderfilter = 0; | 126 | int m_colliderfilter = 0; |
121 | int m_colliderGroundfilter = 0; | 127 | int m_colliderGroundfilter = 0; |
122 | int m_colliderObjectfilter = 0; | 128 | int m_colliderObjectfilter = 0; |
129 | bool m_collisionException = false; | ||
123 | 130 | ||
124 | // Default we're a Character | 131 | // Default we're a Character |
125 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); | 132 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); |
@@ -132,10 +139,11 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
132 | // we do land collisions not ode | CollisionCategories.Land); | 139 | // we do land collisions not ode | CollisionCategories.Land); |
133 | public IntPtr Body = IntPtr.Zero; | 140 | public IntPtr Body = IntPtr.Zero; |
134 | private OdeScene _parent_scene; | 141 | private OdeScene _parent_scene; |
135 | public IntPtr topbox = IntPtr.Zero; | 142 | private IntPtr topbox = IntPtr.Zero; |
136 | public IntPtr midbox = IntPtr.Zero; | 143 | private IntPtr midbox = IntPtr.Zero; |
137 | public IntPtr feetbox = IntPtr.Zero; | 144 | private IntPtr feetbox = IntPtr.Zero; |
138 | public IntPtr bonebox = IntPtr.Zero; | 145 | private IntPtr bbox = IntPtr.Zero; |
146 | public IntPtr collider = IntPtr.Zero; | ||
139 | 147 | ||
140 | public IntPtr Amotor = IntPtr.Zero; | 148 | public IntPtr Amotor = IntPtr.Zero; |
141 | 149 | ||
@@ -143,6 +151,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
143 | 151 | ||
144 | 152 | ||
145 | 153 | ||
154 | |||
146 | public int m_eventsubscription = 0; | 155 | public int m_eventsubscription = 0; |
147 | private int m_cureventsubscription = 0; | 156 | private int m_cureventsubscription = 0; |
148 | private CollisionEventUpdate CollisionEventsThisFrame = null; | 157 | private CollisionEventUpdate CollisionEventsThisFrame = null; |
@@ -160,6 +169,9 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
160 | { | 169 | { |
161 | m_uuid = UUID.Random(); | 170 | m_uuid = UUID.Random(); |
162 | 171 | ||
172 | timeStep = parent_scene.ODE_STEPSIZE; | ||
173 | invtimeStep = 1 / timeStep; | ||
174 | |||
163 | if (pos.IsFinite()) | 175 | if (pos.IsFinite()) |
164 | { | 176 | { |
165 | if (pos.Z > 99999f) | 177 | if (pos.Z > 99999f) |
@@ -208,8 +220,8 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
208 | 220 | ||
209 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default | 221 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default |
210 | 222 | ||
211 | PID_D = basePID_D * m_mass / parent_scene.ODE_STEPSIZE; | 223 | PID_D = basePID_D * m_mass * invtimeStep; |
212 | PID_P = basePID_P * m_mass / parent_scene.ODE_STEPSIZE; | 224 | PID_P = basePID_P * m_mass * invtimeStep; |
213 | 225 | ||
214 | m_isPhysical = false; // current status: no ODE information exists | 226 | m_isPhysical = false; // current status: no ODE information exists |
215 | 227 | ||
@@ -292,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
292 | set | 304 | set |
293 | { | 305 | { |
294 | flying = value; | 306 | flying = value; |
295 | // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); | 307 | // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); |
296 | } | 308 | } |
297 | } | 309 | } |
298 | 310 | ||
@@ -336,25 +348,25 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
336 | get { return m_iscollidingGround; } | 348 | get { return m_iscollidingGround; } |
337 | set | 349 | set |
338 | { | 350 | { |
339 | /* we now control this | 351 | /* we now control this |
340 | if (value) | 352 | if (value) |
341 | { | 353 | { |
342 | m_colliderGroundfilter += 2; | 354 | m_colliderGroundfilter += 2; |
343 | if (m_colliderGroundfilter > 2) | 355 | if (m_colliderGroundfilter > 2) |
344 | m_colliderGroundfilter = 2; | 356 | m_colliderGroundfilter = 2; |
345 | } | 357 | } |
346 | else | 358 | else |
347 | { | 359 | { |
348 | m_colliderGroundfilter--; | 360 | m_colliderGroundfilter--; |
349 | if (m_colliderGroundfilter < 0) | 361 | if (m_colliderGroundfilter < 0) |
350 | m_colliderGroundfilter = 0; | 362 | m_colliderGroundfilter = 0; |
351 | } | 363 | } |
352 | 364 | ||
353 | if (m_colliderGroundfilter == 0) | 365 | if (m_colliderGroundfilter == 0) |
354 | m_iscollidingGround = false; | 366 | m_iscollidingGround = false; |
355 | else | 367 | else |
356 | m_iscollidingGround = true; | 368 | m_iscollidingGround = true; |
357 | */ | 369 | */ |
358 | } | 370 | } |
359 | 371 | ||
360 | } | 372 | } |
@@ -386,7 +398,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
386 | else | 398 | else |
387 | m_iscollidingObj = true; | 399 | m_iscollidingObj = true; |
388 | 400 | ||
389 | // m_iscollidingObj = value; | 401 | // m_iscollidingObj = value; |
390 | 402 | ||
391 | if (m_iscollidingObj) | 403 | if (m_iscollidingObj) |
392 | m_pidControllerActive = false; | 404 | m_pidControllerActive = false; |
@@ -634,8 +646,8 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
634 | get { return m_orientation; } | 646 | get { return m_orientation; } |
635 | set | 647 | set |
636 | { | 648 | { |
637 | // fakeori = value; | 649 | // fakeori = value; |
638 | // givefakeori++; | 650 | // givefakeori++; |
639 | 651 | ||
640 | value.Normalize(); | 652 | value.Normalize(); |
641 | AddChange(changes.Orientation, value); | 653 | AddChange(changes.Orientation, value); |
@@ -690,6 +702,46 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
690 | AddChange(changes.Momentum, momentum); | 702 | AddChange(changes.Momentum, momentum); |
691 | } | 703 | } |
692 | 704 | ||
705 | private void ajustCollider() | ||
706 | { | ||
707 | float vq = _velocity.LengthSquared(); | ||
708 | if (m_lastVelocitySqr != vq) | ||
709 | { | ||
710 | m_lastVelocitySqr = vq; | ||
711 | if (vq > 100.0f) | ||
712 | { | ||
713 | Vector3 off = _velocity; | ||
714 | float t = 0.5f * timeStep; | ||
715 | off = off * t; | ||
716 | d.GeomSetOffsetPosition(bbox, off.X, off.Y, off.Z); | ||
717 | off.X = 2.0f * (m_size.X + Math.Abs(off.X)); | ||
718 | off.Y = 2.0f * (m_size.Y + Math.Abs(off.Y)); | ||
719 | off.Z = m_size.Z + 2.0f * Math.Abs(off.Z); | ||
720 | d.GeomBoxSetLengths(bbox, off.X, off.Y, off.Z); | ||
721 | |||
722 | d.GeomSetCategoryBits(bbox, (uint)m_collisionCategories); | ||
723 | d.GeomSetCollideBits(bbox, (uint)m_collisionFlags); | ||
724 | d.GeomSetCategoryBits(topbox, 0); | ||
725 | d.GeomSetCollideBits(topbox, 0); | ||
726 | d.GeomSetCategoryBits(midbox, 0); | ||
727 | d.GeomSetCollideBits(midbox, 0); | ||
728 | d.GeomSetCategoryBits(feetbox, 0); | ||
729 | d.GeomSetCollideBits(feetbox, 0); | ||
730 | } | ||
731 | else | ||
732 | { | ||
733 | d.GeomSetCategoryBits(bbox, 0); | ||
734 | d.GeomSetCollideBits(bbox, 0); | ||
735 | d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); | ||
736 | d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); | ||
737 | d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); | ||
738 | d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); | ||
739 | d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); | ||
740 | d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); | ||
741 | } | ||
742 | } | ||
743 | } | ||
744 | |||
693 | private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) | 745 | private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) |
694 | { | 746 | { |
695 | // sizes one day should came from visual parameters | 747 | // sizes one day should came from visual parameters |
@@ -697,7 +749,6 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
697 | float sy = m_size.Y; | 749 | float sy = m_size.Y; |
698 | float sz = m_size.Z; | 750 | float sz = m_size.Z; |
699 | 751 | ||
700 | |||
701 | float topsx = sx * 0.9f; | 752 | float topsx = sx * 0.9f; |
702 | float midsx = sx; | 753 | float midsx = sx; |
703 | float feetsx = sx * feetScale; | 754 | float feetsx = sx * feetScale; |
@@ -732,21 +783,17 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
732 | 783 | ||
733 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | 784 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); |
734 | 785 | ||
735 | feetbox = d.CreateBox(_parent_scene.CharsSpace, feetsx, feetsy, feetsz); | 786 | collider = d.HashSpaceCreate(_parent_scene.CharsSpace); |
736 | d.GeomSetCategoryBits(feetbox, (uint)m_collisionCategories); | 787 | d.HashSpaceSetLevels(collider, -4, 3); |
737 | d.GeomSetCollideBits(feetbox, (uint)m_collisionFlags); | 788 | d.SpaceSetSublevel(collider, 3); |
738 | 789 | d.SpaceSetCleanup(collider, false); | |
739 | midbox = d.CreateBox(_parent_scene.CharsSpace, midsx, midsy, midsz); | 790 | d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); |
740 | d.GeomSetCategoryBits(midbox, (uint)m_collisionCategories); | 791 | d.GeomSetCollideBits(collider, (uint)m_collisionFlags); |
741 | d.GeomSetCollideBits(midbox, (uint)m_collisionFlags); | ||
742 | 792 | ||
743 | topbox = d.CreateBox(_parent_scene.CharsSpace, topsx, topsy, topsz); | 793 | feetbox = d.CreateBox(collider, feetsx, feetsy, feetsz); |
744 | d.GeomSetCategoryBits(topbox, (uint)m_collisionCategories); | 794 | midbox = d.CreateBox(collider, midsx, midsy, midsz); |
745 | d.GeomSetCollideBits(topbox, (uint)m_collisionFlags); | 795 | topbox = d.CreateBox(collider, topsx, topsy, topsz); |
746 | 796 | bbox = d.CreateBox(collider, m_size.X, m_size.Y, m_size.Z); | |
747 | bonebox = d.CreateBox(_parent_scene.CharsSpace, bonesx, bonesy, bonesz); | ||
748 | d.GeomSetCategoryBits(bonebox, (uint)m_collisionCategories); | ||
749 | d.GeomSetCollideBits(bonebox, (uint)m_collisionFlags); | ||
750 | 797 | ||
751 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass | 798 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass |
752 | 799 | ||
@@ -758,9 +805,13 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
758 | Body = d.BodyCreate(_parent_scene.world); | 805 | Body = d.BodyCreate(_parent_scene.world); |
759 | 806 | ||
760 | _zeroFlag = false; | 807 | _zeroFlag = false; |
808 | m_collisionException = false; | ||
761 | m_pidControllerActive = true; | 809 | m_pidControllerActive = true; |
762 | m_freemove = false; | 810 | m_freemove = false; |
763 | 811 | ||
812 | _velocity = Vector3.Zero; | ||
813 | m_lastVelocitySqr = 0; | ||
814 | |||
764 | d.BodySetAutoDisableFlag(Body, false); | 815 | d.BodySetAutoDisableFlag(Body, false); |
765 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); | 816 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); |
766 | 817 | ||
@@ -772,12 +823,14 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
772 | d.GeomSetBody(feetbox, Body); | 823 | d.GeomSetBody(feetbox, Body); |
773 | d.GeomSetBody(midbox, Body); | 824 | d.GeomSetBody(midbox, Body); |
774 | d.GeomSetBody(topbox, Body); | 825 | d.GeomSetBody(topbox, Body); |
775 | d.GeomSetBody(bonebox, Body); | 826 | d.GeomSetBody(bbox, Body); |
776 | 827 | ||
777 | d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); | 828 | d.GeomSetOffsetPosition(feetbox, 0, 0, feetz); |
778 | d.GeomSetOffsetPosition(midbox, 0, 0, midz); | 829 | d.GeomSetOffsetPosition(midbox, 0, 0, midz); |
779 | d.GeomSetOffsetPosition(topbox, 0, 0, topz); | 830 | d.GeomSetOffsetPosition(topbox, 0, 0, topz); |
780 | d.GeomSetOffsetPosition(bonebox, 0, 0, m_feetOffset); | 831 | |
832 | ajustCollider(); | ||
833 | |||
781 | 834 | ||
782 | // The purpose of the AMotor here is to keep the avatar's physical | 835 | // The purpose of the AMotor here is to keep the avatar's physical |
783 | // surrogate from rotating while moving | 836 | // surrogate from rotating while moving |
@@ -841,44 +894,109 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
841 | if (topbox != IntPtr.Zero) | 894 | if (topbox != IntPtr.Zero) |
842 | { | 895 | { |
843 | _parent_scene.actor_name_map.Remove(topbox); | 896 | _parent_scene.actor_name_map.Remove(topbox); |
844 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | 897 | _parent_scene.waitForSpaceUnlock(collider); |
845 | d.GeomDestroy(topbox); | 898 | d.GeomDestroy(topbox); |
846 | topbox = IntPtr.Zero; | 899 | topbox = IntPtr.Zero; |
847 | } | 900 | } |
848 | if (midbox != IntPtr.Zero) | 901 | if (midbox != IntPtr.Zero) |
849 | { | 902 | { |
850 | _parent_scene.actor_name_map.Remove(midbox); | 903 | _parent_scene.actor_name_map.Remove(midbox); |
851 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | 904 | _parent_scene.waitForSpaceUnlock(collider); |
852 | d.GeomDestroy(midbox); | 905 | d.GeomDestroy(midbox); |
853 | midbox = IntPtr.Zero; | 906 | midbox = IntPtr.Zero; |
854 | } | 907 | } |
855 | if (feetbox != IntPtr.Zero) | 908 | if (feetbox != IntPtr.Zero) |
856 | { | 909 | { |
857 | _parent_scene.actor_name_map.Remove(feetbox); | 910 | _parent_scene.actor_name_map.Remove(feetbox); |
858 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | 911 | _parent_scene.waitForSpaceUnlock(collider); |
859 | d.GeomDestroy(feetbox); | 912 | d.GeomDestroy(feetbox); |
860 | feetbox = IntPtr.Zero; | 913 | feetbox = IntPtr.Zero; |
861 | } | 914 | } |
862 | 915 | ||
863 | if (bonebox != IntPtr.Zero) | 916 | if (bbox != IntPtr.Zero) |
917 | { | ||
918 | _parent_scene.actor_name_map.Remove(bbox); | ||
919 | _parent_scene.waitForSpaceUnlock(collider); | ||
920 | d.GeomDestroy(bbox); | ||
921 | bbox = IntPtr.Zero; | ||
922 | } | ||
923 | |||
924 | if (collider != IntPtr.Zero) | ||
864 | { | 925 | { |
865 | _parent_scene.actor_name_map.Remove(bonebox); | 926 | d.SpaceDestroy(collider); |
866 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | 927 | collider = IntPtr.Zero; |
867 | d.GeomDestroy(bonebox); | ||
868 | bonebox = IntPtr.Zero; | ||
869 | } | 928 | } |
870 | 929 | ||
871 | } | 930 | } |
872 | 931 | ||
932 | //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z | ||
933 | public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
934 | { | ||
935 | float sin = 2.0f * rot.Z * rot.W; | ||
936 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
937 | float tx = x; | ||
938 | |||
939 | x = tx * cos - y * sin; | ||
940 | y = tx * sin + y * cos; | ||
941 | } | ||
942 | public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
943 | { | ||
944 | float tx = x; | ||
945 | x = tx * cos - y * sin; | ||
946 | y = tx * sin + y * cos; | ||
947 | } | ||
948 | public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
949 | { | ||
950 | float tx = x; | ||
951 | x = tx * cos + y * sin; | ||
952 | y = -tx * sin + y * cos; | ||
953 | } | ||
954 | |||
955 | public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
956 | { | ||
957 | float sin = - 2.0f * rot.Z * rot.W; | ||
958 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
959 | float tx = x; | ||
960 | |||
961 | x = tx * cos - y * sin; | ||
962 | y = tx * sin + y * cos; | ||
963 | } | ||
964 | |||
965 | |||
873 | public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) | 966 | public bool Collide(IntPtr me, bool reverse, ref d.ContactGeom contact, ref bool feetcollision) |
874 | { | 967 | { |
875 | feetcollision = false; | 968 | feetcollision = false; |
969 | if (m_collisionException) | ||
970 | return false; | ||
876 | 971 | ||
877 | if (me == bonebox) // inner bone | 972 | if (me == bbox) // if moving fast |
878 | { | 973 | { |
879 | if (contact.pos.Z - _position.Z < boneOff) | 974 | // force a full inelastic collision |
880 | IsColliding = true; | 975 | m_collisionException = true; |
881 | return true; | 976 | |
977 | Vector3 off = m_size * 0.5f; | ||
978 | off.X += contact.depth; | ||
979 | off.Y += contact.depth; | ||
980 | off.Z += contact.depth; | ||
981 | if (reverse) | ||
982 | { | ||
983 | off.X *= -contact.normal.X; | ||
984 | off.Y *= -contact.normal.Y; | ||
985 | off.Z *= -contact.normal.Z; | ||
986 | } | ||
987 | else | ||
988 | { | ||
989 | off.X *= contact.normal.X; | ||
990 | off.Y *= contact.normal.Y; | ||
991 | off.Z *= contact.normal.Z; | ||
992 | } | ||
993 | |||
994 | off.X += contact.pos.X; | ||
995 | off.Y += contact.pos.Y; | ||
996 | off.Z += contact.pos.Z; | ||
997 | |||
998 | _position = off; | ||
999 | return false; | ||
882 | } | 1000 | } |
883 | 1001 | ||
884 | if (me == topbox) // keep a box head | 1002 | if (me == topbox) // keep a box head |
@@ -890,6 +1008,21 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
890 | 1008 | ||
891 | if (me == midbox) | 1009 | if (me == midbox) |
892 | { | 1010 | { |
1011 | if (Math.Abs(contact.normal.Z) > 0.95f) | ||
1012 | { | ||
1013 | float nz = contact.normal.Z; | ||
1014 | if (!reverse) | ||
1015 | nz = -nz; | ||
1016 | |||
1017 | if (nz > 0) | ||
1018 | return true; // missed head TODO | ||
1019 | |||
1020 | // missed feet collision? | ||
1021 | |||
1022 | |||
1023 | return true; | ||
1024 | } | ||
1025 | |||
893 | t = offx * offx + offy * offy; | 1026 | t = offx * offx + offy * offy; |
894 | t = (float)Math.Sqrt(t); | 1027 | t = (float)Math.Sqrt(t); |
895 | t = 1 / t; | 1028 | t = 1 / t; |
@@ -917,12 +1050,19 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
917 | 1050 | ||
918 | if (Math.Abs(contact.normal.Z) > 0.95f) | 1051 | if (Math.Abs(contact.normal.Z) > 0.95f) |
919 | { | 1052 | { |
1053 | if (contact.normal.Z > 0) | ||
1054 | contact.normal.Z = 1.0f; | ||
1055 | else | ||
1056 | contact.normal.Z = -1.0f; | ||
1057 | contact.normal.X = 0.0f; | ||
1058 | contact.normal.Y = 0.0f; | ||
920 | feetcollision = true; | 1059 | feetcollision = true; |
921 | if (h < boneOff) | 1060 | if (h < boneOff) |
922 | IsColliding = true; | 1061 | IsColliding = true; |
923 | return true; | 1062 | return true; |
924 | } | 1063 | } |
925 | 1064 | ||
1065 | |||
926 | float offz = h - feetOff; // distance from top of feetbox | 1066 | float offz = h - feetOff; // distance from top of feetbox |
927 | 1067 | ||
928 | if (offz > 0) | 1068 | if (offz > 0) |
@@ -971,11 +1111,28 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
971 | /// This is the avatar's movement control + PID Controller | 1111 | /// This is the avatar's movement control + PID Controller |
972 | /// </summary> | 1112 | /// </summary> |
973 | /// <param name="timeStep"></param> | 1113 | /// <param name="timeStep"></param> |
974 | public void Move(float timeStep, List<OdeCharacter> defects) | 1114 | public void Move(List<OdeCharacter> defects) |
975 | { | 1115 | { |
976 | if (Body == IntPtr.Zero) | 1116 | if (Body == IntPtr.Zero) |
977 | return; | 1117 | return; |
978 | 1118 | ||
1119 | if (m_collisionException) | ||
1120 | { | ||
1121 | d.BodySetPosition(Body,_position.X, _position.Y, _position.Z); | ||
1122 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
1123 | |||
1124 | float v = _velocity.Length(); | ||
1125 | if (v != 0) | ||
1126 | { | ||
1127 | v = 6.0f / v; | ||
1128 | _velocity = _velocity * v; | ||
1129 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | ||
1130 | } | ||
1131 | ajustCollider(); | ||
1132 | m_collisionException = false; | ||
1133 | return; | ||
1134 | } | ||
1135 | |||
979 | d.Vector3 dtmp = d.BodyGetPosition(Body); | 1136 | d.Vector3 dtmp = d.BodyGetPosition(Body); |
980 | Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | 1137 | Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); |
981 | 1138 | ||
@@ -1049,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1049 | 1206 | ||
1050 | //****************************************** | 1207 | //****************************************** |
1051 | // colide with land | 1208 | // colide with land |
1209 | |||
1052 | d.AABB aabb; | 1210 | d.AABB aabb; |
1053 | d.GeomGetAABB(feetbox, out aabb); | 1211 | d.GeomGetAABB(feetbox, out aabb); |
1054 | float chrminZ = aabb.MinZ - 0.02f; // move up a bit | 1212 | float chrminZ = aabb.MinZ - 0.02f; // move up a bit |
@@ -1095,8 +1253,8 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1095 | contact.Position.X = localpos.X; | 1253 | contact.Position.X = localpos.X; |
1096 | contact.Position.Y = localpos.Y; | 1254 | contact.Position.Y = localpos.Y; |
1097 | contact.Position.Z = chrminZ; | 1255 | contact.Position.Z = chrminZ; |
1098 | contact.SurfaceNormal.X = 0f; | 1256 | contact.SurfaceNormal.X = 0.0f; |
1099 | contact.SurfaceNormal.Y = 0f; | 1257 | contact.SurfaceNormal.Y = 0.0f; |
1100 | contact.SurfaceNormal.Z = -1f; | 1258 | contact.SurfaceNormal.Z = -1f; |
1101 | contact.RelativeSpeed = -vel.Z; | 1259 | contact.RelativeSpeed = -vel.Z; |
1102 | contact.CharacterFeet = true; | 1260 | contact.CharacterFeet = true; |
@@ -1118,6 +1276,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1118 | m_iscollidingGround = false; | 1276 | m_iscollidingGround = false; |
1119 | } | 1277 | } |
1120 | 1278 | ||
1279 | |||
1121 | //****************************************** | 1280 | //****************************************** |
1122 | 1281 | ||
1123 | bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); | 1282 | bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); |
@@ -1253,21 +1412,58 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1253 | } | 1412 | } |
1254 | 1413 | ||
1255 | // update our local ideia of position velocity and aceleration | 1414 | // update our local ideia of position velocity and aceleration |
1415 | // _position = localpos; | ||
1256 | _position = localpos; | 1416 | _position = localpos; |
1417 | |||
1257 | if (_zeroFlag) | 1418 | if (_zeroFlag) |
1258 | { | 1419 | { |
1259 | _velocity = Vector3.Zero; | 1420 | _velocity = Vector3.Zero; |
1260 | _acceleration = Vector3.Zero; | 1421 | _acceleration = Vector3.Zero; |
1422 | m_rotationalVelocity = Vector3.Zero; | ||
1261 | } | 1423 | } |
1262 | else | 1424 | else |
1263 | { | 1425 | { |
1264 | _acceleration = _velocity; // previus velocity | 1426 | Vector3 a =_velocity; // previus velocity |
1265 | _velocity = vel; | 1427 | SetSmooth(ref _velocity, ref vel, 2); |
1266 | _acceleration = (vel - _acceleration) / timeStep; | 1428 | a = (_velocity - a) * invtimeStep; |
1429 | SetSmooth(ref _acceleration, ref a, 2); | ||
1430 | |||
1431 | dtmp = d.BodyGetAngularVel(Body); | ||
1432 | m_rotationalVelocity.X = 0f; | ||
1433 | m_rotationalVelocity.Y = 0f; | ||
1434 | m_rotationalVelocity.Z = dtmp.Z; | ||
1435 | Math.Round(m_rotationalVelocity.Z,3); | ||
1267 | } | 1436 | } |
1268 | 1437 | ajustCollider(); | |
1269 | } | 1438 | } |
1270 | 1439 | ||
1440 | public void round(ref Vector3 v, int digits) | ||
1441 | { | ||
1442 | v.X = (float)Math.Round(v.X, digits); | ||
1443 | v.Y = (float)Math.Round(v.Y, digits); | ||
1444 | v.Z = (float)Math.Round(v.Z, digits); | ||
1445 | } | ||
1446 | |||
1447 | public void SetSmooth(ref Vector3 dst, ref Vector3 value) | ||
1448 | { | ||
1449 | dst.X = 0.1f * dst.X + 0.9f * value.X; | ||
1450 | dst.Y = 0.1f * dst.Y + 0.9f * value.Y; | ||
1451 | dst.Z = 0.1f * dst.Z + 0.9f * value.Z; | ||
1452 | } | ||
1453 | |||
1454 | public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) | ||
1455 | { | ||
1456 | dst.X = 0.4f * dst.X + 0.6f * value.X; | ||
1457 | dst.X = (float)Math.Round(dst.X, rounddigits); | ||
1458 | |||
1459 | dst.Y = 0.4f * dst.Y + 0.6f * value.Y; | ||
1460 | dst.Y = (float)Math.Round(dst.Y, rounddigits); | ||
1461 | |||
1462 | dst.Z = 0.4f * dst.Z + 0.6f * value.Z; | ||
1463 | dst.Z = (float)Math.Round(dst.Z, rounddigits); | ||
1464 | } | ||
1465 | |||
1466 | |||
1271 | /// <summary> | 1467 | /// <summary> |
1272 | /// Updates the reported position and velocity. | 1468 | /// Updates the reported position and velocity. |
1273 | /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording | 1469 | /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording |
@@ -1394,10 +1590,11 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1394 | 1590 | ||
1395 | AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); | 1591 | AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); |
1396 | 1592 | ||
1397 | _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; | 1593 | _parent_scene.actor_name_map[collider] = (PhysicsActor)this; |
1398 | _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; | ||
1399 | _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; | 1594 | _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; |
1400 | _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; | 1595 | _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; |
1596 | _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; | ||
1597 | _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; | ||
1401 | _parent_scene.AddCharacter(this); | 1598 | _parent_scene.AddCharacter(this); |
1402 | } | 1599 | } |
1403 | else | 1600 | else |
@@ -1450,13 +1647,16 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1450 | _position.Z + (m_size.Z - oldsz) * 0.5f); | 1647 | _position.Z + (m_size.Z - oldsz) * 0.5f); |
1451 | 1648 | ||
1452 | Velocity = Vector3.Zero; | 1649 | Velocity = Vector3.Zero; |
1650 | |||
1453 | 1651 | ||
1454 | _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; | 1652 | _parent_scene.actor_name_map[collider] = (PhysicsActor)this; |
1455 | _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; | ||
1456 | _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; | 1653 | _parent_scene.actor_name_map[feetbox] = (PhysicsActor)this; |
1457 | _parent_scene.actor_name_map[bonebox] = (PhysicsActor)this; | 1654 | _parent_scene.actor_name_map[midbox] = (PhysicsActor)this; |
1655 | _parent_scene.actor_name_map[topbox] = (PhysicsActor)this; | ||
1656 | _parent_scene.actor_name_map[bbox] = (PhysicsActor)this; | ||
1458 | } | 1657 | } |
1459 | m_freemove = false; | 1658 | m_freemove = false; |
1659 | m_collisionException = false; | ||
1460 | m_pidControllerActive = true; | 1660 | m_pidControllerActive = true; |
1461 | } | 1661 | } |
1462 | else | 1662 | else |
@@ -1565,6 +1765,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1565 | 1765 | ||
1566 | if (Body != IntPtr.Zero) | 1766 | if (Body != IntPtr.Zero) |
1567 | d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); | 1767 | d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); |
1768 | ajustCollider(); | ||
1568 | } | 1769 | } |
1569 | 1770 | ||
1570 | private void donullchange() | 1771 | private void donullchange() |
@@ -1573,7 +1774,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1573 | 1774 | ||
1574 | public bool DoAChange(changes what, object arg) | 1775 | public bool DoAChange(changes what, object arg) |
1575 | { | 1776 | { |
1576 | if (topbox == IntPtr.Zero && what != changes.Add && what != changes.Remove) | 1777 | if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) |
1577 | { | 1778 | { |
1578 | return false; | 1779 | return false; |
1579 | } | 1780 | } |