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