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