C++ Vector4f类代码示例

void ITMColorTracker_CPU::F_oneLevel(float *f, ITMPose *pose)
{
	int noTotalPoints = trackingState->pointCloud->noTotalPoints;

	Vector4f projParams;
	if (type == ITMLibSettings::TRACKER_ODOMETRY_COLOR) {
	  projParams.setValues(view->calib->intrinsics_d.projectionParamsSimple.all);
	} else {
	  projParams.setValues(view->calib->intrinsics_rgb.projectionParamsSimple.all);
	}
	projParams.x /= 1 << levelId; projParams.y /= 1 << levelId;
	projParams.z /= 1 << levelId; projParams.w /= 1 << levelId;

	Matrix4f M = pose->GetM();

	Vector2i imgSize = viewHierarchy->levels[levelId]->rgb->noDims;

	float scaleForOcclusions, final_f;

	Vector4f *locations = trackingState->pointCloud->locations->GetData(MEMORYDEVICE_CPU);
	Vector4f *colours = trackingState->pointCloud->colours->GetData(MEMORYDEVICE_CPU);
	Vector4u *rgb = viewHierarchy->levels[levelId]->rgb->GetData(MEMORYDEVICE_CPU);

	final_f = 0; countedPoints_valid = 0;
	for (int locId = 0; locId < noTotalPoints; locId++)
	{
		float colorDiffSq = getColorDifferenceSq(locations, colours, rgb, imgSize, locId, projParams, M);
		if (colorDiffSq >= 0) { final_f += colorDiffSq; countedPoints_valid++; }
	}

	if (countedPoints_valid == 0) { final_f = MY_INF; scaleForOcclusions = 1.0; }
	else { scaleForOcclusions = (float)noTotalPoints / countedPoints_valid; }

	f[0] = final_f * scaleForOcclusions;
}

void RayTracer::CalculateShadow(Ray* _ray){
    // Assume ray has an intersection point;
    //std::cout<<"www"<<std::endl;
    for(unsigned int i =0 ; i < m_LightList.size(); i++){
        std::vector<Vector4f> visiblesample = m_LightList[i]->getVisibleSamplePoint();
        float lightintensity = 0.0;
        //std::cout<<visiblesample.size()<<std::endl;
        for(unsigned int j = 0; j < visiblesample.size();j++){
            Vector4f ptolight = visiblesample[j] -  _ray->m_pIntersectionProperties->
                    m_IntersectionPoint;
            //ptolight.Print();
            ptolight.Normalize();
            Line _testray;
            _testray.m_Direction = ptolight;
            _testray.m_StartPoint = _ray->m_pIntersectionProperties->m_IntersectionPoint + (float)0.0001 * 
                    _ray->m_pIntersectionProperties->m_PlanarNormal;
            bool intersect = checkIntersection(_testray);
            if(!intersect){
                lightintensity+=1.0;
            }
        }
        lightintensity /= (float)(visiblesample.size());
        //std::cout<<lightintensity<<" "<<visiblesample.size()<<std::endl;
        _ray->setBlockedLight(i,lightintensity);
    }
}

// Set up double-sided lights
void lights()
{
  using namespace std;
  using namespace Eigen;
  glEnable(GL_LIGHTING);
  //glLightModelf(GL_LIGHT_MODEL_TWO_SIDE,GL_TRUE);
  glEnable(GL_LIGHT0);
  float WHITE[4] = {1,1,1,1.};
  float GREY[4] = {0.4,0.4,0.4,1.};
  float BLACK[4] = {0.,0.,0.,1.};
  float NEAR_BLACK[4] =  {0.1,0.1,0.1,1.};
  Vector4f pos = light_pos;
  glLightfv(GL_LIGHT0,GL_AMBIENT,BLACK);
  glLightfv(GL_LIGHT0,GL_DIFFUSE,WHITE);
  glLightfv(GL_LIGHT0,GL_SPECULAR,BLACK);
  glLightfv(GL_LIGHT0,GL_POSITION,pos.data());
  //glEnable(GL_LIGHT1);
  //pos(0) *= -1;
  //pos(1) *= -1;
  //pos(2) *= -1;
  //glLightfv(GL_LIGHT1,GL_AMBIENT,BLACK);
  //glLightfv(GL_LIGHT1,GL_DIFFUSE,NEAR_BLACK);
  //glLightfv(GL_LIGHT1,GL_SPECULAR,BLACK);
  //glLightfv(GL_LIGHT1,GL_POSITION,pos.data());
}

void SkeletalModel::traverseLimbTree(Joint* j){

	Vector3f u,v,w;
	Vector4f c;
	Matrix4f rotate,scale,translate;
	
	translate =  Matrix4f::translation(0.0f,0.0f,0.5f);

	m_matrixStack.push(j->transform);

	for (int i = 0; i < j->children.size(); i++){
		
		c = j->children[i]->transform.getCol(3);

		w = c.xyz().normalized();
		v = Vector3f::cross(w,Vector3f(0,0,1)).normalized();
		u = Vector3f::cross(v,w).normalized();

		scale = Matrix4f::scaling(.025,.025,c.xyz().abs());
		rotate = Matrix4f(Vector4f(u,0),Vector4f(v,0),Vector4f(w,0),Vector4f(0,0,0,1));
	
		glLoadMatrixf(m_matrixStack.top()*rotate*scale*translate);
		glutSolidCube(1.0f);

		traverseLimbTree(j->children[i]);
	}
	
	m_matrixStack.pop();
}

void Matrix4f::setRow( int i, const Vector4f& v )
{
	m_elements[ i ] = v.x();
	m_elements[ i + 4 ] = v.y();
	m_elements[ i + 8 ] = v.z();
	m_elements[ i + 12 ] = v.w();
}

void ITMColorTracker_CPU::G_oneLevel(float *gradient, float *hessian, ITMPose *pose) const
{
	int noTotalPoints = trackingState->pointCloud->noTotalPoints;

	Vector4f projParams;
  if (type == ITMLibSettings::TRACKER_ODOMETRY_COLOR) {
    projParams.setValues(view->calib->intrinsics_d.projectionParamsSimple.all);
  } else {
    projParams.setValues(view->calib->intrinsics_rgb.projectionParamsSimple.all);
  }
	projParams.x /= 1 << levelId; projParams.y /= 1 << levelId;
	projParams.z /= 1 << levelId; projParams.w /= 1 << levelId;

	Matrix4f M = pose->GetM();

	Vector2i imgSize = viewHierarchy->levels[levelId]->rgb->noDims;

	float scaleForOcclusions;

	bool rotationOnly = iterationType == TRACKER_ITERATION_ROTATION;
	int numPara = rotationOnly ? 3 : 6, startPara = rotationOnly ? 3 : 0, numParaSQ = rotationOnly ? 3 + 2 + 1 : 6 + 5 + 4 + 3 + 2 + 1;

	float globalGradient[6], globalHessian[21];
	for (int i = 0; i < numPara; i++) globalGradient[i] = 0.0f;
	for (int i = 0; i < numParaSQ; i++) globalHessian[i] = 0.0f;

	Vector4f *locations = trackingState->pointCloud->locations->GetData(MEMORYDEVICE_CPU);
	Vector4f *colours = trackingState->pointCloud->colours->GetData(MEMORYDEVICE_CPU);
	Vector4u *rgb = viewHierarchy->levels[levelId]->rgb->GetData(MEMORYDEVICE_CPU);
	Vector4s *gx = viewHierarchy->levels[levelId]->gradientX_rgb->GetData(MEMORYDEVICE_CPU);
	Vector4s *gy = viewHierarchy->levels[levelId]->gradientY_rgb->GetData(MEMORYDEVICE_CPU);

	for (int locId = 0; locId < noTotalPoints; locId++)
	{
		float localGradient[6], localHessian[21];

		bool isValidPoint = computePerPointGH_rt_Color(localGradient, localHessian, locations, colours, rgb, imgSize, locId,
			projParams, M, gx, gy, numPara, startPara);

		if (isValidPoint)
		{
			for (int i = 0; i < numPara; i++) globalGradient[i] += localGradient[i];
			for (int i = 0; i < numParaSQ; i++) globalHessian[i] += localHessian[i];
		}
	}

	scaleForOcclusions = (float)noTotalPoints / countedPoints_valid;
	if (countedPoints_valid == 0) { scaleForOcclusions = 1.0f; }

	for (int para = 0, counter = 0; para < numPara; para++)
	{
		gradient[para] = globalGradient[para] * scaleForOcclusions;
		for (int col = 0; col <= para; col++, counter++) hessian[para + col * numPara] = globalHessian[counter] * scaleForOcclusions;
	}
	for (int row = 0; row < numPara; row++)
	{
		for (int col = row + 1; col < numPara; col++) hessian[row + col * numPara] = hessian[col + row * numPara];
	}
}

int main()
{
    BEGIN_TESTING(vtk VoxRaytracer);


    // muon scatter //
    MuonScatter muon;
    muon.LineIn().origin() << -6, 12, -6, 1;
    muon.LineIn().direction() << 1 , -1 , 1 , 0;
    muon.LineOut().origin() << 6 , -4 , 6 , 1;
    muon.LineOut().direction() << 1 , -1 , 1 , 0;

    Vtk::vtkMuonScatter v_muon(muon);


    // structured grid //
    ImageData grid(Vector3i(12,10,12));
    grid.SetSpacing(Vector3f(1,1,1));
    grid.SetPosition(Vector3f(0,0,0));


    Vtk::vtkStructuredGrid v_grid(grid);


    // voxraytracer //
    VoxRaytracer rt(grid);
    Vector4f pt;
    rt.GetEntryPoint(muon.LineIn(),pt);
    std::cout << pt.transpose() << "\n";

    Vtk::vtkVoxRaytracerRepresentation v_rt(rt);
    v_rt.SetMuon(muon);
    v_rt.SetRayColor(Vector4f(1,0,0,1));

    //    // renderer //
    Vtk::Viewer viewer;

    // widget //
    vtkBoxWidget *widget = v_grid.GetWidget();


    vtkWidgetCallback *cbk = vtkWidgetCallback::New();
    cbk->SetTracer(&v_rt);
    cbk->SetMuon(&muon);
    cbk->SetAnnotation(viewer.GetAnnotation());
    widget->AddObserver(vtkCommand::InteractionEvent, cbk);
    widget->SetInteractor(viewer.GetInteractor());
    widget->PlaceWidget();
    widget->On();

    viewer.AddPuppet(v_grid);
    viewer.AddPuppet(v_rt);
    viewer.AddPuppet(v_muon);
    viewer.Start();


    END_TESTING;
}

// static
QRgb QImageUtils::vector4fToQRgba( const Vector4f& v )
{
	int r = ColorUtils::floatToInt( v.x() );
	int g = ColorUtils::floatToInt( v.y() );
	int b = ColorUtils::floatToInt( v.z() );
	int a = ColorUtils::floatToInt( v.w() );

	return qRgba( r, g, b, a );
}

    void Vector4Test::testMutableFunctionIteration() {
        GenerateUnique generator;
		
        Vector4f result;
        std::generate( result.Begin(), result.End(), generator );
		
        const float expected[] = { 1.0f, 2.0f, 3.0f, 4.0f };
		
        CPPUNIT_ASSERT( std::equal( result.Begin(), result.End(), expected ) );
    }

Vector4f specularTerm(Vector4f ks, Vector4f intens, Vector4f normal, Vector4f light, Vector4f viewer, float p){
    
    Vector4f refl = light*(float)(-1) + normal*((float)2*(light.adjoint()*normal));
    float dotProduct = refl.adjoint()*viewer;
    dotProduct = max(dotProduct,(float)0);
    if (dotProduct==(float)0){
        return Vector4f(0,0,0,0);
    }else{
        return times(ks,intens)* pow(dotProduct, p);
    }
};

Vector4f get_color(Vector4f viewer, Vector4f normal, Vector4f intersect){
    Vector4f R = objects[hit_index].ka;
    MatrixXf None(4,2); None<<0,0,0,0,0,0,0,0;
    Ray testshadow;
    MatrixXf is_shadow;
    
    if (pt_light_counter != 0) {
        for (int l = 0; l < pt_light_counter; l++) {
            Vector4f pt_light_xyz = pt_xyz[l];
            Vector4f pt_light_rgb = pt_rgb[l];
            Vector4f light = pt_light_xyz - intersect;
            light.normalize();
            
            testshadow = Ray(intersect, light);
            is_shadow = Find_nearest(testshadow, objects);
            
            if(is_shadow==None){
                Vector4f diffuse = diffuseTerm(kd, pt_light_rgb, normal, light);
                
                Vector4f specular = specularTerm(ks, pt_light_rgb, normal, light, viewer, p);
                
                Vector4f ambient = times(ka,pt_light_rgb);
                
                R = R + (diffuse + specular + ambient);
            }
        }
    }
    
    if (dl_light_counter != 0) {
        for (int l = 0; l < dl_light_counter; l++) {
            Vector4f dl_light_rgb = dl_rgb[l];
            Vector4f light = dl_xyz[l];
            light.normalize();
            
            testshadow = Ray(intersect, light);
            is_shadow = Find_nearest(testshadow, objects);
            
            if(is_shadow==None){
                
                Vector4f diffuse = diffuseTerm(kd, dl_light_rgb, normal, light);
                
                Vector4f specular = specularTerm(ks, dl_light_rgb, normal, light, viewer, p);
                
                Vector4f ambient = times(ka,dl_light_rgb);
                
                R = R + (diffuse + specular + ambient);
            }
            
        }
    }
    return R;
};

Vector3f Camera::unProject(const Vector2f& uv, float depth, const Matrix4f& invModelview) const
{
    updateViewMatrix();
    updateProjectionMatrix();
    
    Vector3f a(2.*uv.x()/float(mVpWidth)-1., 2.*uv.y()/float(mVpHeight)-1., 1.);
    a.x() *= depth/mProjectionMatrix(0,0);
    a.y() *= depth/mProjectionMatrix(1,1);
    a.z() = -depth;
    // FIXME /\/|
    Vector4f b = invModelview * Vector4f(a.x(), a.y(), a.z(), 1.);
    return Vector3f(b.x(), b.y(), b.z());
}

bool BVHTree::intersectHelp(shared_ptr<Object> avoid, const Vector3f& pos, const Vector3f& ray, BVHNode *node, HitRecord *hitRecord)
{
    if (node == NULL) { return false; }

    if ((node->bb)->intersection(pos, ray)) {
        // Check if leaf
        if (node->left == NULL && node->right == NULL) {

            shared_ptr<Object> object = node->bb->getObject(); 
            
            if (object != avoid) {
                
                Matrix4f inv = object->getInvXForm();
                
                Vector4f modelPos = inv * Vector4f(pos(0), pos(1), pos(2), 1);
                Vector4f modelRay = inv * Vector4f(ray(0), ray(1), ray(2), 0);
                
                float t = object->intersection(modelPos.head(3), modelRay.head(3));

                if (t >= 0) {
                    *hitRecord = HitRecord(t, object);
                    return HIT;
                } 
            }
              
            return !HIT;

        } else {
            // Either a node is a leaf, or it has two children
             HitRecord lHR, rHR;
             bool lHit, rHit;
             
             lHit = intersectHelp(avoid, pos, ray, node->left, &lHR);
             rHit = intersectHelp(avoid, pos, ray, node->right, &rHR);
             
             if (!lHit && !rHit) { 
                 return !HIT; 
             } else if (!lHit) { 
                 *hitRecord = rHR; 
             } else if (!rHit) { 
                 *hitRecord = lHR; 
             } else {
                 *hitRecord = lHR.getT() < rHR.getT() ? lHR : rHR;
             }
             
             return HIT;    
        }
    }

    return !HIT;
}

void Model::bbox()
{
    matrix=makeMatrix();
    if(mesh!=NULL)
    {
        Vector4f p;
        Matrix4f m=matrix;
        m.transpose();
        min_x=1000000;
        max_x=-1000000;
        min_y=1000000;
        max_y=-1000000;
        min_z=1000000;
        max_z=-1000000;

        for(GLuint i=0;i < mesh->indexCount();i++)
        {
            p.set((mesh->vertexData())[(mesh->indexData())[i]*3]
                    ,mesh->vertexData()[mesh->indexData()[i]*3+1]
                    ,mesh->vertexData()[mesh->indexData()[i]*3+2],1.0);
            p=m*p;
            if(p[0]<min_x)  min_x=p[0];
            if(p[0]>max_x)  max_x=p[0];

            if(p[1]<min_y)  min_y=p[1];
            if(p[1]>max_y)  max_y=p[1];

            if(p[2]<min_z)  min_z=p[2];
            if(p[2]>max_z)  max_z=p[2];
        }
/*
        if(min_x<0)
        {
            m_position[0]-=min_x;
            min_x=0;
        }
        if(min_y<0)
        {
            m_position[1]-=min_y;
            min_y=0;
        }
        if(min_z<0)
        {
            m_position[2]-=min_z;
            min_z=0;
        }
*/
        box.setExtreme(min_x,min_y,min_z,max_x,max_y,max_z);
    }
}

 void Float4Widget::setValue(Vector4f v) {
     comboSlider1->blockSignals(true);
     comboSlider2->blockSignals(true);
     comboSlider3->blockSignals(true);
     comboSlider4->blockSignals(true);
     comboSlider1->setValue(v.x());
     comboSlider2->setValue(v.y());
     comboSlider3->setValue(v.z());
     comboSlider3->setValue(v.w());
     comboSlider1->blockSignals(false);
     comboSlider2->blockSignals(false);
     comboSlider3->blockSignals(false);
     comboSlider4->blockSignals(false);
 }