void Sprite::_get_rects(Rect2 &r_src_rect, Rect2 &r_dst_rect, bool &r_filter_clip) const {
Size2 s;
r_filter_clip = false;
if (region) {
s = region_rect.size;
r_src_rect = region_rect;
r_filter_clip = region_filter_clip;
} else {
s = Size2(texture->get_size());
s = s / Size2(hframes, vframes);
r_src_rect.size = s;
r_src_rect.position.x = float(frame % hframes) * s.x;
r_src_rect.position.y = float(frame / hframes) * s.y;
}
Point2 ofs = offset;
if (centered)
ofs -= s / 2;
if (Engine::get_singleton()->get_use_pixel_snap()) {
ofs = ofs.floor();
}
r_dst_rect = Rect2(ofs, s);
if (hflip)
r_dst_rect.size.x = -r_dst_rect.size.x;
if (vflip)
r_dst_rect.size.y = -r_dst_rect.size.y;
}
inline
Point2
midpoint2(const Point2 &p, const Point2 &q)
{
// return Point2((p.x_ref() + q.x_ref())/FT(2),(p.y_ref() + q.y_ref())/FT(2));
return Point2((p.x() + q.x())/FT(2), (p.y() + q.y())/FT(2));
}
void Cube::draw(Mat4x4 projection, Mat4x4 modelView, Mat4x4 world, Point2 color, Point2 camPos, Point2 camDir)
{
// Activation du shader
glUseProgram(m_shader.getProgramID());
// Verrouillage du VAO
glBindVertexArray(m_vaoID);
// Envoi des matrices
glUniformMatrix4fv(glGetUniformLocation(m_shader.getProgramID(), "u_projection"), 1, GL_FALSE, projection.getMatrix());
glUniformMatrix4fv(glGetUniformLocation(m_shader.getProgramID(), "u_modelView"), 1, GL_FALSE, modelView.getMatrix());
glUniformMatrix4fv(glGetUniformLocation(m_shader.getProgramID(), "u_world"), 1, GL_FALSE, world.getMatrix());
glUniform3f(glGetUniformLocation(m_shader.getProgramID(), "u_camPos"), camPos.Getx(), camPos.Gety(), camPos.Getz());
glUniform3f(glGetUniformLocation(m_shader.getProgramID(), "u_camPos"), camDir.Getx(), camDir.Gety(), camDir.Getz());
glUniform3f(glGetUniformLocation(m_shader.getProgramID(), "u_color"), color.Getx(), color.Gety(), color.Getz());
// Rendu
glDrawElements(GL_TRIANGLES, m_indicesTriangle.size(), GL_UNSIGNED_SHORT, 0);
// Déverrouillage du VAO
glBindVertexArray(0);
// Désactivation du shader
glUseProgram(0);
}
double VectorMeters2NorthHeading(const Point2& ref, const Point2& p, const Vector2& dir)
{
double h = atan2(dir.dx, dir.dy) * RAD_TO_DEG;
if (h < 0.0) h += 360.0;
double lon_delta = p.x() - ref.x();
return h + lon_delta * sin(p.y() * DEG_TO_RAD);
}
/******************************************************************************
* Renders the image in a rectangle given in viewport coordinates.
******************************************************************************/
void DefaultImagePrimitive::renderViewport(SceneRenderer* renderer, const Point2& pos, const Vector2& size)
{
QSize imageSize = renderer->outputSize();
Point2 windowPos((pos.x() + 1.0f) * imageSize.width() / 2, (-(pos.y() + size.y()) + 1.0) * imageSize.height() / 2);
Vector2 windowSize(size.x() * imageSize.width() / 2, size.y() * imageSize.height() / 2);
renderWindow(renderer, windowPos, windowSize);
}
double edgeDistance(Point2 p1, Point2 p2, Point2 p)
{
double x1 = p1.x;
double x2 = p2.x;
double y1 = p1.y;
double y2 = p2.y;
double x0 = p.x;
double y0 = p.y;
double den = fabs((y2 - y1)*x0 - (x2-x1)*y0 + x2*y1-y2*x1);
double num = sqrt((y2-y1)*(y2-y1) + (x2-x1)*(x2-x1));
double linedist = den / num;
double p1_dist = p1.distance(p);
double p2_dist = p2.distance(p);
double edge_len = p1.distance(p2);
if (p1_dist > edge_len)
return std::numeric_limits<double>::infinity();
if (p2_dist > edge_len)
return std::numeric_limits<double>::infinity();
return linedist;
}
pscalar PointSolver::LineSegment2D::errorJacobian (Point2 &P, pscalar *jm)
{
pscalar lsegmentsq = this->lengthSquared();
pscalar dep1x, dep1y, dep2x, dep2y;
pscalar p1x = A.coord.x(),
p1y = A.coord.y(),
p2x = B.coord.x(),
p2y = B.coord.y(),
px = P.x(), py = P.y();
dep1x = -2*(p2y-p1y)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
(lsegmentsq * lsegmentsq);
dep1y = 2*(p2x-p1x)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
(lsegmentsq * lsegmentsq);
dep2x = 2*(p2y-p1y)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
(lsegmentsq * lsegmentsq);
dep2y = -2*(p2x-p1x)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x)*
(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
(lsegmentsq * lsegmentsq);
for (int i=0; i<7; i++) {
jm[i] = dep1x*A.jacobian[i][0] + dep1y*A.jacobian[i][1] + dep2x*B.jacobian[i][0] + dep2y*B.jacobian[i][1];
}
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
Dcal->resize(2, 3);
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
Dp->resize(2,2);
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix3();
(*H1) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = Matrix3::Identity();
return Pose2(R,t);
}
Matrix H(const Point2& x_t1) const {
// Update Jacobian
Matrix H(1,2);
H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5;
H(0,1) = -1*x_t1.x() + 7;
return H;
}
void NorthHeading2VectorDegs(const Point2& ref, const Point2& p, double heading, Vector2& dir)
{
double lon_delta = p.x() - ref.x();
double real_heading = heading - lon_delta * sin(p.y() * DEG_TO_RAD);
dir.dx = sin(real_heading * DEG_TO_RAD) / cos (ref.y() * DEG_TO_RAD);
dir.dy = cos(real_heading * DEG_TO_RAD);
}
inline
Point2
midpoint1(const Point2 &p, const Point2 &q)
{
FT x,y;
midpointC2(p.x(),p.y(),q.x(),q.y(),x,y);
return Point2(x,y);
}
unsigned char getCode(Point2 p) //This function returns a bitwise OR value
{
unsigned char code = 0;
if(p.getX()<LEFT) code |= 8; //if on the left, then TFFF or 1000
if(p.getY()>TOP) code |= 4; //if on the top, then FTFF or 0100
if(p.getX()>RIGHT) code |= 2; //if on the right, then FFTF or 0010
if(p.getY()<BOTTOM) code |= 1; //if on the bottom, then FFFT or 0001
return code;
}
void TestCase::assertEqualsImpl(const Point2 &actual, const Point2 &expected, Float epsilon, const char *file, int line) {
bool match = true;
for (int i=0; i<2; ++i)
if (std::abs(actual[i]-expected[i]) > epsilon)
match = false;
if (!match)
Thread::getThread()->getLogger()->log(EError, NULL, file, line, "Assertion failure: "
"expected point %s, got %s.", expected.toString().c_str(), actual.toString().c_str());
}
void ModelLoader::ReadTextureData(FILE *apFile)
{
Point2 tmpUV;
float Unused;
fscanf(apFile, "%f %f %f",&tmpUV.X(),&tmpUV.Y(),&Unused);
mUVs.push_back(tmpUV);
}
Exact_adaptive_kernel::Oriented_side Exact_adaptive_kernel::oriented_circle (Point2 const& pa, Point2 const& pb, Point2 const& pc, Point2 const& test)
{
double r = incircle(pa.coord(), pb.coord(), pc.coord(), test.coord());
if (r > 0.0)
return ON_POSITIVE_SIDE;
else if (r < 0.0)
return ON_NEGATIVE_SIDE;
else
return ON_ORIENTED_BOUNDARY;
}
void MetersToLLE(const Point2& ref, int count, Point2 * pts)
{
while(count--)
{
pts->y_ = ref.y() + pts->y() * MTR_TO_DEG_LAT;
pts->x_ = ref.x() + pts->x() * MTR_TO_DEG_LAT / cos(pts->y() * DEG_TO_RAD);
++pts;
}
}
// Calculate the Jacobian of the nonlinear function f()
Matrix F(const Point2& x_t0) const {
// Calculate Jacobian of f()
Matrix F = Matrix(2,2);
F(0,0) = 2*x_t0.x();
F(0,1) = 0.0;
F(1,0) = x_t0.y();
F(1,1) = x_t0.x();
return F;
}
// Calculate the next state prediction using the nonlinear function f()
Point2 f(const Point2& x_t0) const {
// Calculate f(x)
double x = x_t0.x() * x_t0.x();
double y = x_t0.x() * x_t0.y();
Point2 x_t1(x,y);
// In this example, the noiseModel remains constant
return x_t1;
}
void computeProjectionJacobian (
Point3 &t, // Camera center coordinate
Quaternion &q, // Camera orientation
Point3 &point, // Original point position
Point3 &pcam, // Point in camera coordinate
Point2 &pim, // Point in image
float fx,
float fy,
float cx,
float cy, // From Intrinsic Matrix
pscalar jacobian[7][2] // jacobian result
)
{
pscalar Z = pcam.z();
pscalar x = point.x(),
y = point.y(),
z = point.z();
pscalar
Tx = 2 * (q.w()*q.y() - q.x()*q.z()),
Ty = -2 * (q.y()*q.z() + q.w()*q.x()),
Tz = 2*q.y()*q.y() + 2*q.x()*q.x() - 1,
Qx = -4*q.x()*(z-t.z()) + 2*q.w()*(y-t.y()) + 2*q.z()*(x-t.x()),
Qy = -4*q.y()*(z-t.z()) + 2*q.z()*(y-t.y()) - 2*q.w()*(x-t.x()),
Qz = 2*q.y()*(y - t.y()) + 2*q.x()*(x - t.x()),
Qw = 2*q.x()*(y - t.y()) - 2*q.y()*(x - t.x());
// dtxx
jacobian[0][0] = ((fx*(2*q.z()*q.z() + 2*q.y()*q.y()-1) + cx*Tx) / Z) - Tx*pim.x()/Z;
// dtxy
jacobian[0][1] = ((cy*Tx + fy*(-2*q.w()*q.z()-2*q.x()*q.y())) / Z) - Tx*pim.y()/Z;
// dtyx
jacobian[1][0] = ((cx*Ty+fx*(2*q.w()*q.z()-2*q.x()*q.y())) / Z) - Ty*pim.x()/Z;
// dtyy
jacobian[1][1] = ((fy*(2*q.z()*q.z() + 2*q.x()*q.x() - 1) + cy*Ty) / Z) - Ty*pim.y()/Z;
// dtzx
jacobian[2][0] = ((fx*(-2*q.x()*q.z()-2*q.w()*q.y()) + cx*Tz) / Z) - ((Tz*pim.x())/Z);
// dtzy
jacobian[2][1] = ((fx*(-2*q.x()*q.z()-2*q.w()*q.y()) + cx*Tz) / Z) - ((Tz*pim.y())/Z);
// dqxx
jacobian[3][0] = ((fx*(2*q.z()*(z-t.z()) + 2*q.y()*(y-t.y())) + cx*Qx)/Z) - (Qx*pim.x()/Z);
// dqxy
jacobian[3][1] = ((cy*Qx + fy*(-2*q.w()*(z-t.z()) -4*q.x()*(y-t.y()) + 2*q.y()*(x-t.x())) )/Z) - (Qx*pim.y()/Z);
// dqyx
jacobian[4][0] = (cx*Qy + fx*(2*q.w()*(z-t.z()) +2*q.x()*(y-t.y()) -4*q.y()*(x-t.x())))/Z - Qy*pim.x()/Z;
// dqyy
jacobian[4][1] = (fy*(2*q.z()*(z-t.z()) + 2*q.x()*(x-t.x())) + cy*Qy)/Z - Qy*pim.y()/Z;
// dqzx
jacobian[5][0] = (fx*(2*q.x()*(z-t.z()) - 2*q.w()*(y-t.y()) - 4*q.z()*(x-t.x())) + cx*Qz)/Z - Qz*pim.x()/Z;
// dqzy
jacobian[5][1] = (fy*(2*q.y()*(z-t.z()) - 4*q.z()*(y-t.y()) + 2*q.w()*(x-t.x())) + cy*Qz)/Z - Qz*pim.y()/Z;
// dqwx
jacobian[6][0] = (fx*(2*q.y()*(z-t.z()) - 2*q.z()*(y-t.y())) + cx*Qw)/Z - Qw*pim.x()/Z;
// dqwy
jacobian[6][1] = (fy*(2*q.z()*(x-t.x()) - 2*q.x()*(z-t.z())) + cy*Qw)/Z - Qw*pim.y()/Z;
}
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = (Matrix(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
return q;
}
static void scanConvert(const WFMath::Polygon<2>& inPoly, Surface& sf)
{
if (!inPoly.isValid()) return;
std::list<Edge> pending;
std::vector<Edge> active;
Point2 lastPt = inPoly.getCorner(inPoly.numCorners() - 1);
for (std::size_t p=0; p < inPoly.numCorners(); ++p) {
Point2 curPt = inPoly.getCorner(p);
// skip horizontal edges
if (curPt.y() != lastPt.y())
pending.emplace_back(lastPt, curPt);
lastPt = curPt;
}
if (pending.empty()) return;
// sort edges by starting (lowest) z value
pending.sort();
active.push_back(pending.front());
pending.pop_front();
// advance to the row of the first z value, and ensure z sits in the
// middle of sample rows - we do this by offseting by 1/2 a row height
// if you don't do this, you'll find alternating rows are over/under
// sampled, producing a charming striped effect.
WFMath::CoordType z = std::floor(active.front().start().y()) + ROW_HEIGHT * 0.5f;
for (; !pending.empty() || !active.empty(); z += ROW_HEIGHT)
{
while (!pending.empty() && (pending.front().start().y() <= z)) {
active.push_back(pending.front());
pending.pop_front();
}
// sort by x value - note active will be close to sorted anyway
std::sort(active.begin(), active.end(), EdgeAtZ(z));
// delete finished edges
for (unsigned int i=0; i< active.size(); ) {
if (active[i].end().y() <= z)
active.erase(active.begin() + i);
else
++i;
}
// draw pairs of active edges
for (unsigned int i=1; i < active.size(); i += 2)
span(sf, z, active[i - 1].xValueAtZ(z), active[i].xValueAtZ(z));
} // of active edges loop
}
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H;
return r;
}
void drawArc(Point2 centre, float radius, float startAngle, float sweep)
{
Turtle t;
const int n = 30;
float angle = startAngle * M_PI / 180;
float angleInc = sweep * M_PI / (180 * n);
float cx = centre.getX(), cy = centre.getY();
t.moveTo(Point2(cx + radius*cos(angle), cy + radius*sin(angle)));
for (int k = 1; k < n; k++, angle += angleInc)
{
t.lineTo(Point2(cx + radius*cos(angle), cy + radius*sin(angle)));
}
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
const double r = xx + yy ;
const double r2 = r*r ;
const double f = f_ ;
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
//const double g = (1+k1_*r+k2_*r2) ;
return Matrix_(2, 3,
g*x, f*x*r , f*x*r2,
g*y, f*y*r , f*y*r2);
}
float dotProduct(Point2 A, Point2 B)
{
Point2 ret;
ret.set((A.getX()*B.getX()),(A.getY()*B.getY()));
int r = ret.getX() + ret.getY();
return r;
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return (Vector(3) << t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vector(3) << v.x(), v.y(), w);
}
}
int RectangularMesh::find_element(const Point2 &point,
bool throw_exception) const
{
const double px = point.x(); // coordinates of the point of interest
const double pz = point.z();
const double x0 = _min_coord.x(); // limits of the rect mesh
const double x1 = _max_coord.x();
const double z0 = _min_coord.z();
const double z1 = _max_coord.z();
// check that the point is within the mesh
const double tol = FIND_CELL_TOLERANCE;
if (px < x0 - tol || px > x1 + tol ||
pz < z0 - tol || pz > z1 + tol)
{
if (throw_exception)
require(false, "The given point " + d2s(point) + " doesn't belong "
"to the rectangular mesh");
return -1; // to show that the point in not here
}
// since the elements of the rectangular mesh are numerated in the following
// way:
// -----------
// | 0 | 1 |
// -----------
// | 2 | 3 |
// -----------
// we can simplify the search of the element containing the given point:
const double hx = (x1 - x0) / _n_elements_x;
const double hz = (z1 - z0) / _n_elements_z;
const int nx = std::min(static_cast<int>((px-x0)/hx), _n_elements_x-1);
const int nz = std::min(static_cast<int>((pz-z0)/hz), _n_elements_z-1);
if (nx < 0 || nx >= _n_elements_x ||
nz < 0 || nz >= _n_elements_z)
{
if (throw_exception)
require(false, "The rectangular element for the point " + d2s(point) +
" wasn't found");
return -1; // to show that the point in not here
}
return (nz*_n_elements_x + nx);
}
