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;
}
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.
float idist = Constants.RegionSize;
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;
}
/// <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 iray, Quaternion parentrot)
{
// In this case we're using a sphere with a radius of the largest dimension of the prim
// TODO: Change to take shape into account
EntityIntersection result = new EntityIntersection();
Vector3 vAbsolutePosition = AbsolutePosition;
Vector3 vScale = Scale;
Vector3 rOrigin = iray.Origin;
Vector3 rDirection = iray.Direction;
//rDirection = rDirection.Normalize();
// Buidling the first part of the Quadratic equation
Vector3 r2ndDirection = rDirection*rDirection;
float itestPart1 = r2ndDirection.X + r2ndDirection.Y + r2ndDirection.Z;
// Buidling the second part of the Quadratic equation
Vector3 tmVal2 = rOrigin - vAbsolutePosition;
Vector3 r2Direction = rDirection*2.0f;
Vector3 tmVal3 = r2Direction*tmVal2;
float itestPart2 = tmVal3.X + tmVal3.Y + tmVal3.Z;
// Buidling the third part of the Quadratic equation
Vector3 tmVal4 = rOrigin*rOrigin;
Vector3 tmVal5 = vAbsolutePosition*vAbsolutePosition;
Vector3 tmVal6 = vAbsolutePosition*rOrigin;
// Set Radius to the largest dimension of the prim
float radius = 0f;
if (vScale.X > radius)
radius = vScale.X;
if (vScale.Y > radius)
radius = vScale.Y;
if (vScale.Z > radius)
radius = vScale.Z;
// the second part of this is the default prim size
// once we factor in the aabb of the prim we're adding we can
// change this to;
// radius = (radius / 2) - 0.01f;
//
radius = (radius / 2) + (0.5f / 2) - 0.1f;
//radius = radius;
float itestPart3 = tmVal4.X + tmVal4.Y + tmVal4.Z + tmVal5.X + tmVal5.Y + tmVal5.Z -
(2.0f*(tmVal6.X + tmVal6.Y + tmVal6.Z + (radius*radius)));
// Yuk Quadradrics.. Solve first
float rootsqr = (itestPart2*itestPart2) - (4.0f*itestPart1*itestPart3);
if (rootsqr < 0.0f)
{
// No intersection
return result;
}
float root = ((-itestPart2) - (float) Math.Sqrt((double) rootsqr))/(itestPart1*2.0f);
if (root < 0.0f)
{
// perform second quadratic root solution
root = ((-itestPart2) + (float) Math.Sqrt((double) rootsqr))/(itestPart1*2.0f);
// is there any intersection?
if (root < 0.0f)
{
// nope, no intersection
return result;
}
}
// We got an intersection. putting together an EntityIntersection object with the
// intersection information
Vector3 ipoint =
new Vector3(iray.Origin.X + (iray.Direction.X*root), iray.Origin.Y + (iray.Direction.Y*root),
iray.Origin.Z + (iray.Direction.Z*root));
result.HitTF = true;
result.ipoint = ipoint;
// Normal is calculated by the difference and then normalizing the result
Vector3 normalpart = ipoint - vAbsolutePosition;
result.normal = normalpart / normalpart.Length();
// It's funny how the Vector3 object has a Distance function, but the Axiom.Math object doesn't.
// I can write a function to do it.. but I like the fact that this one is Static.
Vector3 distanceConvert1 = new Vector3(iray.Origin.X, iray.Origin.Y, iray.Origin.Z);
Vector3 distanceConvert2 = new Vector3(ipoint.X, ipoint.Y, ipoint.Z);
float distance = (float) Util.GetDistanceTo(distanceConvert1, distanceConvert2);
result.distance = distance;
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;
}
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();
SceneObjectPart[] parts = m_parts.GetArray();
// Find closest hit here
float idist = float.MaxValue;
for (int i = 0; i < parts.Length; i++)
{
SceneObjectPart part = parts[i];
// 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);
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;
idist = inter.distance;
}
}
}
return result;
}
protected internal EntityIntersection GetClosestIntersectingPrim(Ray hray, bool frontFacesOnly, bool faceCenters)
{
// Primitive Ray Tracing
float closestDistance = 280f;
EntityIntersection returnResult = new EntityIntersection();
List<EntityBase> EntityList = GetEntities();
foreach (EntityBase ent in EntityList)
{
if (ent is SceneObjectGroup)
{
SceneObjectGroup reportingG = (SceneObjectGroup)ent;
EntityIntersection result = reportingG.TestIntersection(hray, frontFacesOnly, faceCenters);
if (result.HitTF)
{
if (result.distance < closestDistance)
{
closestDistance = result.distance;
returnResult = result;
}
}
}
}
return returnResult;
}
/// <summary>
/// A method which find the first object the ray hit. This is done by comparing distances.
/// The smallest distance is the distance between the intersection point and the ray start point.
/// </summary>
/// <param name="ray">The ray which will be used to test the intersection against each part in the scene</param>
/// <param name="source">An object which the ray start from. We need this to make sure that the shortest
/// distance isn't zero (from itself to itself)</param>
/// <returns>Intersection point with True if this ray intersect something, or false if nothing at all</returns>
public EntityIntersectionWithPart findNextHit(Ray ray, SceneObjectPart source)
{
float smallestDistance = 1000000f;
EntityIntersection closestIntersection = new EntityIntersection();
SceneObjectPart closestPart = new SceneObjectPart();
foreach (SceneObjectPart part in worldObjects)
{
if (!part.Equals(source)) //Cannot intersect itself (i.e. ignore/do nothing when it is itself)
{
//Test if the ray intersect this part.
EntityIntersection intersection;
if (part.GetPrimType() == OpenSim.Region.Framework.Scenes.PrimType.SPHERE)
{
intersection = part.TestIntersection(ray, part.ParentGroup.GroupRotation);
}
else
{
intersection = part.TestIntersectionOBB(ray, part.ParentGroup.GroupRotation, true, false);
}
//If it intersects, then check again with another method checkPointIntersectPrim().
if (intersection.HitTF && checkPointIntersectPrim(intersection.ipoint, part, 0.1f))
{
//If yes, then remember this distance and this intersection as the current 'return' candidate
if (intersection.distance < smallestDistance)
{
smallestDistance = intersection.distance;
closestIntersection = intersection;
closestPart = part;
}//if
}//if
}//if
}//foreach
return new EntityIntersectionWithPart(closestIntersection, closestPart);
}
public EntityIntersectionWithPart(EntityIntersection _intersection, SceneObjectPart part)
{
intersection = _intersection;
intersectPart = part;
}//constructor
