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; }