Ray Frustum::UnProject(float x, float y) const
{
assume1(x >= -1.f, x);
assume1(x <= 1.f, x);
assume1(y >= -1.f, y);
assume1(y <= 1.f, y);
if (type == PerspectiveFrustum)
{
vec nearPlanePos = NearPlanePos(x, y);
return Ray(pos, (nearPlanePos - pos).Normalized());
}
else
return UnProjectFromNearPlane(x, y);
}
void Quat::SetFromAxisAngle(const float3 &axis, float angle)
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
SetFromAxisAngle(load_vec3(axis.ptr(), 0.f), angle);
#else
assume1(axis.IsNormalized(), axis);
assume1(MATH_NS::IsFinite(angle), angle);
float sinz, cosz;
SinCos(angle*0.5f, sinz, cosz);
x = axis.x * sinz;
y = axis.y * sinz;
z = axis.z * sinz;
w = cosz;
#endif
}
void Plane::Set(const vec &v1, const vec &v2, const vec &v3)
{
normal = (v2-v1).Cross(v3-v1);
float len = normal.Length();
assume1(len > 1e-10f, len);
normal /= len;
assume2(normal.IsNormalized(), normal, normal.LengthSq());
d = normal.Dot(v1);
}
void Quat::SetFromAxisAngle(const float4 &axis, float angle)
{
assume1(EqualAbs(axis.w, 0.f), axis);
assume2(axis.IsNormalized(1e-4f), axis, axis.Length4());
assume1(MATH_NS::IsFinite(angle), angle);
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE2)
// Best: 26.499 nsecs / 71.024 ticks, Avg: 26.856 nsecs, Worst: 27.651 nsecs
simd4f halfAngle = set1_ps(0.5f*angle);
simd4f sinAngle, cosAngle;
sincos_ps(halfAngle, &sinAngle, &cosAngle);
simd4f quat = mul_ps(axis, sinAngle);
// Set the w component to cosAngle.
simd4f highPart = _mm_unpackhi_ps(quat, cosAngle); // [_ _ 1 z]
q = _mm_movelh_ps(quat, highPart); // [1 z y x]
#else
// Best: 36.868 nsecs / 98.312 ticks, Avg: 36.980 nsecs, Worst: 41.477 nsecs
SetFromAxisAngle(axis.xyz(), angle);
#endif
}
void assume()
{
int allright,row,colum,count;
save_current_state();
for(row=0;row<9;row++)
for(colum=0;colum<9;colum++)
for(count=1;count<10;count++)
if(node[row][colum].check[count]==EMPTY)
{
st_node(row,colum,count);
if(0==assume1())
break;
}
}
int Sphere::IntersectLine(const vec &linePos, const vec &lineDir, const vec &sphereCenter,
float sphereRadius, float &t1, float &t2)
{
assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq());
assume1(sphereRadius >= 0.f, sphereRadius);
/* A line is represented explicitly by the set { linePos + t * lineDir }, where t is an arbitrary float.
A sphere is represented implictly by the set of vectors that satisfy ||v - sphereCenter|| == sphereRadius.
To solve which points on the line are also points on the sphere, substitute v <- linePos + t * lineDir
to obtain:
|| linePos + t * lineDir - sphereCenter || == sphereRadius, and squaring both sides we get
|| linePos + t * lineDir - sphereCenter ||^2 == sphereRadius^2, or rearranging:
|| (linePos - sphereCenter) + t * lineDir ||^2 == sphereRadius^2. */
// This equation represents the set of points which lie both on the line and the sphere. There is only one
// unknown variable, t, for which we solve to get the actual points of intersection.
// Compute variables from the above equation:
const vec a = linePos - sphereCenter;
const float radSq = sphereRadius * sphereRadius;
/* so now the equation looks like
|| a + t * lineDir ||^2 == radSq.
Since ||x||^2 == <x,x> (i.e. the square of a vector norm equals the dot product with itself), we get
<a + t * lineDir, a + t * lineDir> == radSq,
and using the identity <a+b, a+b> == <a,a> + 2*<a,b> + <b,b> (which holds for dot product when a and b are reals),
we have
<a,a> + 2 * <a, t * lineDir> + <t * lineDir, t * lineDir> == radSq, or
<a,a> - radSq + 2 * <a, lineDir> * t + <lineDir, lineDir> * t^2 == 0, or
C + Bt + At^2 == 0, where
C = <a,a> - radSq,
B = 2 * <a, lineDir>, and
A = <lineDir, lineDir> == 1, since we assumed lineDir is normalized. */
// Warning! If Dot(a,a) is large (distance between line pos and sphere center) and sphere radius very small,
// catastrophic cancellation can occur here!
const float C = Dot(a,a) - radSq;
const float B = 2.f * Dot(a, lineDir);
/* The equation A + Bt + Ct^2 == 0 is a second degree equation on t, which is easily solvable using the
known formula, and we obtain
t = [-B +/- Sqrt(B^2 - 4AC)] / 2A. */
float D = B*B - 4.f * C; // D = B^2 - 4AC.
if (D < 0.f) // There is no solution to the square root, so the ray doesn't intersect the sphere.
{
// Output a degenerate enter-exit range so that batch processing code may use min of t1's and max of t2's to
// compute the nearest enter and farthest exit without requiring branching on the return value of this function.
t1 = FLOAT_INF;
t2 = -FLOAT_INF;
return 0;
}
if (D < 1e-4f) // The expression inside Sqrt is ~ 0. The line is tangent to the sphere, and we have one solution.
{
t1 = t2 = -B * 0.5f;
return 1;
}
// The Sqrt expression is strictly positive, so we get two different solutions for t.
D = Sqrt(D);
t1 = (-B - D) * 0.5f;
t2 = (-B + D) * 0.5f;
return 2;
}
/** The implementation of this function is based on the paper
"Kong, Everett, Toussant. The Graham Scan Triangulates Simple Polygons."
See also p. 772-775 of Geometric Tools for Computer Graphics.
The running time of this function is O(n^2). */
TriangleArray Polygon::Triangulate() const
{
assume1(IsPlanar(), this->SerializeToString());
TriangleArray t;
// Handle degenerate cases.
if (NumVertices() < 3)
return t;
if (NumVertices() == 3)
{
t.push_back(Triangle(Vertex(0), Vertex(1), Vertex(2)));
return t;
}
std::vector<float2> p2d;
std::vector<int> polyIndices;
for(int v = 0; v < NumVertices(); ++v)
{
p2d.push_back(MapTo2D(v));
polyIndices.push_back(v);
}
// Clip ears of the polygon until it has been reduced to a triangle.
int i = 0;
int j = 1;
int k = 2;
size_t numTries = 0; // Avoid creating an infinite loop.
while(p2d.size() > 3 && numTries < p2d.size())
{
if (float2::OrientedCCW(p2d[i], p2d[j], p2d[k]) && IsAnEar(p2d, i, k))
{
// The vertex j is an ear. Clip it off.
t.push_back(Triangle(p[polyIndices[i]], p[polyIndices[j]], p[polyIndices[k]]));
p2d.erase(p2d.begin() + j);
polyIndices.erase(polyIndices.begin() + j);
// The previous index might now have become an ear. Move back one index to see if so.
if (i > 0)
{
i = (i + (int)p2d.size() - 1) % p2d.size();
j = (j + (int)p2d.size() - 1) % p2d.size();
k = (k + (int)p2d.size() - 1) % p2d.size();
}
numTries = 0;
}
else
{
// The vertex at j is not an ear. Move to test next vertex.
i = j;
j = k;
k = (k+1) % p2d.size();
++numTries;
}
}
assume3(p2d.size() == 3, (int)p2d.size(), (int)polyIndices.size(), (int)NumVertices());
if (p2d.size() > 3) // If this occurs, then the polygon is NOT counter-clockwise oriented.
return t;
/*
{
// For conveniency, create a copy that has the winding order fixed, and triangulate that instead.
// (Causes a large performance hit!)
Polygon p2 = *this;
for(size_t i = 0; i < p2.p.size()/2; ++i)
std::swap(p2.p[i], p2.p[p2.p.size()-1-i]);
return p2.Triangulate();
}
*/
// Add the last poly.
t.push_back(Triangle(p[polyIndices[0]], p[polyIndices[1]], p[polyIndices[2]]));
return t;
}
