C++ Vector4f::dot方法代码示例

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 &currentPoint = _aligner->currentScene()->points()[correspondence.currentIndex];
      const HomogeneousNormal3f &currentNormal = _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 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){
//.........这里部分代码省略.........