| public TestIntersectionOBB ( Ray iray, Quaternion parentrot, bool frontFacesOnly, bool faceCenters ) : EntityIntersection | ||
| iray | Ray | |
| parentrot | Quaternion | |
| frontFacesOnly | bool | |
| faceCenters | bool | |
| return | EntityIntersection | |
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++)
{
//m_log.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();
//m_log.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();
result.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;
//m_log.Info("[FACE]:" + i.ToString());
//m_log.Info("[POINT]: " + q.ToString());
//m_log.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>
/// Find a point on a prim which is the reflection point of 2 points (pointFrom and pointTo).
/// This method uses the concept of "angle of incident = angle of rreflection" then solve
/// that equation.
/// </summary>
/// <param name="pointFrom">Source</param>
/// <param name="pointTo">Target</param>
/// <param name="part">a prim which is used as a reflection surface</param>
/// <returns></returns>
public ReflectionPoint findReflectionPoint(Vector3 pointFrom, Vector3 pointTo, SceneObjectPart part)
{
float x = 0, y = 0, z = 0;
bool xIsZero = true, yIsZero = true, zIsZero = true;
//Get a vector from start to the prim absolute position. Use that vector to test intersection
//and get the normal of the prim.
Vector3 direction = part.AbsolutePosition - pointFrom;
EntityIntersection intersection = part.TestIntersectionOBB(new Ray(pointFrom, direction), part.ParentGroup.GroupRotation, true, false);
//Return false if the ray hits nothing.
if (!intersection.HitTF)
return new ReflectionPoint();
if (intersection.normal.X != 0) { xIsZero = false; }
if (intersection.normal.Y != 0) { yIsZero = false; }
if (intersection.normal.Z != 0) { zIsZero = false; }
//Note: We can get rid of the first three if(else) statement below. But for computation result,
//it is better to use them as the 'last else' statement is quite computationally intensive.
/*
if (!xIsZero && yIsZero && zIsZero) //{1, 0, 0}
{
x = intersection.ipoint.X;
y = solvePointEquation(pointFrom.Y, pointTo.Y, x, pointFrom.X, pointTo.X);
z = solvePointEquation(pointFrom.Z, pointTo.Z, x, pointFrom.X, pointTo.X);
}
else if (xIsZero && !yIsZero && zIsZero) //{0, 1, 0}
{
y = intersection.ipoint.Y;
x = solvePointEquation(pointFrom.X, pointTo.X, y, pointFrom.Y, pointTo.Y);
z = solvePointEquation(pointFrom.Z, pointTo.Z, y, pointFrom.Y, pointTo.Y);
}
else if (xIsZero && yIsZero && !zIsZero) //{0, 0, 1}
{
z = intersection.ipoint.Z;
x = solvePointEquation(pointFrom.X, pointTo.X, z, pointFrom.Z, pointTo.Z);
y = solvePointEquation(pointFrom.Y, pointTo.Y, z, pointFrom.Z, pointTo.Z);
}
else if (xIsZero && yIsZero && zIsZero)
{
throw new Exception("There is no such normal vector which has x, y, and z = 0");
}
//else create a virtual enviroment where the reflection plane has a normal to be x-axis.
//Find the reflection point on that virtual plane, then rotate it back to map the prim surface.
else
{
*/
//Initialise all the required rotation variables
Quaternion partRotation = Vector3.RotationBetween(intersection.normal, new Vector3(1, 0, 0));
Quaternion inverseRotation = Quaternion.Inverse(partRotation);
Matrix4 partRotationM = Matrix4.CreateFromQuaternion(partRotation);
Matrix4 inverseRotationM = Matrix4.CreateFromQuaternion(inverseRotation);
//Direction vectors from prim absolute position to pointFrom, pointTo, and the intersection point.
Vector3 newPointFrom = pointFrom - part.AbsolutePosition;
Vector3 newPointTo = pointTo - part.AbsolutePosition;
Vector3 newIntersection = intersection.ipoint - part.AbsolutePosition;
//Rotate those directions vectors so that they map on a plane where normal = x-axis
newPointFrom = Vector3.Transform(newPointFrom, partRotationM);
newPointTo = Vector3.Transform(newPointTo, partRotationM);
newIntersection = Vector3.Transform(newIntersection, partRotationM);
//Convert those directions vectors back into a point
newPointFrom += part.AbsolutePosition;
newPointTo += part.AbsolutePosition;
newIntersection += part.AbsolutePosition;
//Now we calculate the relfection point by solving the equation.
x = newIntersection.X;
y = solvePointEquation(newPointFrom.Y, newPointTo.Y, x, newPointFrom.X, newPointTo.X);
z = solvePointEquation(newPointFrom.Z, newPointTo.Z, x, newPointFrom.X, newPointTo.X);
//Rotate the reflection point back to map the original prim surface.
Vector3 virtualReflectionPoint = new Vector3(x, y, z) - part.AbsolutePosition;
virtualReflectionPoint = Vector3.Transform(virtualReflectionPoint, inverseRotationM);
virtualReflectionPoint += part.AbsolutePosition;
//Set result x, y and z.
x = virtualReflectionPoint.X;
y = virtualReflectionPoint.Y;
z = virtualReflectionPoint.Z;
//}
//Check if the found reflection point (x, y, and z) is actually on the prim surface.
//If yes then return that point and true. If not then return point(0, 0, 0) and false;
ReflectionPoint reflectionPoint = new ReflectionPoint(new Vector3(x, y, z), true);
if (checkPointIntersectPrim(reflectionPoint.reflectionPoint, part, 0.1f))
{
reflectionPoint.surfaceMaterial = part.Material;
return reflectionPoint;
}
else { return new ReflectionPoint(); }
}
