aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
diff options
context:
space:
mode:
authorUbitUmarov2012-12-11 04:36:27 +0000
committerUbitUmarov2012-12-11 04:36:27 +0000
commit80639ace95089414423b16f90c8994f50e010373 (patch)
tree961834085922f4e585344faebf5204eaa2c7d0e0 /OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
parent typo fix (diff)
downloadopensim-SC-80639ace95089414423b16f90c8994f50e010373.zip
opensim-SC-80639ace95089414423b16f90c8994f50e010373.tar.gz
opensim-SC-80639ace95089414423b16f90c8994f50e010373.tar.bz2
opensim-SC-80639ace95089414423b16f90c8994f50e010373.tar.xz
a few more changes on avatar collider
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs347
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 }