Intersection Sphere::GetIntersection(const Ray& r)
{
Ray r_obj(r.getTransformedCopy(this->transform.invT()));
float radius = 0.5f;
glm::vec3 center(0.f);
float A = glm::length2(r_obj.direction);
float B = 2 * (glm::dot(r_obj.direction, r_obj.origin - center));
float C = glm::length2(r_obj.origin - center) - glm::pow(radius, 2.f);
float disc = glm::pow(B, 2.f) - 4 * A * C;
if (disc<0) return Intersection(); // B^2-4AC<0 then no intersection
float t = ( -B + glm::sqrt(disc) ) / (2 * A);
float temp = ( -B - glm::sqrt(disc) ) / (2 * A);
if (t >= 0)
{
if (temp < t && temp >= 0) t = temp;
}
else
t = temp;
if (t < 0 ) return Intersection(); // not in the front
glm::vec3 ipoint(r_obj.origin + t * r_obj.direction);
glm::vec3 inormal(glm::normalize(ipoint - center));
glm::vec3 itangent = glm::cross(glm::vec3(0,1,0), inormal);
if (fequal(glm::length2(itangent), 0.f))
{
itangent = glm::vec3(0,0,1);
}
glm::vec3 ipoint_world(this->transform.T() * glm::vec4(ipoint, 1.f));
glm::vec4 normal4_world(this->transform.invTransT() * glm::vec4(inormal,0.f));
glm::vec3 normal_world( normal4_world );
normal_world = glm::normalize(normal_world);
glm::vec3 tangent_world = glm::normalize(glm::vec3(this->transform.invTransT() * glm::vec4(itangent, 0.f)));
//bitangent computed in Intersection constructor
float t_world = glm::dot(ipoint_world - r.origin, r.direction);
glm::vec2 uv = this->GetUVCoordinates(ipoint);
glm::vec3 s_color = Material::GetImageColorInterp(uv, this->material->texture);
return Intersection(ipoint_world, normal_world, tangent_world, t_world, s_color, this);
}
Intersection SquarePlane::GetIntersection(const Ray& r)
{
Ray r_obj(r.getTransformedCopy(this->transform.invT()));
glm::vec3 center(0.f, 0.f, 0.f);
glm::vec3 normal(0.f, 0.f, 1.f);
float halfside = 1 * 0.5f;
if (fequal(r_obj.direction.z, 0.f))
return Intersection();
// since it's a fixed XY plane, this is a simplified ray-plane intersection
float t = (center.z - r_obj.origin.z) / r_obj.direction.z;
if (t < 0) return Intersection();
glm::vec3 ipoint(r_obj.origin + t * r_obj.direction);
if (ipoint.x > center.x + halfside || ipoint.x < center.x - halfside)
return Intersection();
if (ipoint.y > center.y + halfside || ipoint.y < center.y - halfside)
return Intersection();
glm::vec3 ipoint_world(this->transform.T() * glm::vec4(ipoint, 1.f));
glm::vec4 normal4_world(this->transform.invTransT() * glm::vec4(normal,0.f));
glm::vec3 normal_world(normal4_world);
normal_world = glm::normalize(normal_world);
float t_world = glm::dot(ipoint_world - r.origin, r.direction);
glm::vec3 s_color = Material::GetImageColorInterp(this->GetUVCoordinates(ipoint), this->material->texture);
glm::vec4 tangent_o(1,0,0,0);
glm::vec3 tangent_world = glm::normalize(glm::vec3(this->transform.invTransT() * tangent_o));
//bitangent computed in Intersection constructor
return Intersection(ipoint_world, normal_world, tangent_world, t_world, s_color, this);
}
UInt32 RigidBodyShape::ComputeFitTo(RigidTransform3& T, const RigidBodyShape& base) const
{
size_t num_points = NumMarkers();
if (num_points == base.NumMarkers())
{
// get centroids
Vector3 centroidA(0,0,0);
Vector3 centroidB(0,0,0);
const RigidBodyMarker* iter_A = &base.marker[0];
const RigidBodyMarker* iter_B = &marker[0];
size_t ipoint(0);
size_t num_in_common(0);
for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++,iter_B++)
{
if (iter_A->visible && iter_B->visible)
{
Vector3::Add(centroidA.x, centroidA.x, iter_A->position);
Vector3::Add(centroidB.x, centroidB.x, iter_B->position);
++num_in_common;
}
}
if (num_in_common >= 3)
{
// normalise
centroidA /= num_in_common;
centroidB /= num_in_common;
// correlation matrix
double sum_correl[9] =
{ 0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
// find correlation matrix
iter_A = &base.marker[0];
iter_B = &marker[0];
for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++, iter_B++)
{
if (iter_A->visible && iter_B->visible)
{
// input minus centroid
Vector3 normA;
Vector3::Sub(normA, iter_A->position, centroidA);
// cal minus centroid
Vector3 normB;
Vector3::Sub(normB, iter_B->position, centroidB);
// correlation (outer product)
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
sum_correl[3*i+j] += normA[i]*normB[j];
}
}
// SVD of correlation to get rotation
// from mean coords to this frame
double s[3];
double U[9], VT[9];
Matrix3x3::SVD(U, s, VT, sum_correl);
// force right-handed coord system
double detU = Matrix3x3::Det(U);
double detV = Matrix3x3::Det(VT);
if (detU*detV < 0.0)
{
VT[6] *= -1.0;
VT[7] *= -1.0;
VT[8] *= -1.0;
}
// this is the correct way round
Matrix3x3::Mul(T.R, U, VT);
// do post-subtraction of centroid B to get translation vector
double RcentroidB[3];
Matrix3x3::MulVec(RcentroidB, T.R, centroidB);
Vector3::Sub(T.t, centroidA, RcentroidB);
#if 0
iter_A = ptA;
iter_B = ptB;
for (ipoint = 0; ipoint < npoints; ipoint++, iter_A+=3, iter_B+=3)
{
Vector3 Tb;
RigidTransform3::MulVec(Tb, R, t, iter_B);
cerr << "x: " << iter_A[0] << " " << Tb[0] << endl;
cerr << "y: " << iter_A[1] << " " << Tb[1] << endl;
cerr << "z: " << iter_A[2] << " " << Tb[2] << endl;
Vector3::Sub(Tb, Tb, iter_A);
cerr << "Mod: " << Tb.Modulus() << endl;
}
#endif
return RigidBodyResult::success;
}
}
return RigidBodyResult::insufficient_points;
}
