std::vector<Ray*>& Ray::reflect(const Vector4f& norm){ //norm.Print(); assert(norm[3]==0); if(m_pIntersectionProperties != NULL && m_iRecursiveTime > 1){ float reflectance = Ray::CalculateReflectance(norm,this->m_RayLine.m_Direction,NULL, this->m_pIntersectionProperties->m_Material); Vector4f dir = this->m_RayLine.m_Direction - (norm * 2.0 * norm.dot(this->m_RayLine.m_Direction)); Line ref_dir; ref_dir.m_Direction = dir; ref_dir.m_StartPoint = this->m_pIntersectionProperties->m_IntersectionPoint + (float)0.001 * norm; // Mirror reflection if(m_pIntersectionProperties->m_Material->m_eMaterialType == eMaterialType_opague){ Ray* newray = new Ray(this, ref_dir , this->m_iRecursiveTime-1); if(this->m_bShadowEnabled){ newray->enableShadow(this->m_iNumOfLighting); } else{ newray->disableShadow(); } newray->m_fReflectionIntensity = 1.0; newray->m_iID = -1; this->m_pReflectedRayList.push_back(newray); } // glossy reflection if(m_pIntersectionProperties->m_Material->m_eMaterialType == eMaterialType_glossy){ Vector4f horizontal = ref_dir.m_Direction.cross(this->m_pIntersectionProperties->m_PlanarNormal); Vector4f projection = this->m_pIntersectionProperties->m_PlanarNormal.cross(horizontal); horizontal.Normalize(); projection.Normalize(); for(unsigned int i = 0; i < m_pIntersectionProperties->m_Material->m_iGlossySamepleCount; i++){ Ray* newray = new Ray(this, ref_dir, this->m_iRecursiveTime-1); if(this->m_bShadowEnabled){ newray->enableShadow(this->m_iNumOfLighting); } else{ newray->disableShadow(); } Vector4f newdir = newray->jitter(horizontal,projection,this->m_pIntersectionProperties->m_PlanarNormal,0.6); newray->m_fReflectionIntensity = newdir.dot(ref_dir.m_Direction); newray->m_iID = -1; this->m_pReflectedRayList.push_back(newray); // TODO: add glossy reflection here. } } } return m_pReflectedRayList; }
float Ray::CalculateReflectance(const Vector4f& normal, const Vector4f& incident, Material* from, Material* to){ float n1,n2; if(from == NULL) n1 = 1; if(to == NULL) n2 = 1; if(to->m_eMaterialType == eMaterialType_opague) return 1.0; float n = n1 / n2; float cosI = -(incident.dot(normal)); float sint2 = n * n* (1.0 - cosI * cosI); if(sint2 > 1.0) return 1.0; // TIR float cosT = sqrt(1.0 - sint2); float rorth = (n1 * cosI - n2 * cosT) / (n1 * cosI + n2 * cosT); float rpar = (n2 * cosI - n1 * cosT) / (n2 * cosI + n1 * cosT); return (rorth * rorth + rpar * rpar) / 2; }
void RayTracer::InitializeViewToWorldMatrix(){ assert(m_pRenderAttribute != NULL); Vector4f up = m_pRenderAttribute->m_ViewFrustrum->m_ViewUpDirection; Vector4f dir = m_pRenderAttribute->m_ViewFrustrum->m_ViewDirection; dir.Normalize(); up = up - (up.dot(dir)) * dir; up.Normalize(); Vector4f w = dir.cross(up); this->m_ViewToWorld[0] = w[0]; this->m_ViewToWorld[4] = w[1]; this->m_ViewToWorld[8] = w[2]; this->m_ViewToWorld[1] = up[0]; this->m_ViewToWorld[5] = up[1]; this->m_ViewToWorld[9] = up[2]; this->m_ViewToWorld[2] = -dir[0]; this->m_ViewToWorld[6] = -dir[1]; this->m_ViewToWorld[10] = -dir[2]; this->m_ViewToWorld[3] = m_pRenderAttribute->m_ViewFrustrum->m_ViewPoint[0]; this->m_ViewToWorld[7] = m_pRenderAttribute->m_ViewFrustrum->m_ViewPoint[1]; this->m_ViewToWorld[11] = m_pRenderAttribute->m_ViewFrustrum->m_ViewPoint[2]; }
/* * Given a vector of points (p_vec), find the interpolated value * with the passed-in u_vec and B matrix. * * Equivalent to evaluating * f(u) = uBp * at some u. */ float interpolate_points(Vector4f u_vec, MatrixXf B, Vector4f p_vec) { float interpolated = u_vec.dot(B * p_vec); return interpolated; }
void Linearizer::update() { // Variables initialization. _b = Vector6f::Zero(); _H = Matrix6f::Zero(); const HomogeneousPoint3fOmegaVector &pointOmegas = _aligner->currentScene()->pointOmegas(); const HomogeneousPoint3fOmegaVector &normalOmegas = _aligner->currentScene()->normalOmegas(); // allocate the variables for the sum reduction; int numThreads = omp_get_max_threads(); Matrix4f _Htt[numThreads], _Htr[numThreads], _Hrr[numThreads]; Vector4f _bt[numThreads], _br[numThreads]; int _inliers[numThreads]; float _errors[numThreads]; int iterationsPerThread = _aligner->correspondenceGenerator()->numCorrespondences()/numThreads; #pragma omp parallel { int threadId = omp_get_thread_num(); int imin = iterationsPerThread*threadId; int imax = imin + iterationsPerThread; if (imax > _aligner->correspondenceGenerator()->numCorrespondences()) imax = _aligner->correspondenceGenerator()->numCorrespondences(); Eigen::Matrix4f &Htt= _Htt[threadId]; Eigen::Matrix4f &Htr= _Htr[threadId]; Eigen::Matrix4f &Hrr= _Hrr[threadId]; Eigen::Vector4f &bt = _bt[threadId]; Eigen::Vector4f &br = _br[threadId]; Htt = Matrix4f::Zero(), Htr = Matrix4f::Zero(), Hrr = Matrix4f::Zero(); bt = Vector4f::Zero(), br = Vector4f::Zero(); int &inliers = _inliers[threadId]; float &error = _errors[threadId]; error = 0; inliers = 0; for(int i = imin; i < imax; i++) { const Correspondence &correspondence = _aligner->correspondenceGenerator()->correspondences()[i]; const HomogeneousPoint3f referencePoint = _T * _aligner->referenceScene()->points()[correspondence.referenceIndex]; const HomogeneousNormal3f referenceNormal = _T * _aligner->referenceScene()->normals()[correspondence.referenceIndex]; const HomogeneousPoint3f ¤tPoint = _aligner->currentScene()->points()[correspondence.currentIndex]; const HomogeneousNormal3f ¤tNormal = _aligner->currentScene()->normals()[correspondence.currentIndex]; const HomogeneousPoint3fOmega &omegaP = pointOmegas[correspondence.currentIndex]; const HomogeneousPoint3fOmega &omegaN = normalOmegas[correspondence.currentIndex]; const Vector4f pointError = referencePoint - currentPoint; const Vector4f normalError = referenceNormal - currentNormal; const Vector4f ep = omegaP * pointError; const Vector4f en = omegaN * normalError; float localError = pointError.dot(ep) + normalError.dot(en); if(localError > _inlierMaxChi2) continue; inliers++; error += localError; Matrix4f Sp = skew(referencePoint); Matrix4f Sn = skew(referenceNormal); Htt.noalias() += omegaP; Htr.noalias() += omegaP * Sp; Hrr.noalias() +=Sp.transpose() * omegaP * Sp + Sn.transpose() * omegaN * Sn; bt.noalias() += ep; br.noalias() += Sp.transpose() * ep + Sn.transpose() * en; } } // now do the reduce Eigen::Matrix4f Htt = Eigen::Matrix4f::Zero(); Eigen::Matrix4f Htr = Eigen::Matrix4f::Zero(); Eigen::Matrix4f Hrr = Eigen::Matrix4f::Zero(); Eigen::Vector4f bt = Eigen::Vector4f::Zero(); Eigen::Vector4f br = Eigen::Vector4f::Zero(); this->_inliers = 0; this->_error = 0; for (int t = 0; t < numThreads; t++) { Htt += _Htt[t]; Htr += _Htr[t]; Hrr += _Hrr[t]; bt += _bt[t]; br += _br[t]; this->_inliers += _inliers[t]; this->_error += _errors[t]; } _H.block<3, 3>(0, 0) = Htt.block<3, 3>(0, 0); _H.block<3, 3>(0, 3) = Htr.block<3, 3>(0, 0); _H.block<3, 3>(3, 3) = Hrr.block<3, 3>(0, 0); _H.block<3, 3>(3, 0) = _H.block<3, 3>(0, 3).transpose(); _b.block<3, 1>(0, 0) = bt.block<3, 1>(0, 0); _b.block<3, 1>(3, 0) = br.block<3, 1>(0, 0); }
void Linearizer::update() { assert(_aligner && "Aligner: missing _aligner"); // Variables initialization. _b = Vector6f::Zero(); _H = Matrix6f::Zero(); const InformationMatrixVector &pointOmegas = _aligner->currentCloud()->pointInformationMatrix(); const InformationMatrixVector &normalOmegas = _aligner->currentCloud()->normalInformationMatrix(); // Allocate the variables for the sum reduction; int numThreads = omp_get_max_threads(); Matrix4f _Htt[numThreads], _Htr[numThreads], _Hrr[numThreads]; Vector4f _bt[numThreads], _br[numThreads]; int _inliers[numThreads]; float _errors[numThreads]; int iterationsPerThread = _aligner->correspondenceFinder()->numCorrespondences() / numThreads; #pragma omp parallel { int threadId = omp_get_thread_num(); int imin = iterationsPerThread * threadId; int imax = imin + iterationsPerThread; if(imax > _aligner->correspondenceFinder()->numCorrespondences()) imax = _aligner->correspondenceFinder()->numCorrespondences(); Eigen::Matrix4f &Htt = _Htt[threadId]; Eigen::Matrix4f &Htr = _Htr[threadId]; Eigen::Matrix4f &Hrr = _Hrr[threadId]; Eigen::Vector4f &bt = _bt[threadId]; Eigen::Vector4f &br = _br[threadId]; Htt = Matrix4f::Zero(); Htr = Matrix4f::Zero(); Hrr = Matrix4f::Zero(); bt = Vector4f::Zero(); br = Vector4f::Zero(); int &inliers = _inliers[threadId]; float &error = _errors[threadId]; error = 0; inliers = 0; for(int i = imin; i < imax; i++) { const Correspondence &correspondence = _aligner->correspondenceFinder()->correspondences()[i]; const Point referencePoint = _T * _aligner->referenceCloud()->points()[correspondence.referenceIndex]; const Normal referenceNormal = _T * _aligner->referenceCloud()->normals()[correspondence.referenceIndex]; const Point ¤tPoint = _aligner->currentCloud()->points()[correspondence.currentIndex]; const Normal ¤tNormal = _aligner->currentCloud()->normals()[correspondence.currentIndex]; const InformationMatrix &omegaP = pointOmegas[correspondence.currentIndex]; const InformationMatrix &omegaN = normalOmegas[correspondence.currentIndex]; const Vector4f pointError = referencePoint - currentPoint; const Vector4f normalError = referenceNormal - currentNormal; const Vector4f ep = omegaP * pointError; const Vector4f en = omegaN * normalError; float localError = pointError.dot(ep) + normalError.dot(en); float kscale = 1; if(localError > _inlierMaxChi2) { if (_robustKernel) { kscale = sqrt(_inlierMaxChi2 / localError); } else { continue; } } inliers++; error += kscale * localError; Matrix4f Sp = skew(referencePoint); Matrix4f Sn = skew(referenceNormal); Htt.noalias() += omegaP; Htr.noalias() += omegaP * Sp; Hrr.noalias() += Sp.transpose() * omegaP * Sp + Sn.transpose() * omegaN * Sn; bt.noalias() += kscale * ep; br.noalias() += kscale * (Sp.transpose() * ep + Sn.transpose() * en); } } // Now do the reduce Eigen::Matrix4f Htt = Eigen::Matrix4f::Zero(); Eigen::Matrix4f Htr = Eigen::Matrix4f::Zero(); Eigen::Matrix4f Hrr = Eigen::Matrix4f::Zero(); Eigen::Vector4f bt = Eigen::Vector4f::Zero(); Eigen::Vector4f br = Eigen::Vector4f::Zero(); this->_inliers = 0; this->_error = 0; for(int t = 0; t < numThreads; t++) { Htt += _Htt[t]; Htr += _Htr[t]; Hrr += _Hrr[t]; bt += _bt[t]; br += _br[t]; this->_inliers += _inliers[t]; this->_error += _errors[t]; } _H.block<3, 3>(0, 0) = Htt.block<3, 3>(0, 0); _H.block<3, 3>(0, 3) = Htr.block<3, 3>(0, 0); _H.block<3, 3>(3, 3) = Hrr.block<3, 3>(0, 0); _H.block<3, 3>(3, 0) = _H.block<3, 3>(0, 3).transpose(); _b.block<3, 1>(0, 0) = bt.block<3, 1>(0, 0); _b.block<3, 1>(3, 0) = br.block<3, 1>(0, 0); }
void reachGoal(Vector4f goal){ float distance = 100; float totalLength = 0; int counter = 0; float difference = 1000; for (int i = 0; i <number_of_arms;i++){ totalLength+=arm[i].length; } float reachable_length = cal_distance(goal, arm[0].startpoint); // cout << "totalLength is: " << totalLength << endl; // cout << "reachable_length is: " << reachable_length << endl; // cout << endl; if( reachable_length > totalLength){ cout << "out of reach !! " << endl; Vector4f straight_direction = goal - arm[0].startpoint; // straight_direction = xyz_normalize(straight_direction); for (int i = 0; i <number_of_arms;i++){ arm[i].change_link_direction(straight_direction); } Matrix4f non_rotation; non_rotation<<1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1; rotate_links(non_rotation, 0); } else{ float debug_angle = 0; float debug_dotP = 0; while(distance>0.01){ cout << distance << endl; for (int i = number_of_arms-1; i >-1; i--) { // cout << distance << endl; if (distance>0.01){ Vector4f RD = xyz_normalize(goal - arm[i].startpoint); Vector4f end = arm[number_of_arms-1].startpoint + arm[number_of_arms-1].get_link_direction() * arm[number_of_arms-1].length; Vector4f RE = xyz_normalize(end - arm[i].startpoint); float angle = acos(RD.dot(RE)); debug_angle = angle; debug_dotP = RD.dot(RE); cout << endl; cout<<angle<<endl; cout << RD.dot(RE) <<endl; if (RD.dot(RE) > 1){ cout << "problem is right here!!"<< endl; return; } cout << endl; if(angle>-0.0001 and angle<0.0001){ counter++; if (counter>number_of_arms-1){ // break; cout << "loop exit because of angle" << endl; return; } } else{ counter = 0; } Matrix4f applyM = rotationM(RD, RE, angle); rotate_links(applyM, i); // cout<<applyM<<endl; // myDraw(); // cout<<arm[0].direction<<endl; distance = cal_distance(end, goal); // cout<<distance<<endl; // if (difference-distance>0){ // difference = distance; // }else{ // break; // } } else { cout << "distance too small, exit with distance: " << distance << endl; } // myDraw(); } // if (counter>number_of_arms-1){ // break; // } // if (difference-distance>0){ // difference = distance; // }else{ // break; // } } cout << "function exiting with distance: " << distance << endl; cout << "function exiting with angle: " << debug_angle << endl; cout << "function exiting with dot product: " << debug_dotP << endl; } };