void do_bump_map( //const eiScalar i_bumpValue, //const eiScalar i_bumpDepth, const normal& i_normalCamera, normal& o_outNormal ) { eiScalar LOW = -1.0f, HEIGHT=1.0f; eiScalar offset = clamp(bumpValue(), LOW, HEIGHT) * abs(bumpDepth()); //Du_offset=d(offset)/du, Dv_offset=d(offset)/dv eiScalar Du_offset, Dv_offset; eiScalar DbumpValue_du, DbumpValue_dv; eiScalar DbumpDepth_du, DbumpDepth_dv; Du(bumpValue, DbumpValue_du); Dv(bumpValue, DbumpValue_dv); Du(bumpDepth, DbumpDepth_du); Dv(bumpDepth, DbumpDepth_dv); Du_offset = D_clamp(bumpValue(), LOW,HEIGHT) * DbumpValue_du * abs(bumpDepth()) + clamp(bumpValue(), LOW,HEIGHT) * D_abs(bumpDepth()) * DbumpDepth_du; Dv_offset = D_clamp(bumpValue(), LOW,HEIGHT) * DbumpValue_dv * abs(bumpDepth()) + clamp(bumpValue(), LOW,HEIGHT) * D_abs(bumpDepth()) * DbumpDepth_dv; /* These scale factors are a bit experimental. The constant is to roughly match maya's bump mapping. The part about dPdu/dPdv ensures that the bump's scale does not depend on the underlying parametrization of the patch (the use of Du and Dv below introduce that) and that it happens in object space. Note that maya's handling of object space appears to be slightly broken since an enlarged sphere will have the same bump as a sphere with its control points moved outwards by a scale, somehow. */ matrix toLocal = to_object(); eiScalar uscale = 1.0f / len(&(dPdu() * toLocal)) * 6.0f; eiScalar vscale = 1.0f / len(&(dPdv() * toLocal)) * 6.0f; vector gu = vector(1.0f, 0.0f, Du_offset * uscale); vector gv = vector(0.0f, 1.0f, Dv_offset * vscale); normal n = normal(gu ^ gv); vector basisz = normalize(i_normalCamera); vector basisx = normalize((basisz ^ dPdu()) ^ basisz); vector basisy = normalize((basisz ^ dPdv()) ^ basisz); o_outNormal = normal( n.x * basisx + n.y * basisy + n.z * basisz); o_outNormal = normalize(o_outNormal); }
void RigidResponseMethodBLCP<Scalar, Dim>::collisionResponse() { //initialize unsigned int m = this->rigid_driver_->numContactPoint();//m: number of contact points unsigned int n = this->rigid_driver_->numRigidBody();//n: number of rigid bodies if(m == 0 || n == 0)//no collision or no rigid body return; unsigned int dof = n * (Dim + RotationDof<Dim>::degree);//DoF(Degree of Freedom) of a rigid-body system unsigned int fric_sample_count = 2;//count of friction sample directions unsigned int s = m * fric_sample_count;//s: number of friction sample. Here a square sample is adopted CompressedJacobianMatrix<Scalar, Dim> J(m, n);//Jacobian matrix. (m, dof) dimension when uncompressed, (m, 12) dimension after compression CompressedJacobianMatrix<Scalar, Dim> D(s, n);//Jacobian matrix of friction. (s, dof) dimension when uncompressed, (s, 12) dimension after compression CompressedInertiaMatrix<Scalar, Dim> M(n);//inertia matrix. (dof, dof) dimension when uncompressed, (dof, 6) dimension after compression CompressedInertiaMatrix<Scalar, Dim> M_inv(n);//inversed inertia matrix. (dof, dof) dimension when uncompressed, (dof, 6) dimension after compression CompressedJacobianMatrix<Scalar, Dim> MJ(n, m);//M * J. (dof, m) dimension when uncompressed, (12, m) dimension after compression CompressedJacobianMatrix<Scalar, Dim> MD(n, s);//M * D. (dof, s) dimension when uncompressed, (12, s) dimension after compression VectorND<Scalar> v(dof, 0);//generalized velocity of the system VectorND<Scalar> Jv(m, 0);//normal relative velocity of each contact point (for normal contact impulse calculation) VectorND<Scalar> post_Jv(m, 0);//expected post-impact normal relative velocity of each contact point (for normal contact impulse calculation) VectorND<Scalar> Dv(s, 0);//tangent relative velocity of each contact point (for frictional contact impulse calculation) VectorND<Scalar> CoR(m, 0);//coefficient of restitution (for normal contact impulse calculation) VectorND<Scalar> CoF(s, 0);//coefficient of friction (for frictional contact impulse calculation) VectorND<Scalar> z_norm(m, 0);//normal contact impulse. The key of collision response VectorND<Scalar> z_fric(s, 0);//frictional contact impulse. The key of collision response RigidBodyDriverUtility<Scalar, Dim>::computeMassMatrix(this->rigid_driver_, M, M_inv); RigidBodyDriverUtility<Scalar, Dim>::computeJacobianMatrix(this->rigid_driver_, J); RigidBodyDriverUtility<Scalar, Dim>::computeFricJacobianMatrix(this->rigid_driver_, D); RigidBodyDriverUtility<Scalar, Dim>::computeGeneralizedVelocity(this->rigid_driver_, v); //compute other matrix in need CompressedJacobianMatrix<Scalar, Dim> J_T = J; J_T = J.transpose(); CompressedJacobianMatrix<Scalar, Dim> D_T = D; D_T = D.transpose(); MJ = M_inv * J_T; MD = M_inv * D_T; Jv = J * v; Dv = D * v; //update CoR and CoF RigidBodyDriverUtility<Scalar, Dim>::computeCoefficient(this->rigid_driver_, CoR, CoF); //calculate the expected post-impact velocity for(unsigned int i = 0; i < m; ++i) post_Jv[i] = -Jv[i] * CoR[i]; //solve BLCP with PGS. z_norm and z_fric are the unknown variables RigidBodyDriverUtility<Scalar, Dim>::solveBLCPWithPGS(this->rigid_driver_, J, D, MJ, MD, Jv, post_Jv, Dv, z_norm, z_fric, CoR, CoF, 20); //apply impulse RigidBodyDriverUtility<Scalar, Dim>::applyImpulse(this->rigid_driver_, z_norm, z_fric, J_T, D_T); }
void OpticalFlow::baseCalculate(cv::Mat& Im1, cv::Mat& Im2, flowUV& UV, const OpticalFlowParams& params){ int rows = Im1.rows; int cols = Im1.cols; FlowOperator flowOp(rows, cols); FArray X0(2 * rows * cols, false); FArray dUdV(2 * rows * cols, true, 0); cv::Mat Ix1(rows, cols, OPTFLOW_TYPE); cv::Mat Iy1(rows, cols, OPTFLOW_TYPE); cv::Mat Ix(rows, cols, OPTFLOW_TYPE); cv::Mat Iy(rows, cols, OPTFLOW_TYPE); getDXsCV(Im1, Ix1, Iy1); for (int i = 0; i < params.getIters(); ++i){ cv::Mat Ix2(rows, cols, OPTFLOW_TYPE); cv::Mat Iy2(rows, cols, OPTFLOW_TYPE); cv::Mat It(rows, cols, OPTFLOW_TYPE); cv::Mat im2Warpped(rows, cols, Im1.type()); WarpImage(Im2, UV.getU(), UV.getV(), im2Warpped); getDXsCV(im2Warpped, Ix2, Iy2); Ix = params.getWeightedDeriveFactor() * (Ix1 + Ix2); Iy = params.getWeightedDeriveFactor() * (Iy1 + Iy2); cv::subtract(im2Warpped, Im1, It); if (params.getDisplayDerivativs()){ cv::imshow("Derivative Ix", Ix); cv::imshow("Derivative Iy", Iy); cv::waitKey(1); } cv::Mat Du(rows, cols, OPTFLOW_TYPE, cv::Scalar(0)); cv::Mat Dv(rows, cols, OPTFLOW_TYPE, cv::Scalar(0)); for (int j = 0; j < params.getLinearIters(); ++j){ #if OPTFLOW_VERBOSE cout << "solving Ax=b with SOR "; clock_t start = std::clock(); #endif flowOp.construct(UV, Du, Dv, Ix, Iy, It, params); memcpy(X0.ptr, UV.getU().data, rows * cols * sizeof(float)); memcpy(X0.ptr + (rows * cols), UV.getV().data, rows * cols * sizeof(float)); //UtilsDebug::printCRSSparseMat(flowOp.getA(), "aaaa.txt"); if (params.getCheckResidualTolerance()){ LinearSolver::sparseMatSor(flowOp.getA(), X0 ,dUdV, flowOp.getb(), params.getOverRelaxation(), params.getSorIters(), params.getResidualTolerance()); }else{ //LinearSolver::multigrid(10,10,flowOp.getA(),flowOp.getb(), params.getResidualTolerance(), dUdV, 20, 20, LinearSolver::vCycle); LinearSolver::sparseMatSorNoResidual(flowOp.getA(), X0 ,dUdV, flowOp.getb(), params.getOverRelaxation(), params.getSorIters()); } #if OPTFLOW_VERBOSE std::cout<<" --- "<< (std::clock() - start) / (double)CLOCKS_PER_SEC <<'\n'; #endif #if OPTFLOW_DEBUG for(int i = 0; i < dUdV.size(); ++i){ if (!(dUdV.ptr[i] == dUdV.ptr[i])){ cout << "ERROR - NAN"; } } #endif UtilsMat::clamp(dUdV, -1, 1); memcpy(Du.data, dUdV.ptr, rows * cols * sizeof(float)); memcpy(Dv.data, dUdV.ptr + (rows * cols), rows * cols * sizeof(float)); flowUV UV0(UV); UV.getU() += Du; UV.getV() += Dv; cv::Mat tmpU, tmpV; UV.getU().copyTo(tmpU); UV.getV().copyTo(tmpV); WeightedMedianFilter::computeMedianFilter(UV.getU(), UV.getV(), Im1, Im2, params.getMedianFilterRadius()); Du = UV.getU() - UV0.getU(); Dv = UV.getV() - UV0.getV(); UV0.getU().copyTo(UV.getU()); UV0.getV().copyTo(UV.getV()); UV0.getU() += Du; UV0.getV() += Dv; if (params.isDisplay()) UtilsFlow::DrawFlow(UV0.getU(), UV0.getV(), "Flow"); } UV.getU() += Du; UV.getV() += Dv; } }
NurbsSurfaceSP<T,N> NurbsSurfaceSP<T,N>::generateParallel(T d) const { NurbsSurfaceSP<T,N> p(*this) ; Vector< Point_nD<T,N> > offset(this->P.rows()*this->P.cols()) ; Vector<T> ur(this->P.rows()*this->P.cols()) ; Vector<T> vr(this->P.rows()*this->P.cols()) ; Vector_INT Du(this->P.rows()*this->P.cols()) ; Vector_INT Dv(this->P.rows()*this->P.cols()) ; Du.reset(0) ; Dv.reset(0) ; int i,j,no ; no = 0 ; for(i=0;i<this->P.rows();++i) for(j=0;j<this->P.cols();++j){ Point_nD<T,N> norm ; norm = normal(maxAtU_[i],maxAtV_[j]) ; if(norm.x() == T(0) && norm.y() == T(0) && norm.z() == T(0)){ // normal is undefined there... // compute an average and find a suitable normal const T delta = 0.00001 ; // must handle the corner cases int ok = 0 ; if(i==0 && j==0){ norm = normal(maxAtU_[i]+delta,maxAtV_[j]) ; norm += normal(maxAtU_[i],maxAtV_[j]+delta) ; norm /= T(2) ; ok = 1 ; } if(i==this->P.rows()-1 && j==this->P.cols()-1){ norm = normal(maxAtU_[i]-delta,maxAtV_[j]) ; norm += normal(maxAtU_[i],maxAtV_[j]-delta) ; norm /= T(2) ; ok = 1 ; } if(i==0 && j==this->P.cols()-1){ norm = normal(maxAtU_[i]-delta,maxAtV_[j]) ; norm += normal(maxAtU_[i],maxAtV_[j]+delta) ; norm /= T(2) ; ok = 1 ; } if(i==this->P.rows()-1 && j==0){ norm = normal(maxAtU_[i]-delta,maxAtV_[j]) ; norm += normal(maxAtU_[i],maxAtV_[j]+delta) ; norm /= T(2) ; ok = 1 ; } if(!ok){ T nt = 1.0 ; while(norm.x() == T(0) && norm.y() == T(0) && norm.z() == T(0)){ if( nt*d >(this->U[this->U.n()-1]-this->U[0])){ #ifdef USE_EXCEPTION throw NurbsComputationError(); #else Error error("generateParallel"); error << "Can't compute a normal point.\n" ; error.fatal() ; #endif } T u1,u2,v1,v2 ; if(i==0 || i==this->P.rows()-1){ u1 = u2 = maxAtU_[i] ; v1 = maxAtV_[j]+ nt*delta ; v2 = maxAtV_[j]- nt*delta ; if(v1>this->V[this->V.n()-1]) v1 = this->V[this->V.n()-1] ; if(v2<this->V[0]) v2 = this->V[0] ; norm = normal(u1,v1); norm += normal(u2,v2) ; norm /= 2 ; } else{ u1 = maxAtU_[i]- nt*delta ; u2 = maxAtU_[i]+ nt*delta ; v1 = v2 = maxAtV_[j] ; if(u1 < this->U[0]) u1 = this->U[0] ; if(u2 > this->U[this->U.n()-1]) u2 = this->U[this->U.n()-1] ; T u3,v3 ; u3 = maxAtU_[i] ; if(j==0) v3 = maxAtV_[j]+ nt*delta ; else v3 = maxAtV_[j]- nt*delta ; if(v3<this->V[0]) v3 = this->V[0] ; if(v3>this->V[this->V.n()-1]) v3 = this->V[this->V.n()-1] ; norm = normal(u1,v1); norm += normal(u2,v2) ; norm += normal(u3,v3) ; norm /= 3 ; } nt *= 10.0 ; } } } Point_nD<T,N> unit = norm.unitLength(); unit *= d ; //HPoint_nD<T,N> offset(unit ) ; //offset.w() = 0.0 ; //p.modSurfCPby(i,j,offset) ; Du[no] = i ; Dv[no] = j ; offset[no] = unit ; ++no ; } p.movePoint(maxAtU_,maxAtV_,offset,Du,Dv) ; return p ; }