public EntityIntersection TestIntersectionOBB(Ray iray, Quaternion parentrot, bool frontFacesOnly,
bool faceCenters)
{
// In this case we're using a rectangular prism, which has 6 faces and therefore 6 planes
// This breaks down into the ray---> plane equation.
// TODO: Change to take shape into account
Vector3[] vertexes = new Vector3[8];
// float[] distance = new float[6];
Vector3[] FaceA = new Vector3[6]; // vertex A for Facei
Vector3[] FaceB = new Vector3[6]; // vertex B for Facei
Vector3[] FaceC = new Vector3[6]; // vertex C for Facei
Vector3[] FaceD = new Vector3[6]; // vertex D for Facei
Vector3[] normals = new Vector3[6]; // Normal for Facei
Vector3[] AAfacenormals = new Vector3[6]; // Axis Aligned face normals
AAfacenormals[0] = new Vector3(1, 0, 0);
AAfacenormals[1] = new Vector3(0, 1, 0);
AAfacenormals[2] = new Vector3(-1, 0, 0);
AAfacenormals[3] = new Vector3(0, -1, 0);
AAfacenormals[4] = new Vector3(0, 0, 1);
AAfacenormals[5] = new Vector3(0, 0, -1);
Vector3 AmBa = new Vector3(0, 0, 0); // Vertex A - Vertex B
Vector3 AmBb = new Vector3(0, 0, 0); // Vertex B - Vertex C
Vector3 cross = new Vector3();
Vector3 pos = GetWorldPosition();
Quaternion rot = GetWorldRotation();
// Variables prefixed with AX are Axiom.Math copies of the LL variety.
Quaternion AXrot = rot;
AXrot.Normalize();
Vector3 AXpos = pos;
// tScale is the offset to derive the vertex based on the scale.
// it's different for each vertex because we've got to rotate it
// to get the world position of the vertex to produce the Oriented Bounding Box
Vector3 tScale = Vector3.Zero;
Vector3 AXscale = new Vector3(m_shape.Scale.X*0.5f, m_shape.Scale.Y*0.5f, m_shape.Scale.Z*0.5f);
//Vector3 pScale = (AXscale) - (AXrot.Inverse() * (AXscale));
//Vector3 nScale = (AXscale * -1) - (AXrot.Inverse() * (AXscale * -1));
// rScale is the rotated offset to find a vertex based on the scale and the world rotation.
Vector3 rScale = new Vector3();
// Get Vertexes for Faces Stick them into ABCD for each Face
// Form: Face<vertex>[face] that corresponds to the below diagram
#region ABCD Face Vertex Map Comment Diagram
// A _________ B
// | |
// | 4 top |
// |_________|
// C D
// A _________ B
// | Back |
// | 3 |
// |_________|
// C D
// A _________ B B _________ A
// | Left | | Right |
// | 0 | | 2 |
// |_________| |_________|
// C D D C
// A _________ B
// | Front |
// | 1 |
// |_________|
// C D
// C _________ D
// | |
// | 5 bot |
// |_________|
// A B
#endregion
#region Plane Decomposition of Oriented Bounding Box
tScale = new Vector3(AXscale.X, -AXscale.Y, AXscale.Z);
rScale = tScale*AXrot;
vertexes[0] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[0].X = pos.X + vertexes[0].X;
//vertexes[0].Y = pos.Y + vertexes[0].Y;
//vertexes[0].Z = pos.Z + vertexes[0].Z;
FaceA[0] = vertexes[0];
FaceB[3] = vertexes[0];
FaceA[4] = vertexes[0];
tScale = AXscale;
rScale = tScale*AXrot;
vertexes[1] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[1].X = pos.X + vertexes[1].X;
// vertexes[1].Y = pos.Y + vertexes[1].Y;
//vertexes[1].Z = pos.Z + vertexes[1].Z;
FaceB[0] = vertexes[1];
FaceA[1] = vertexes[1];
FaceC[4] = vertexes[1];
tScale = new Vector3(AXscale.X, -AXscale.Y, -AXscale.Z);
rScale = tScale*AXrot;
vertexes[2] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
//vertexes[2].X = pos.X + vertexes[2].X;
//vertexes[2].Y = pos.Y + vertexes[2].Y;
//vertexes[2].Z = pos.Z + vertexes[2].Z;
FaceC[0] = vertexes[2];
FaceD[3] = vertexes[2];
FaceC[5] = vertexes[2];
tScale = new Vector3(AXscale.X, AXscale.Y, -AXscale.Z);
rScale = tScale*AXrot;
vertexes[3] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
//vertexes[3].X = pos.X + vertexes[3].X;
// vertexes[3].Y = pos.Y + vertexes[3].Y;
// vertexes[3].Z = pos.Z + vertexes[3].Z;
FaceD[0] = vertexes[3];
FaceC[1] = vertexes[3];
FaceA[5] = vertexes[3];
tScale = new Vector3(-AXscale.X, AXscale.Y, AXscale.Z);
rScale = tScale*AXrot;
vertexes[4] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[4].X = pos.X + vertexes[4].X;
// vertexes[4].Y = pos.Y + vertexes[4].Y;
// vertexes[4].Z = pos.Z + vertexes[4].Z;
FaceB[1] = vertexes[4];
FaceA[2] = vertexes[4];
FaceD[4] = vertexes[4];
tScale = new Vector3(-AXscale.X, AXscale.Y, -AXscale.Z);
rScale = tScale*AXrot;
vertexes[5] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[5].X = pos.X + vertexes[5].X;
// vertexes[5].Y = pos.Y + vertexes[5].Y;
// vertexes[5].Z = pos.Z + vertexes[5].Z;
FaceD[1] = vertexes[5];
FaceC[2] = vertexes[5];
FaceB[5] = vertexes[5];
tScale = new Vector3(-AXscale.X, -AXscale.Y, AXscale.Z);
rScale = tScale*AXrot;
vertexes[6] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[6].X = pos.X + vertexes[6].X;
// vertexes[6].Y = pos.Y + vertexes[6].Y;
// vertexes[6].Z = pos.Z + vertexes[6].Z;
FaceB[2] = vertexes[6];
FaceA[3] = vertexes[6];
FaceB[4] = vertexes[6];
tScale = new Vector3(-AXscale.X, -AXscale.Y, -AXscale.Z);
rScale = tScale*AXrot;
vertexes[7] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
// vertexes[7].X = pos.X + vertexes[7].X;
// vertexes[7].Y = pos.Y + vertexes[7].Y;
// vertexes[7].Z = pos.Z + vertexes[7].Z;
FaceD[2] = vertexes[7];
FaceC[3] = vertexes[7];
FaceD[5] = vertexes[7];
#endregion
// Get our plane normals
for (int i = 0; i < 6; i++)
{
//MainConsole.Instance.Info("[FACECALCULATION]: FaceA[" + i + "]=" + FaceA[i] + " FaceB[" + i + "]=" + FaceB[i] + " FaceC[" + i + "]=" + FaceC[i] + " FaceD[" + i + "]=" + FaceD[i]);
// Our Plane direction
AmBa = FaceA[i] - FaceB[i];
AmBb = FaceB[i] - FaceC[i];
cross = Vector3.Cross(AmBb, AmBa);
// normalize the cross product to get the normal.
normals[i] = cross/cross.Length();
//MainConsole.Instance.Info("[NORMALS]: normals[ " + i + "]" + normals[i].ToString());
//distance[i] = (normals[i].X * AmBa.X + normals[i].Y * AmBa.Y + normals[i].Z * AmBa.Z) * -1;
}
EntityIntersection result = new EntityIntersection {distance = 1024};
float c = 0;
float a = 0;
float d = 0;
Vector3 q = new Vector3();
#region OBB Version 2 Experiment
//float fmin = 999999;
//float fmax = -999999;
//float s = 0;
//for (int i=0;i<6;i++)
//{
//s = iray.Direction.Dot(normals[i]);
//d = normals[i].Dot(FaceB[i]);
//if (s == 0)
//{
//if (iray.Origin.Dot(normals[i]) > d)
//{
//return result;
//}
// else
//{
//continue;
//}
//}
//a = (d - iray.Origin.Dot(normals[i])) / s;
//if (iray.Direction.Dot(normals[i]) < 0)
//{
//if (a > fmax)
//{
//if (a > fmin)
//{
//return result;
//}
//fmax = a;
//}
//}
//else
//{
//if (a < fmin)
//{
//if (a < 0 || a < fmax)
//{
//return result;
//}
//fmin = a;
//}
//}
//}
//if (fmax > 0)
// a= fmax;
//else
// a=fmin;
//q = iray.Origin + a * iray.Direction;
#endregion
// Loop over faces (6 of them)
for (int i = 0; i < 6; i++)
{
AmBa = FaceA[i] - FaceB[i];
AmBb = FaceB[i] - FaceC[i];
d = Vector3.Dot(normals[i], FaceB[i]);
//if (faceCenters)
//{
// c = normals[i].Dot(normals[i]);
//}
//else
//{
c = Vector3.Dot(iray.Direction, normals[i]);
//}
if (c == 0)
continue;
a = (d - Vector3.Dot(iray.Origin, normals[i]))/c;
if (a < 0)
continue;
// If the normal is pointing outside the object
if (Vector3.Dot(iray.Direction, normals[i]) < 0 || !frontFacesOnly)
{
//if (faceCenters)
//{ //(FaceA[i] + FaceB[i] + FaceC[1] + FaceD[i]) / 4f;
// q = iray.Origin + a * normals[i];
//}
//else
//{
q = iray.Origin + iray.Direction*a;
//}
float distance2 = (float) GetDistanceTo(q, AXpos);
// Is this the closest hit to the object's origin?
//if (faceCenters)
//{
// distance2 = (float)GetDistanceTo(q, iray.Origin);
//}
if (distance2 < result.distance)
{
result.distance = distance2;
result.HitTF = true;
result.ipoint = q;
//MainConsole.Instance.Info("[FACE]:" + i.ToString());
//MainConsole.Instance.Info("[POINT]: " + q.ToString());
//MainConsole.Instance.Info("[DIST]: " + distance2.ToString());
if (faceCenters)
{
result.normal = AAfacenormals[i]*AXrot;
Vector3 scaleComponent = AAfacenormals[i];
float ScaleOffset = 0.5f;
if (scaleComponent.X != 0) ScaleOffset = AXscale.X;
if (scaleComponent.Y != 0) ScaleOffset = AXscale.Y;
if (scaleComponent.Z != 0) ScaleOffset = AXscale.Z;
ScaleOffset = Math.Abs(ScaleOffset);
Vector3 offset = result.normal*ScaleOffset;
result.ipoint = AXpos + offset;
//pos = (intersectionpoint + offset);
}
else
{
result.normal = normals[i];
}
result.AAfaceNormal = AAfacenormals[i];
}
}
}
return result;
}
/// <summary>
/// Get a scene object group that contains the prim with the given uuid
/// </summary>
/// <param name="hray"></param>
/// <param name="frontFacesOnly"></param>
/// <param name="faceCenters"></param>
/// <returns>null if no scene object group containing that prim is found</returns>
protected internal EntityIntersection GetClosestIntersectingPrim(Ray hray, bool frontFacesOnly, bool faceCenters)
{
// Primitive Ray Tracing
float closestDistance = 280f;
EntityIntersection result = new EntityIntersection();
ISceneEntity[] EntityList = Entities.GetEntities(hray.Origin, closestDistance);
foreach (ISceneEntity ent in EntityList)
{
if (ent is SceneObjectGroup)
{
SceneObjectGroup reportingG = (SceneObjectGroup) ent;
EntityIntersection inter = reportingG.TestIntersection(hray, frontFacesOnly, faceCenters);
if (inter.HitTF && inter.distance < closestDistance)
{
closestDistance = inter.distance;
result = inter;
}
}
}
return result;
}
/// <summary>
/// Gets a list of scene object group that intersect with the given ray
/// </summary>
public List<EntityIntersection> GetIntersectingPrims(Ray hray, float length, int count,
bool frontFacesOnly, bool faceCenters, bool getAvatars,
bool getLand, bool getPrims)
{
// Primitive Ray Tracing
List<EntityIntersection> result = new List<EntityIntersection>(count);
if (getPrims)
{
ISceneEntity[] EntityList = Entities.GetEntities(hray.Origin, length);
result.AddRange(
EntityList.OfType<SceneObjectGroup>()
.Select(reportingG => reportingG.TestIntersection(hray, frontFacesOnly, faceCenters))
.Where(inter => inter.HitTF));
}
if (getAvatars)
{
List<IScenePresence> presenceList = Entities.GetPresences();
foreach (IScenePresence ent in presenceList)
{
//Do rough approximation and keep the # of loops down
Vector3 newPos = hray.Origin;
for (int i = 0; i < 100; i++)
{
newPos += ((Vector3.One*(length*(i/100)))*hray.Direction);
if (ent.AbsolutePosition.ApproxEquals(newPos, ent.PhysicsActor.Size.X*2))
{
EntityIntersection intersection = new EntityIntersection();
intersection.distance = length*(i/100);
intersection.face = 0;
intersection.HitTF = true;
intersection.obj = ent;
intersection.ipoint = newPos;
intersection.normal = newPos;
result.Add(intersection);
break;
}
}
}
}
if (getLand)
{
//TODO
}
result.Sort((a, b) => a.distance.CompareTo(b.distance));
if (result.Count > count)
result.RemoveRange(count, result.Count - count);
return result;
}
public EntityIntersection TestIntersection(Ray hRay, bool frontFacesOnly, bool faceCenters)
{
// We got a request from the inner_scene to raytrace along the Ray hRay
// We're going to check all of the prim in this group for intersection with the ray
// If we get a result, we're going to find the closest result to the origin of the ray
// and send back the intersection information back to the innerscene.
EntityIntersection result = new EntityIntersection();
foreach (SceneObjectPart part in m_partsList)
{
// Temporary commented to stop compiler warning
//Vector3 partPosition =
// new Vector3(part.AbsolutePosition.X, part.AbsolutePosition.Y, part.AbsolutePosition.Z);
Quaternion parentrotation = GroupRotation;
// Telling the prim to raytrace.
//EntityIntersection inter = part.TestIntersection(hRay, parentrotation);
EntityIntersection inter = part.TestIntersectionOBB(hRay, parentrotation, frontFacesOnly, faceCenters);
// This may need to be updated to the maximum draw distance possible..
// We might (and probably will) be checking for prim creation from other sims
// when the camera crosses the border.
if (m_scene != null)
{
float idist = (m_scene.RegionInfo.RegionSizeX + m_scene.RegionInfo.RegionSizeY)/2;
if (inter.HitTF)
{
// We need to find the closest prim to return to the testcaller along the ray
if (inter.distance < idist)
{
result.HitTF = true;
result.ipoint = inter.ipoint;
result.obj = part;
result.normal = inter.normal;
result.distance = inter.distance;
}
}
}
}
return result;
}