double TetMesh::getTetDeterminant(Vec3d * a, Vec3d * b, Vec3d * c, Vec3d * d) { // computes the determinant of the 4x4 matrix // [ a 1 ] // [ b 1 ] // [ c 1 ] // [ d 1 ] Mat3d m0 = Mat3d(*b, *c, *d); Mat3d m1 = Mat3d(*a, *c, *d); Mat3d m2 = Mat3d(*a, *b, *d); Mat3d m3 = Mat3d(*a, *b, *c); return (-det(m0) + det(m1) - det(m2) + det(m3)); }
void TetMesh::getElementInertiaTensor(int el, Mat3d & inertiaTensor) const { Vec3d a = *getVertex(el, 0); Vec3d b = *getVertex(el, 1); Vec3d c = *getVertex(el, 2); Vec3d d = *getVertex(el, 3); Vec3d center = getElementCenter(el); a -= center; b -= center; c -= center; d -= center; double absdetJ = 6.0 * getElementVolume(el); double x1 = a[0], x2 = b[0], x3 = c[0], x4 = d[0]; double y1 = a[1], y2 = b[1], y3 = c[1], y4 = d[1]; double z1 = a[2], z2 = b[2], z3 = c[2], z4 = d[2]; double A = absdetJ * (y1*y1 + y1*y2 + y2*y2 + y1*y3 + y2*y3 + y3*y3 + y1*y4 + y2*y4 + y3*y4 + y4*y4 + z1*z1 + z1 * z2 + z2 * z2 + z1 * z3 + z2 * z3 + z3 * z3 + z1 * z4 + z2 * z4 + z3 * z4 + z4 * z4) / 60.0; double B = absdetJ * (x1*x1 + x1*x2 + x2*x2 + x1*x3 + x2*x3 + x3*x3 + x1*x4 + x2*x4 + x3*x4 + x4*x4 + z1*z1 + z1 * z2 + z2 * z2 + z1 * z3 + z2 * z3 + z3 * z3 + z1 * z4 + z2 * z4 + z3 * z4 + z4 * z4) / 60.0; double C = absdetJ * (x1*x1 + x1*x2 + x2*x2 + x1*x3 + x2*x3 + x3*x3 + x1*x4 + x2*x4 + x3*x4 + x4*x4 + y1*y1 + y1 * y2 + y2 * y2 + y1 * y3 + y2 * y3 + y3 * y3 + y1 * y4 + y2 * y4 + y3 * y4 + y4 * y4) / 60.0; double Ap = absdetJ * (2*y1*z1 + y2*z1 + y3*z1 + y4*z1 + y1*z2 + 2*y2*z2 + y3*z2 + y4*z2 + y1*z3 + y2*z3 + 2*y3*z3 + y4*z3 + y1*z4 + y2*z4 + y3*z4 + 2*y4*z4) / 120.0; double Bp = absdetJ * (2*x1*z1 + x2*z1 + x3*z1 + x4*z1 + x1*z2 + 2*x2*z2 + x3*z2 + x4*z2 + x1*z3 + x2*z3 + 2*x3*z3 + x4*z3 + x1*z4 + x2*z4 + x3*z4 + 2*x4*z4) / 120.0; double Cp = absdetJ * (2*x1*y1 + x2*y1 + x3*y1 + x4*y1 + x1*y2 + 2*x2*y2 + x3*y2 + x4*y2 + x1*y3 + x2*y3 + 2*x3*y3 + x4*y3 + x1*y4 + x2*y4 + x3*y4 + 2*x4*y4) / 120.0; inertiaTensor = Mat3d(A, -Bp, -Cp, -Bp, B, -Ap, -Cp, -Ap, C); }
static int position_orientationto(lua_State* l) { CelxLua celx(l); celx.checkArgs(3, 3, "Two arguments expected for position:orientationto"); UniversalCoord* src = this_position(l); UniversalCoord* target = to_position(l, 2); if (target == NULL) { celx.doError("First argument to position:orientationto must be a position"); } Vec3d* upd = celx.toVector(3); if (upd == NULL) { celx.doError("Second argument to position:orientationto must be a vector"); } Vec3d src2target = *target - *src; src2target.normalize(); Vec3d v = src2target ^ *upd; v.normalize(); Vec3d u = v ^ src2target; Quatd qd = Quatd(Mat3d(v, u, -src2target)); celx.newRotation(qd); return 1; }
bool CornellBoxScene::Intersect( Ray& ray, Intersection& isect ) { bool intersected = false; for (auto& primitive : primitives) { if (primitive->Intersect(ray, isect)) { intersected = true; isect.primitive = primitive; } } if (intersected) { // Fill in the information in isect // Compute conversion to/from shading coordinates isect.worldToShading = Math::Transpose(Mat3d(isect.ss, isect.st, isect.sn)); isect.shadingToWorld = Math::Inverse(isect.worldToShading); } return intersected; }
/* convert to camera matrix */ Mat3d Intrinsic::toKMatrix() { return Mat3d(focal_x,0.0,CC[0],0.0,focal_y,CC[1],0.0,0.0,1.0); }
void proc_image(Mat &in, Mat &out, const ProcData & proc, const Idx & pos) { int flags_remain; Mat curr_in; Mat curr_out = in; int flags = proc.flags(); double min = proc.min(); double max = proc.max(); Datastore *store = proc.store(); double depth = proc.depth(); double focus_point = proc.focus_point(); flags &= ~NO_MEM_CACHE; flags &= ~NO_DISK_CACHE; //add placeholder for image scaling if (proc.scale() != 1.0) flags |= Improc::_SCALE; bool sub = false; bool scale = false; double scale_val; if (!std::isnan(min)) sub = true; else min = 0.0; if (!std::isnan(max)) { scale = true; scale_val = max-min; } else { scale_val = BaseType_max(in.type())-min; if (in.type() == BaseType::UINT32) { //UINT32 is converted to float by cvMat :-( scale_val = BaseType_max(BaseType::FLOAT)-min; } } int cv_interpolation = cv::INTER_LINEAR; if (flags & HQ) { cv_interpolation = cv::INTER_LANCZOS4; flags &= ~HQ; } //FIXME hdr may need 4! assert(in.size() == 3); if (flags == 0) { printf("FIXME implement copy!\n"); out.create(in.type(), in); cvMat(in).copyTo(cvMat(out)); return; } if (_handle_preproc(Improc::DEMOSAIC, curr_in, curr_out, out, flags)) { curr_out.create(curr_in.type(), {curr_in.r(0,1),3}); cv::Mat cv_out; cv::cvtColor(cvMat(curr_in.bind(2,0)), cv_out, order2cv_conf_flag(store->order())); //FIXME copy! //curr_out = Mat3d(cv_out); cvMat(Mat3d(cv_out)).copyTo(cvMat(curr_out)); } if (_handle_preproc(Improc::CVT_GRAY, curr_in, curr_out, out, flags)) { cv::Mat accumulate; //FIXME whats about images in double format? cvMat(curr_in.bind(2,0)).convertTo(accumulate, CV_32F); for(int c=1;c<curr_in[2];c++) cv::add(accumulate, cvMat(curr_in.bind(2,c)), accumulate, cv::noArray(), accumulate.type()); accumulate *= 1.0/curr_in[2]; Idx outsize = curr_in; outsize[2] = 1; curr_out.create(curr_in.type(), outsize); accumulate.copyTo(cvMat(curr_out.bind(2,0))); } if (_handle_preproc(Improc::CVT_8U, curr_in, curr_out, out, flags)) { curr_out.create(BaseType::UINT8, curr_in); if (sub) { cv::Mat subbed = cvMat(curr_in) - min; subbed.convertTo(cvMat(curr_out), CV_8U, BaseType_max<uint8_t>()/scale_val); } else cvMat(curr_in).convertTo(cvMat(curr_out), CV_8U, BaseType_max<uint8_t>()/scale_val); sub = false; scale = false; } if (sub || scale) { curr_in = curr_out; if (!flags) //write to final output curr_out = out; else //use new matrix curr_out.release(); curr_out.create(curr_in.type(), curr_in); cv::Mat tmp = (cvMat(curr_in)-min) * BaseType_max(curr_in.type())/scale_val; tmp.copyTo(cvMat(curr_out)); } if (_handle_preproc(Improc::UNDISTORT, curr_in, curr_out, out, flags)) { //FIXME path logic? which undist to use? DepthDist *undist; //#pragma omp critical { undist = store->undist(depth); if (undist) undist->undistort(curr_in, curr_out, pos, cv_interpolation); else { printf("distortion model not supported\n"); abort(); } if (!std::isnan(focus_point) && focus_point != 0) { std::cout << "focus_point: " << focus_point << std::endl; //do rectification double step_length = proc.step_length(); double fx = proc.f(0); double fy = proc.f(1); int w = proc.w(); int h = proc.h(); double cx = w / 2.0; double cy = h / 2.0; double k[3][3] = {{fx, 0, cx}, {0, fy, cy}, {0, 0, 1}}; cv::Mat K = cv::Mat(3, 3, CV_64F, k); double init_trans; double translation; init_trans = (proc.img_count() - 1) * step_length / 2.0; translation = init_trans - pos[3] * step_length; double alpha = -atan2(translation, focus_point); cv::Mat t = cv::Mat::zeros(3, 1, CV_64F); // translation vector t.at<double>(2, 0) = focus_point; //TODO for now we only support horizontal lines! t.at<double>(0, 0) = translation; // vertical: t.at<double>(1, 0) = translation; t = t / t.at<double>(2, 0); // normalize vector // construct translation matrix cv::Mat T = cv::Mat::eye(3, 3, CV_64F); t.copyTo(T.col(2)); // Define rotation matrix //TODO for now we only support horizontal lines! double r[3][3] = {{cos(alpha), 0, sin(alpha)}, {0, 1, 0 }, {-sin(alpha), 0, cos(alpha)}}; /* vertical: double r[3][3] = {{1, 0, 0 }, {0, cos(alpha), sin(alpha)}, {0, -sin(alpha), cos(alpha)}};*/ cv::Mat R = cv::Mat(3, 3, CV_64F, r); cv::Mat H = cv::Mat(3, 3, CV_64F); H = K * R * T * K.inv(); // compute homography matrix H = H / H.at<double>(2, 2); // normalize matrix cv::Size img_size = cv::Size_<int>(w, h); // iterate color channels for(int c=0;c<curr_out[2];c++){ cv::Mat out_mat = cvMat(curr_out.bind(2, c)); cv::Mat in_mat = out_mat.clone(); cv::warpPerspective(in_mat, out_mat, H, img_size, cv::INTER_LINEAR | cv::WARP_INVERSE_MAP); } } } } if (_handle_preproc(Improc::_SCALE, curr_in, curr_out, out, flags)) { float sigma = 0.5/proc.scale(); int ks = int(sigma)*6+1; cv::Mat tmp; //FIXME sync size with opencv resize sizing (also in other places!) curr_out.create(curr_in.type(), {proc.scale(curr_in[0]), proc.scale(curr_in[1]), curr_in[2]}); for(int c=0;c<curr_in[2];c++) { cv::GaussianBlur(cvMat(curr_in.bind(2,c)), tmp, cv::Size(ks,ks), sigma, sigma); cv::resize (tmp, cvMat(curr_out.bind(2,c)), cv::Size(0,0), proc.scale(), proc.scale(), cv::INTER_NEAREST); } } out = curr_out; if (flags) { printf("WARNING: proc_image() did not handle all processing flags!\n "); } }
void Primitive::InitializeTransform() { worldToLocal = Math::Inverse(localToWorld); normalLocalToWorld = Mat3d(Math::Transpose(worldToLocal)); }