VECTOR4D computeNewPosition(VECTOR4D& V0, VECTOR4D& V1, VECTOR4D& V2, VECTOR4D& Point)
{
VECTOR4D Plane;
VECTOR4D NewPos = { 0,0,0,0 };
Plane = Cross3((V1 - V0), (V2 - V0));
Plane = Normalize(Plane);
Plane.w = -Dot(V0, Plane);
/* Compute normal */
VECTOR4D Raydir;
Raydir = Normalize(Cross3(V1 - V0, V2 - V0));
float n, d;
PlaneIntersect(Point, Raydir, Plane, n, d);
if (fabs(d) < 0.0001)
{
MessageBox(NULL, L"No interseccion 1 ", L"Algo esta mal con el check", MB_ICONERROR);
return NewPos;
}
float u = n / d;
if (u < 0.0f)
{
MessageBox(NULL, L"No interseccion 2 ", L"Algo esta mal con el check", MB_ICONERROR);
return NewPos;
}
VECTOR4D Offset = { Raydir.x*u, Raydir.y*u, Raydir.z*u, 0 };
return Point + Offset;
}
MATRIX4D Orthogonalize(MATRIX4D &M)
{
MATRIX4D R = M;
float x = M.m03;
float y = M.m13;
float z = M.m23;
M.m03 = 0;
M.m13 = 0;
M.m23 = 0;
R.vec[0] = Normalize(M.vec[0]);
R.vec[2] = Cross3(R.vec[0], Normalize(M.vec[1]));
R.vec[1] = Cross3(R.vec[2], R.vec[0]);
R.m03 = x;
R.m13 = y;
R.m23 = z;
return R;
}
MATRIX4D Orthogonalize(MATRIX4D &M) {
MATRIX4D R = M;
float a = M.m03;
float b = M.m13;
float c = M.m23;
M.m03 = 0.0f;
M.m13 = 0.0f;
M.m23 = 0.0f;
R.vec[0] = Normalize(M.vec[0]);
R.vec[2] = Cross3(R.vec[0], Normalize(M.vec[1]));
R.vec[1] = Cross3(R.vec[2], R.vec[0]);
R.m03 = a;
R.m13 = b;
R.m23 = c;
return R;
}
MATRIX4D View(VECTOR4D& EyePos,VECTOR4D& Target, VECTOR4D& Up)
{
MATRIX4D View=Identity();
VECTOR4D N = Normalize(Target-EyePos);
VECTOR4D U = Normalize(Cross3(Up,N));
VECTOR4D V = Cross3(N,U);
View.m00 = U.x;
View.m10 = U.y;
View.m20 = U.z;
View.m30 = -Dot(U, EyePos);
View.m01 = V.x;
View.m11 = V.y;
View.m21 = V.z;
View.m31 = -Dot(V, EyePos);
View.m02 = N.x;
View.m12 = N.y;
View.m22 = N.z;
View.m32 = -Dot(N, EyePos);
return View;
}
void MatrixLookAtRH(float *mOut, const vec3_t vEye, const vec3_t vAt, const vec3_t vUp)
{
vec3_t f, vUpActual, s, u;
float t[16];
f[0] = vAt[0] - vEye[0];
f[1] = vAt[1] - vEye[1];
f[2] = vAt[2] - vEye[2];
Normalize3(f, f);
Normalize3(vUpActual, vUp);
Cross3(s, f, vUpActual);
Cross3(u, s, f);
mOut[ 0] = s[0];
mOut[ 1] = u[0];
mOut[ 2] = -f[0];
mOut[ 3] = 0;
mOut[ 4] = s[1];
mOut[ 5] = u[1];
mOut[ 6] = -f[1];
mOut[ 7] = 0;
mOut[ 8] = s[2];
mOut[ 9] = u[2];
mOut[10] = -f[2];
mOut[11] = 0;
mOut[12] = 0;
mOut[13] = 0;
mOut[14] = 0;
mOut[15] = 1;
MatrixTranslation(t, -vEye[0], -vEye[1], -vEye[2]);
MatrixMultiply(mOut, t, mOut);
}
bool RayCastOnTriangle(VECTOR4D &V0, VECTOR4D &V1, VECTOR4D &V2, VECTOR4D &RayOrigin, VECTOR4D &RayDir, VECTOR4D &Intersection) {
VECTOR4D Plane, Offset;
float n, d, u, w0, w1, w2;
Plane = Cross3((V1 - V0), (V2 - V0));
Plane = Normalize(Plane);
Plane.w = -Dot(V0, Plane);
PlaneIntersect(RayOrigin, RayDir, Plane, n, d);
if (fabs(d) < EPSILON)
return false;
u = n / d;
if (u < 0.0f)
return false;
Offset = { RayDir.x * u, RayDir.y * u, RayDir.z * u, 0.0f };
Intersection = RayOrigin + Offset;
return PtInTriangleBarycentric(V0, V1, V2, Intersection, w0, w1, w2);
}
bool PointIsBelowPlane(VECTOR4D& V0, VECTOR4D& V1, VECTOR4D& V2, VECTOR4D& Point)
{
VECTOR4D Plane;
Plane = Cross3((V1 - V0), (V2 - V0));
Plane = Normalize(Plane);
Plane.w = -Dot(V0, Plane);
double planeEq = Dot(Plane,Point);
double TOLERANCE = 1.0e-9;
/*if (fabs(planeEq) <= TOLERANCE) {
return false; //on
}*/
if (planeEq > TOLERANCE) {
return false; //above
}
else {
return true; //below
}
}
bool RayCastOnTriangle(
VECTOR4D& V0, VECTOR4D& V1, VECTOR4D& V2,
VECTOR4D& RayOrigin, VECTOR4D RayDir,
VECTOR4D& Intersection, float* w0, float* w1, float* w2)
{
VECTOR4D Plane;
Plane = Cross3((V1 - V0), (V2 - V0));
Plane = Normalize(Plane);
Plane.w = -Dot(V0, Plane);
//Plane = [A,B,C,D] los coeficientes del plano.
float n, d;
PlaneIntersect(RayOrigin, RayDir, Plane, n, d);
if (fabs(d) < 0.0001)
return false;
float u = n / d;
if (u < 0.0f)
return false;
VECTOR4D Offset = { RayDir.x*u, RayDir.y*u, RayDir.z*u, 0 };
Intersection = RayOrigin + Offset;
//float w0, w1, w2;
return PtInTriangleBarycentric(
V0, V1, V2, Intersection, *w0, *w1, *w2);
}
