diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 182 |
1 files changed, 104 insertions, 78 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index 1b25faf..bb04ea7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | |||
@@ -80,6 +80,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
80 | private Vector3 m_rotationalVelocity; | 80 | private Vector3 m_rotationalVelocity; |
81 | private Vector3 m_size; | 81 | private Vector3 m_size; |
82 | private Quaternion m_orientation; | 82 | private Quaternion m_orientation; |
83 | private Quaternion m_orientation2D; | ||
83 | private float m_mass = 80f; | 84 | private float m_mass = 80f; |
84 | public float m_density = 60f; | 85 | public float m_density = 60f; |
85 | private bool m_pidControllerActive = true; | 86 | private bool m_pidControllerActive = true; |
@@ -165,9 +166,10 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
165 | 166 | ||
166 | 167 | ||
167 | 168 | ||
168 | public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) | 169 | public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) |
169 | { | 170 | { |
170 | m_uuid = UUID.Random(); | 171 | m_uuid = UUID.Random(); |
172 | m_localID = localID; | ||
171 | 173 | ||
172 | timeStep = parent_scene.ODE_STEPSIZE; | 174 | timeStep = parent_scene.ODE_STEPSIZE; |
173 | invtimeStep = 1 / timeStep; | 175 | invtimeStep = 1 / timeStep; |
@@ -206,6 +208,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
206 | 208 | ||
207 | m_feetOffset = pfeetOffset; | 209 | m_feetOffset = pfeetOffset; |
208 | m_orientation = Quaternion.Identity; | 210 | m_orientation = Quaternion.Identity; |
211 | m_orientation2D = Quaternion.Identity; | ||
209 | m_density = density; | 212 | m_density = density; |
210 | 213 | ||
211 | // force lower density for testing | 214 | // force lower density for testing |
@@ -648,7 +651,6 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
648 | { | 651 | { |
649 | // fakeori = value; | 652 | // fakeori = value; |
650 | // givefakeori++; | 653 | // givefakeori++; |
651 | |||
652 | value.Normalize(); | 654 | value.Normalize(); |
653 | AddChange(changes.Orientation, value); | 655 | AddChange(changes.Orientation, value); |
654 | } | 656 | } |
@@ -969,78 +971,86 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
969 | if (m_collisionException) | 971 | if (m_collisionException) |
970 | return false; | 972 | return false; |
971 | 973 | ||
974 | Vector3 offset; | ||
975 | |||
972 | if (me == bbox) // if moving fast | 976 | if (me == bbox) // if moving fast |
973 | { | 977 | { |
974 | // force a full inelastic collision | 978 | // force a full inelastic collision |
975 | m_collisionException = true; | 979 | m_collisionException = true; |
976 | 980 | ||
977 | Vector3 off = m_size * 0.5f; | 981 | offset = m_size * m_orientation2D; |
978 | off.X += contact.depth; | 982 | |
979 | off.Y += contact.depth; | 983 | offset.X = (float)Math.Abs(offset.X) * 0.5f + contact.depth; |
980 | off.Z += contact.depth; | 984 | offset.Y = (float)Math.Abs(offset.Y) * 0.5f + contact.depth; |
985 | offset.Z = (float)Math.Abs(offset.Z) * 0.5f + contact.depth; | ||
986 | |||
981 | if (reverse) | 987 | if (reverse) |
982 | { | 988 | { |
983 | off.X *= -contact.normal.X; | 989 | offset.X *= -contact.normal.X; |
984 | off.Y *= -contact.normal.Y; | 990 | offset.Y *= -contact.normal.Y; |
985 | off.Z *= -contact.normal.Z; | 991 | offset.Z *= -contact.normal.Z; |
986 | } | 992 | } |
987 | else | 993 | else |
988 | { | 994 | { |
989 | off.X *= contact.normal.X; | 995 | offset.X *= contact.normal.X; |
990 | off.Y *= contact.normal.Y; | 996 | offset.Y *= contact.normal.Y; |
991 | off.Z *= contact.normal.Z; | 997 | offset.Z *= contact.normal.Z; |
992 | } | 998 | } |
993 | 999 | ||
994 | off.X += contact.pos.X; | 1000 | offset.X += contact.pos.X; |
995 | off.Y += contact.pos.Y; | 1001 | offset.Y += contact.pos.Y; |
996 | off.Z += contact.pos.Z; | 1002 | offset.Z += contact.pos.Z; |
997 | 1003 | ||
998 | _position = off; | 1004 | _position = offset; |
999 | return false; | 1005 | return false; |
1000 | } | 1006 | } |
1001 | 1007 | ||
1002 | if (me == topbox) // keep a box head | 1008 | offset.X = contact.pos.X - _position.X; |
1003 | return true; | 1009 | offset.Y = contact.pos.Y - _position.Y; |
1004 | 1010 | ||
1005 | float t; | 1011 | if (me == topbox) |
1006 | float offx = contact.pos.X - _position.X; | ||
1007 | float offy = contact.pos.Y - _position.Y; | ||
1008 | |||
1009 | if (me == midbox) | ||
1010 | { | 1012 | { |
1011 | if (Math.Abs(contact.normal.Z) > 0.95f) | 1013 | offset.Z = contact.pos.Z - _position.Z; |
1012 | { | ||
1013 | float nz = contact.normal.Z; | ||
1014 | if (!reverse) | ||
1015 | nz = -nz; | ||
1016 | 1014 | ||
1017 | if (nz > 0) | 1015 | offset.Normalize(); |
1018 | return true; // missed head TODO | ||
1019 | 1016 | ||
1020 | // missed feet collision? | 1017 | if (reverse) |
1021 | 1018 | { | |
1022 | 1019 | contact.normal.X = offset.X; | |
1023 | return true; | 1020 | contact.normal.Y = offset.Y; |
1021 | contact.normal.Z = offset.Z; | ||
1022 | } | ||
1023 | else | ||
1024 | { | ||
1025 | contact.normal.X = -offset.X; | ||
1026 | contact.normal.Y = -offset.Y; | ||
1027 | contact.normal.Z = -offset.Z; | ||
1024 | } | 1028 | } |
1029 | return true; | ||
1030 | } | ||
1025 | 1031 | ||
1026 | t = offx * offx + offy * offy; | 1032 | if (me == midbox) |
1027 | t = (float)Math.Sqrt(t); | 1033 | { |
1028 | t = 1 / t; | 1034 | if (Math.Abs(contact.normal.Z) > 0.95f) |
1029 | offx *= t; | 1035 | offset.Z = contact.pos.Z - _position.Z; |
1030 | offy *= t; | 1036 | else |
1037 | offset.Z = contact.normal.Z; | ||
1038 | |||
1039 | offset.Normalize(); | ||
1031 | 1040 | ||
1032 | if (reverse) | 1041 | if (reverse) |
1033 | { | 1042 | { |
1034 | contact.normal.X = offx; | 1043 | contact.normal.X = offset.X; |
1035 | contact.normal.Y = offy; | 1044 | contact.normal.Y = offset.Y; |
1045 | contact.normal.Z = offset.Z; | ||
1036 | } | 1046 | } |
1037 | else | 1047 | else |
1038 | { | 1048 | { |
1039 | contact.normal.X = -offx; | 1049 | contact.normal.X = -offset.X; |
1040 | contact.normal.Y = -offy; | 1050 | contact.normal.Y = -offset.Y; |
1051 | contact.normal.Z = -offset.Z; | ||
1041 | } | 1052 | } |
1042 | 1053 | ||
1043 | contact.normal.Z = 0; | ||
1044 | return true; | 1054 | return true; |
1045 | } | 1055 | } |
1046 | 1056 | ||
@@ -1062,39 +1072,33 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1062 | return true; | 1072 | return true; |
1063 | } | 1073 | } |
1064 | 1074 | ||
1075 | offset.Z = h - feetOff; // distance from top of feetbox | ||
1065 | 1076 | ||
1066 | float offz = h - feetOff; // distance from top of feetbox | 1077 | if (offset.Z > 0) |
1067 | |||
1068 | if (offz > 0) | ||
1069 | return false; | 1078 | return false; |
1070 | 1079 | ||
1071 | if (offz > -0.01) | 1080 | if (offset.Z > -0.01) |
1072 | { | 1081 | { |
1073 | offx = 0; | 1082 | offset.X = 0; |
1074 | offy = 0; | 1083 | offset.Y = 0; |
1075 | offz = -1.0f; | 1084 | offset.Z = -1.0f; |
1076 | } | 1085 | } |
1077 | else | 1086 | else |
1078 | { | 1087 | { |
1079 | t = offx * offx + offy * offy + offz * offz; | 1088 | offset.Normalize(); |
1080 | t = (float)Math.Sqrt(t); | ||
1081 | t = 1 / t; | ||
1082 | offx *= t; | ||
1083 | offy *= t; | ||
1084 | offz *= t; | ||
1085 | } | 1089 | } |
1086 | 1090 | ||
1087 | if (reverse) | 1091 | if (reverse) |
1088 | { | 1092 | { |
1089 | contact.normal.X = offx; | 1093 | contact.normal.X = offset.X; |
1090 | contact.normal.Y = offy; | 1094 | contact.normal.Y = offset.Y; |
1091 | contact.normal.Z = offz; | 1095 | contact.normal.Z = offset.Z; |
1092 | } | 1096 | } |
1093 | else | 1097 | else |
1094 | { | 1098 | { |
1095 | contact.normal.X = -offx; | 1099 | contact.normal.X = -offset.X; |
1096 | contact.normal.Y = -offy; | 1100 | contact.normal.Y = -offset.Y; |
1097 | contact.normal.Z = -offz; | 1101 | contact.normal.Z = -offset.Z; |
1098 | } | 1102 | } |
1099 | feetcollision = true; | 1103 | feetcollision = true; |
1100 | if (h < boneOff) | 1104 | if (h < boneOff) |
@@ -1124,7 +1128,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1124 | float v = _velocity.Length(); | 1128 | float v = _velocity.Length(); |
1125 | if (v != 0) | 1129 | if (v != 0) |
1126 | { | 1130 | { |
1127 | v = 6.0f / v; | 1131 | v = 5.0f / v; |
1128 | _velocity = _velocity * v; | 1132 | _velocity = _velocity * v; |
1129 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | 1133 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); |
1130 | } | 1134 | } |
@@ -1140,10 +1144,10 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1140 | // so force it back to identity | 1144 | // so force it back to identity |
1141 | 1145 | ||
1142 | d.Quaternion qtmp; | 1146 | d.Quaternion qtmp; |
1143 | qtmp.W = m_orientation.W; | 1147 | qtmp.W = m_orientation2D.W; |
1144 | qtmp.X = m_orientation.X; | 1148 | qtmp.X = m_orientation2D.X; |
1145 | qtmp.Y = m_orientation.Y; | 1149 | qtmp.Y = m_orientation2D.Y; |
1146 | qtmp.Z = m_orientation.Z; | 1150 | qtmp.Z = m_orientation2D.Z; |
1147 | d.BodySetQuaternion(Body, ref qtmp); | 1151 | d.BodySetQuaternion(Body, ref qtmp); |
1148 | 1152 | ||
1149 | if (m_pidControllerActive == false) | 1153 | if (m_pidControllerActive == false) |
@@ -1209,7 +1213,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1209 | 1213 | ||
1210 | d.AABB aabb; | 1214 | d.AABB aabb; |
1211 | d.GeomGetAABB(feetbox, out aabb); | 1215 | d.GeomGetAABB(feetbox, out aabb); |
1212 | float chrminZ = aabb.MinZ - 0.02f; // move up a bit | 1216 | float chrminZ = aabb.MinZ; ; // move up a bit |
1213 | Vector3 posch = localpos; | 1217 | Vector3 posch = localpos; |
1214 | 1218 | ||
1215 | float ftmp; | 1219 | float ftmp; |
@@ -1252,7 +1256,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1252 | contact.PenetrationDepth = depth; | 1256 | contact.PenetrationDepth = depth; |
1253 | contact.Position.X = localpos.X; | 1257 | contact.Position.X = localpos.X; |
1254 | contact.Position.Y = localpos.Y; | 1258 | contact.Position.Y = localpos.Y; |
1255 | contact.Position.Z = chrminZ; | 1259 | contact.Position.Z = terrainheight; |
1256 | contact.SurfaceNormal.X = 0.0f; | 1260 | contact.SurfaceNormal.X = 0.0f; |
1257 | contact.SurfaceNormal.Y = 0.0f; | 1261 | contact.SurfaceNormal.Y = 0.0f; |
1258 | contact.SurfaceNormal.Z = -1f; | 1262 | contact.SurfaceNormal.Z = -1f; |
@@ -1676,14 +1680,36 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1676 | 1680 | ||
1677 | private void changeOrientation(Quaternion newOri) | 1681 | private void changeOrientation(Quaternion newOri) |
1678 | { | 1682 | { |
1679 | d.Quaternion myrot = new d.Quaternion(); | 1683 | if (m_orientation != newOri) |
1680 | myrot.X = newOri.X; | 1684 | { |
1681 | myrot.Y = newOri.Y; | 1685 | m_orientation = newOri; // keep a copy for core use |
1682 | myrot.Z = newOri.Z; | 1686 | // but only use rotations around Z |
1683 | myrot.W = newOri.W; | 1687 | |
1684 | float t = d.JointGetAMotorAngle(Amotor, 2); | 1688 | m_orientation2D.W = newOri.W; |
1685 | d.BodySetQuaternion(Body,ref myrot); | 1689 | m_orientation2D.Z = newOri.Z; |
1686 | m_orientation = newOri; | 1690 | |
1691 | float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; | ||
1692 | if (t > 0) | ||
1693 | { | ||
1694 | t = 1.0f / (float)Math.Sqrt(t); | ||
1695 | m_orientation2D.W *= t; | ||
1696 | m_orientation2D.Z *= t; | ||
1697 | } | ||
1698 | else | ||
1699 | { | ||
1700 | m_orientation2D.W = 1.0f; | ||
1701 | m_orientation2D.Z = 0f; | ||
1702 | } | ||
1703 | m_orientation2D.Y = 0f; | ||
1704 | m_orientation2D.X = 0f; | ||
1705 | |||
1706 | d.Quaternion myrot = new d.Quaternion(); | ||
1707 | myrot.X = m_orientation2D.X; | ||
1708 | myrot.Y = m_orientation2D.Y; | ||
1709 | myrot.Z = m_orientation2D.Z; | ||
1710 | myrot.W = m_orientation2D.W; | ||
1711 | d.BodySetQuaternion(Body, ref myrot); | ||
1712 | } | ||
1687 | } | 1713 | } |
1688 | 1714 | ||
1689 | private void changeVelocity(Vector3 newVel) | 1715 | private void changeVelocity(Vector3 newVel) |