aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Environment/Scenes/SceneObjectPart.cs')
-rw-r--r--OpenSim/Region/Environment/Scenes/SceneObjectPart.cs229
1 files changed, 209 insertions, 20 deletions
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 51a633e..ddfa332 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -933,29 +933,60 @@ namespace OpenSim.Region.Environment.Scenes
933 933
934 return returnresult; 934 return returnresult;
935 } 935 }
936 public EntityIntersection TestIntersectionOABB(Ray iray, Quaternion parentrot) 936
937 public double GetDistanceTo(Vector3 a, Vector3 b)
938 {
939 float dx = a.x - b.x;
940 float dy = a.y - b.y;
941 float dz = a.z - b.z;
942 return Math.Sqrt(dx * dx + dy * dy + dz * dz);
943 }
944
945 public EntityIntersection TestIntersectionOBB(Ray iray, Quaternion parentrot)
937 { 946 {
938 // In this case we're using a rectangular prism, which has 6 faces and therefore 6 planes 947 // In this case we're using a rectangular prism, which has 6 faces and therefore 6 planes
939 // This breaks down into the ray---> plane equation. 948 // This breaks down into the ray---> plane equation.
940 // TODO: Change to take shape into account 949 // TODO: Change to take shape into account
941 Vector3[] vertexes = new Vector3[8]; 950 Vector3[] vertexes = new Vector3[8];
942
943 Vector3[] FaceA = new Vector3[6];
944 Vector3[] FaceB = new Vector3[6];
945 Vector3[] FaceC = new Vector3[6];
946 Vector3[] FaceD = new Vector3[6];
947 951
948 Vector3[] normals = new Vector3[6]; 952 float[] distance = new float[6];
953 Vector3[] FaceA = new Vector3[6]; // vertex A for Facei
954 Vector3[] FaceB = new Vector3[6]; // vertex B for Facei
955 Vector3[] FaceC = new Vector3[6]; // vertex C for Facei
956 Vector3[] FaceD = new Vector3[6]; // vertex D for Facei
957
958 Vector3[] normals = new Vector3[6]; // Normal for Facei
949 959
950 Vector3 AmBa = new Vector3(0, 0, 0); 960 Vector3 AmBa = new Vector3(0, 0, 0); // Vertex A - Vertex B
951 Vector3 AmBb = new Vector3(0, 0, 0); 961 Vector3 AmBb = new Vector3(0, 0, 0); // Vertex B - Vertex C
962 Vector3 cross = new Vector3();
952 963
953 LLVector3 pos = GetWorldPosition(); 964 LLVector3 pos = GetWorldPosition();
954 LLQuaternion rot = GetWorldRotation(); 965 LLQuaternion rot = GetWorldRotation();
966
967 // Variables prefixed with AX are Axiom.Math copies of the LL variety.
968
955 Quaternion AXrot = new Quaternion(rot.W,rot.X,rot.Y,rot.Z); 969 Quaternion AXrot = new Quaternion(rot.W,rot.X,rot.Y,rot.Z);
970 AXrot.Normalize();
971
972 Vector3 AXpos = new Vector3(pos.X, pos.Y, pos.Z);
973
974 // tScale is the offset to derive the vertex based on the scale.
975 // it's different for each vertex because we've got to rotate it
976 // to get the world position of the vertex to produce the Oriented Bounding Box
977
978 Vector3 tScale = new Vector3();
956 979
980 Vector3 AXscale = new Vector3(m_shape.Scale.X * 0.5f, m_shape.Scale.Y * 0.5f, m_shape.Scale.Z * 0.5f);
981
982 //Vector3 pScale = (AXscale) - (AXrot.Inverse() * (AXscale));
983 //Vector3 nScale = (AXscale * -1) - (AXrot.Inverse() * (AXscale * -1));
957 984
985 // rScale is the rotated offset to find a vertex based on the scale and the world rotation.
986 Vector3 rScale = new Vector3();
987
958 // Get Vertexes for Faces Stick them into ABCD for each Face 988 // Get Vertexes for Faces Stick them into ABCD for each Face
989 // Form: Face<vertex>[face] that corresponds to the below diagram
959 #region ABCD Face Vertex Map Comment Diagram 990 #region ABCD Face Vertex Map Comment Diagram
960 // A _________ B 991 // A _________ B
961 // | | 992 // | |
@@ -987,64 +1018,222 @@ namespace OpenSim.Region.Environment.Scenes
987 // |_________| 1018 // |_________|
988 // A B 1019 // A B
989 #endregion 1020 #endregion
990 vertexes[0] = (AXrot * new Vector3((pos.X - m_shape.Scale.X),(pos.Y - m_shape.Scale.Y),(pos.Z + m_shape.Scale.Z))); 1021
1022 #region Plane Decomposition of Oriented Bounding Box
1023 tScale = new Vector3(AXscale.x, -AXscale.y, AXscale.z);
1024 rScale = ((AXrot * tScale));
1025 vertexes[0] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1026 // vertexes[0].x = pos.X + vertexes[0].x;
1027 //vertexes[0].y = pos.Y + vertexes[0].y;
1028 //vertexes[0].z = pos.Z + vertexes[0].z;
991 1029
992 FaceA[0] = vertexes[0]; 1030 FaceA[0] = vertexes[0];
993 FaceA[3] = vertexes[0]; 1031 FaceA[3] = vertexes[0];
994 FaceA[4] = vertexes[0]; 1032 FaceA[4] = vertexes[0];
995 1033
996 vertexes[1] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z))); 1034 tScale = AXscale;
1035 rScale = ((AXrot * tScale));
1036 vertexes[1] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1037
1038 // vertexes[1].x = pos.X + vertexes[1].x;
1039 // vertexes[1].y = pos.Y + vertexes[1].y;
1040 //vertexes[1].z = pos.Z + vertexes[1].z;
997 1041
998 FaceB[0] = vertexes[1]; 1042 FaceB[0] = vertexes[1];
999 FaceA[1] = vertexes[1]; 1043 FaceA[1] = vertexes[1];
1000 FaceC[4] = vertexes[1]; 1044 FaceC[4] = vertexes[1];
1001 1045
1002 vertexes[2] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z))); 1046 tScale = new Vector3(AXscale.x, -AXscale.y, -AXscale.z);
1047 rScale = ((AXrot * tScale));
1048
1049 vertexes[2] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1050
1051 //vertexes[2].x = pos.X + vertexes[2].x;
1052 //vertexes[2].y = pos.Y + vertexes[2].y;
1053 //vertexes[2].z = pos.Z + vertexes[2].z;
1003 1054
1004 FaceC[0] = vertexes[2]; 1055 FaceC[0] = vertexes[2];
1005 FaceC[3] = vertexes[2]; 1056 FaceC[3] = vertexes[2];
1006 FaceC[5] = vertexes[2]; 1057 FaceC[5] = vertexes[2];
1007 1058
1008 vertexes[3] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z))); 1059 tScale = new Vector3(AXscale.x, AXscale.y, -AXscale.z);
1060 rScale = ((AXrot * tScale));
1061 vertexes[3] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1062
1063 //vertexes[3].x = pos.X + vertexes[3].x;
1064 // vertexes[3].y = pos.Y + vertexes[3].y;
1065 // vertexes[3].z = pos.Z + vertexes[3].z;
1009 1066
1010 FaceD[0] = vertexes[3]; 1067 FaceD[0] = vertexes[3];
1011 FaceC[1] = vertexes[3]; 1068 FaceC[1] = vertexes[3];
1012 FaceA[5] = vertexes[3]; 1069 FaceA[5] = vertexes[3];
1013 1070
1014 vertexes[4] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z))); 1071 tScale = new Vector3(-AXscale.x, AXscale.y, AXscale.z);
1072 rScale = ((AXrot * tScale));
1073 vertexes[4] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1074
1075 // vertexes[4].x = pos.X + vertexes[4].x;
1076 // vertexes[4].y = pos.Y + vertexes[4].y;
1077 // vertexes[4].z = pos.Z + vertexes[4].z;
1015 1078
1016 FaceB[1] = vertexes[4]; 1079 FaceB[1] = vertexes[4];
1017 FaceA[2] = vertexes[4]; 1080 FaceA[2] = vertexes[4];
1018 FaceD[4] = vertexes[4]; 1081 FaceD[4] = vertexes[4];
1019 1082
1020 vertexes[5] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z))); 1083 tScale = new Vector3(-AXscale.x, AXscale.y, -AXscale.z);
1084 rScale = ((AXrot * tScale));
1085 vertexes[5] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1086
1087 // vertexes[5].x = pos.X + vertexes[5].x;
1088 // vertexes[5].y = pos.Y + vertexes[5].y;
1089 // vertexes[5].z = pos.Z + vertexes[5].z;
1021 1090
1022 FaceD[1] = vertexes[5]; 1091 FaceD[1] = vertexes[5];
1023 FaceC[2] = vertexes[5]; 1092 FaceC[2] = vertexes[5];
1024 FaceB[5] = vertexes[5]; 1093 FaceB[5] = vertexes[5];
1025 1094
1026 vertexes[6] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z))); 1095 tScale = new Vector3(-AXscale.x, -AXscale.y, AXscale.z);
1096 rScale = ((AXrot * tScale));
1097 vertexes[6] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1098
1099 // vertexes[6].x = pos.X + vertexes[6].x;
1100 // vertexes[6].y = pos.Y + vertexes[6].y;
1101 // vertexes[6].z = pos.Z + vertexes[6].z;
1027 1102
1028 FaceB[2] = vertexes[6]; 1103 FaceB[2] = vertexes[6];
1029 FaceB[3] = vertexes[6]; 1104 FaceB[3] = vertexes[6];
1030 FaceB[4] = vertexes[6]; 1105 FaceB[4] = vertexes[6];
1031 1106
1032 vertexes[7] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z))); 1107 tScale = new Vector3(-AXscale.x, -AXscale.y, -AXscale.z);
1108 rScale = ((AXrot * tScale));
1109 vertexes[7] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
1110
1111 // vertexes[7].x = pos.X + vertexes[7].x;
1112 // vertexes[7].y = pos.Y + vertexes[7].y;
1113 // vertexes[7].z = pos.Z + vertexes[7].z;
1033 1114
1034 FaceD[2] = vertexes[7]; 1115 FaceD[2] = vertexes[7];
1035 FaceD[3] = vertexes[7]; 1116 FaceD[3] = vertexes[7];
1036 FaceD[5] = vertexes[7]; 1117 FaceD[5] = vertexes[7];
1118 #endregion
1037 1119
1038 // Get our plane normals 1120 // Get our plane normals
1039 for (int i = 0; i < 6; i++) 1121 for (int i = 0; i < 6; i++)
1040 { 1122 {
1041 AmBa = FaceB[i] - FaceA[i]; 1123 //m_log.Info("[FACECALCULATION]: FaceA[" + i + "]=" + FaceA[i] + " FaceB[" + i + "]=" + FaceB[i] + " FaceC[" + i + "]=" + FaceC[i] + " FaceD[" + i + "]=" + FaceD[i]);
1042 AmBb = FaceC[i] - FaceA[i]; 1124
1043 normals[i] = AmBa.Cross(AmBb); 1125 // Our Plane direction
1126 AmBa = FaceA[i] - FaceB[i];
1127 AmBb = FaceB[i] - FaceC[i];
1128
1129 cross = AmBb.Cross(AmBa);
1130
1131 // normalize the cross product to get the normal.
1132 normals[i] = cross / cross.Length;
1133
1134 //m_log.Info("[NORMALS]: normals[ " + i + "]" + normals[i].ToString());
1135 //distance[i] = (normals[i].x * AmBa.x + normals[i].y * AmBa.y + normals[i].z * AmBa.z) * -1;
1044 } 1136 }
1045 1137
1046 EntityIntersection returnresult = new EntityIntersection(); 1138 EntityIntersection returnresult = new EntityIntersection();
1139
1140 returnresult.distance = 1024;
1141 float c = 0;
1142 float a = 0;
1143 float d = 0;
1144 Vector3 q = new Vector3();
1145
1146 #region OBB Version 2 Experiment
1147 //float fmin = 999999;
1148 //float fmax = -999999;
1149 //float s = 0;
1150
1151 //for (int i=0;i<6;i++)
1152 //{
1153 //s = iray.Direction.Dot(normals[i]);
1154 //d = normals[i].Dot(FaceB[i]);
1155
1156 //if (s == 0)
1157 //{
1158 //if (iray.Origin.Dot(normals[i]) > d)
1159 //{
1160 //return returnresult;
1161 //}
1162 // else
1163 //{
1164 //continue;
1165 //}
1166 //}
1167 //a = (d - iray.Origin.Dot(normals[i])) / s;
1168 //if ( iray.Direction.Dot(normals[i]) < 0)
1169 //{
1170 //if (a > fmax)
1171 //{
1172 //if (a > fmin)
1173 //{
1174 //return returnresult;
1175 //}
1176 //fmax = a;
1177 //}
1178
1179 //}
1180 //else
1181 //{
1182 //if (a < fmin)
1183 //{
1184 //if (a < 0 || a < fmax)
1185 //{
1186 //return returnresult;
1187 //}
1188 //fmin = a;
1189 //}
1190 //}
1191 //}
1192 //if (fmax > 0)
1193 // a= fmax;
1194 //else
1195 // a=fmin;
1196
1197 //q = iray.Origin + a * iray.Direction;
1198 #endregion
1047 1199
1200 // Loop over faces (6 of them)
1201 for (int i = 0; i < 6; i++)
1202 {
1203 AmBa = FaceA[i] - FaceB[i];
1204 AmBb = FaceB[i] - FaceC[i];
1205 d = normals[i].Dot(FaceB[i]);
1206 c = iray.Direction.Dot(normals[i]);
1207 if (c == 0)
1208 continue;
1209
1210 a = (d - iray.Origin.Dot(normals[i])) / c;
1211
1212 if (a < 0)
1213 continue;
1214
1215 // If the normal is pointing outside the object
1216 if (iray.Direction.Dot(normals[i]) < 0)
1217 {
1218
1219 q = iray.Origin + a * iray.Direction;
1220
1221 // Is this the closest hit to the object's origin?
1222 //float distance2 = (float)GetDistanceTo(q, iray.Origin);
1223 float distance2 = (float)GetDistanceTo(q, AXpos);
1224
1225 if (distance2 < returnresult.distance)
1226 {
1227 returnresult.distance = distance2;
1228 returnresult.HitTF = true;
1229 returnresult.ipoint = q;
1230 //m_log.Info("[POINT]: " + q.ToString());
1231 returnresult.normal = 1;
1232
1233 }
1234 }
1235
1236 }
1048 return returnresult; 1237 return returnresult;
1049 } 1238 }
1050 1239