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