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 Matrix4f::setCol( int j, const Vector4f& v ) { int colStart = 4 * j; m_elements[ colStart ] = v.x(); m_elements[ colStart + 1 ] = v.y(); m_elements[ colStart + 2 ] = v.z(); m_elements[ colStart + 3 ] = v.w(); }
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); }
void CameraSettings::spinModel( const float x, const float y, const float z ) { if( x == 0.f && y == 0.f && z == 0.f ) return; Matrix4f matInverse; cameraRotation_.inverse( matInverse ); Vector4f shift = matInverse * Vector4f( x, y, z, 1 ); modelRotation_.pre_rotate_x( shift.x( )); modelRotation_.pre_rotate_y( shift.y( )); modelRotation_.pre_rotate_z( shift.z( )); setDirty( DIRTY_ALL ); }
VoxRaytracer::RayData VoxRaytracer::TraceBetweenPoints(const Vector4f &in, const Vector4f &out) const { RayData ray; Vector4f pt1 = m_Image->GetLocalPoint(in); Vector4f pt2 = m_Image->GetLocalPoint(out); Vector4f s = pt2 - pt1; Vector3f scale; // FIXXX scale << (m_Image->GetWorldMatrix() * Vector4f(1,0,0,0)).norm(), (m_Image->GetWorldMatrix() * Vector4f(0,1,0,0)).norm(), (m_Image->GetWorldMatrix() * Vector4f(0,0,1,0)).norm(); float l = s.head(3).norm(); Vector3f L(l/s(0), l/s(1), l/s(2)); Vector3f offset; for(int i=0;i<3;++i) offset(i) = (s(i)>=0) - (pt1(i)-floor(pt1(i))) ; offset = offset.cwiseProduct(L).cwiseAbs(); L = L.cwiseAbs(); int id; float d; Vector3i vid = m_Image->Find(in); while(l>0) { d = offset.minCoeff(&id); if(m_Image->IsInsideGrid(vid)) ray.AddElement(m_Image->Map(vid), d * scale(id) ); // nan check // // if(unlikely(!isFinite(d * scale(id)))) { // std:: cout << "NAN in raytracer\n"; // exit(1); // } vid(id) += (int)fast_sign(s(id)); l -= d; offset.array() -= d; offset(id) = fmin(L(id),l); } return ray; }
bool VoxRaytracer::GetEntryPoint(const HLine3f line, Vector4f &pt) const { Vector4f s = m_Image->GetLocalPoint(line.direction()); pt = m_Image->GetLocalPoint(line.origin()); // Considers Structured grid dimensions // Vector4f dims = m_Image->GetDims().homogeneous().cast<float>(); pt = pt.cwiseQuotient(dims); s = s.cwiseQuotient(dims); float l = s.head(3).norm(); Vector3f L(l/s(0), l/s(1), l/s(2)); Vector3f offset; for(int i=0;i<3;++i) offset(i) = (s(i)>0) - (pt(i)-floor(pt(i))) ; offset = offset.cwiseProduct(L).cwiseAbs(); int id; float d; for(int loop=0; loop<8; loop++) { int check_border = 0; for ( int i=0; i<3 ;++i) { check_border += pt(i) > 1; check_border += pt(i) < 0; } if(check_border == 0) { for(int i=0;i<3;++i) pt(i) *= (float)dims(i); pt = m_Image->GetWorldPoint(pt); return true; } d = offset.minCoeff(&id); for(int i=0; i<3; ++i) pt(i) += d / L(i); pt(id) = rintf(pt(id)); offset.array() -= d; offset(id) = fabs(L(id)); } for(int i=0;i<3;++i) pt(i) *= (float)dims(i); pt = m_Image->GetWorldPoint(pt); return false; }
cv::Scalar VectorToCVScalarColor(Vector4f vec) { vec.Clamp(0, 1); // Scale to 255. vec *= 255.f; return cv::Scalar(vec.z, vec.y, vec.x, vec.w); }
Vector3f TrackballCamera::pos(const Vector3f &p, int x, int y) { int viewport[4] = { }; glGetIntegerv(GL_VIEWPORT, viewport); if (changed) calcMatr(); Vector4f v = Vector4f(p) * matr; v.Cartesian(); v.x = ((float)x - viewport[0]) / viewport[2]*2.0f - 1.0f; v.y = ((viewport[3] - (float)y) - viewport[1]) / viewport[3]*2.0f - 1.0f; v.w = 1.0f; v *= matr_inv; v.Cartesian(); return Vector3f(v); }
// [0,1) 0 to almost 1 void Vector4f::getClamped01 ( Vector4f& result ) const { result = *this; result.clamp01(); }
void update() { if (targetCamera) view = look_at(position, target, Vector3f(0.f, 1.f, 0.f)); else { Vector4f p = invert(view) * Vector4f(0.f, 0.f, 0.f, 1.f); p /= p.w; position = p.xyz(); } Matrix4x4f m = perspective(fov, 1.f, 0.1f, 100.f) * view; viewToClip = m; clipToView = viewToClip; clipToView.invert(); }
Vector3f Viewer3D::pos(const Vector3f &p, int x, int y) { float viewport[4] = { }; glGetFloatv(GL_VIEWPORT, viewport); if (changed) calcMatr(); Vector4f v = Vector4f(p) * matr; v.Cartesian(); float winZ = (1.0f + v.z) * 0.5f; v.x = ((float)x - viewport[0]) / viewport[2]*2.0f - 1.0f; v.y = ((viewport[3] - (float)y) - viewport[1]) / viewport[3]*2.0f - 1.0f; v.z = 2.0f*winZ - 1.0f; v.w = 1.0f; v *= matr_inv; v.Cartesian(); return Vector3f(v); }
//---------------------------------------------------------------------------------------------------- Vector4f Vector4f::operator/ ( float fScalar ) const { Vector4f quot; if ( fScalar != 0.0f ) { float fInvScalar = 1.0f / fScalar; quot.x = x * fInvScalar; quot.y = y * fInvScalar; quot.z = z * fInvScalar; quot.w = w * fInvScalar; } else { quot.MakeZero(); } return( quot ); }
void test_reverse() { for(int i = 0; i < g_repeat; i++) { CALL_SUBTEST( reverse(Matrix<float, 1, 1>()) ); CALL_SUBTEST( reverse(Matrix2f()) ); CALL_SUBTEST( reverse(Matrix4f()) ); CALL_SUBTEST( reverse(Matrix4d()) ); CALL_SUBTEST( reverse(MatrixXcf(3, 3)) ); CALL_SUBTEST( reverse(MatrixXi(6, 3)) ); CALL_SUBTEST( reverse(MatrixXcd(20, 20)) ); CALL_SUBTEST( reverse(Matrix<float, 100, 100>()) ); CALL_SUBTEST( reverse(Matrix<float,Dynamic,Dynamic,RowMajor>(6,3)) ); } Vector4f x; x << 1, 2, 3, 4; Vector4f y; y << 4, 3, 2, 1; VERIFY(x.reverse()[1] == 3); VERIFY(x.reverse() == y); }
// Shading Color diffuseTerm(Color kd, Color intens, Vector4f normal, Vector4f light){ float dotProduct = light.adjoint()*normal; dotProduct = max(dotProduct,(float)0); if (dotProduct == (float)0) { return Color(0.0f, 0.0f, 0.0f); } else{ return (kd*intens)*dotProduct; } };
void Vector4f::getClamped ( const Vector4f& min, const Vector4f& max, Vector4f& result ) const { result = *this; result.clamp( min, max ); }
HitRecord Scene::intersections(shared_ptr<Object> avoid, const Vector3f& p0, const Vector3f& d) { HitRecord hrTree; bool hitTree; // Check tree hitTree = bvhTree.intersection(avoid, p0, d, &hrTree); // Check planes float best = numeric_limits<float>::infinity(); float t; shared_ptr<Object> object; for (unsigned int i = 0; i < planes.size(); i++) { Vector4f pXForm = Vector4f(p0(0), p0(1), p0(2), 1); Vector4f dXForm = Vector4f(d(0), d(1), d(2), 0); Matrix4f inv = planes[i]->getInvXForm(); Vector4f modelpos = inv * pXForm; Vector4f modelray = inv * dXForm; float t = planes[i]->intersection(modelpos.head(3), modelray.head(3)); if ( t >= 0 && t < best && planes[i] != avoid ) { best = t; object = planes[i]; } } if (!hitTree && best < 0) { return HitRecord(-1.0, NULL); } else { if (!hitTree) { return HitRecord(best, object); } if (best < 0) { return hrTree; } return hrTree.getT() < best ? hrTree : HitRecord(best, object); } }
// Shading Vector4f diffuseTerm(Vector4f kd, Vector4f intens, Vector4f normal, Vector4f light){ float dotProduct = light.adjoint()*normal; dotProduct = max(dotProduct,(float)0); if (dotProduct == (float)0) { return Vector4f(0,0,0,0); } else{ return times(kd,intens).adjoint()*dotProduct; } };