417 lines
11 KiB
C++
417 lines
11 KiB
C++
/*****************************************************************************
|
|
* VCGLib o o *
|
|
* Visual and Computer Graphics Library o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2004-2022 \/)\/ *
|
|
* Visual Computing Lab /\/| *
|
|
* ISTI - Italian National Research Council | *
|
|
* \ *
|
|
* All rights reserved. *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 of the License, or *
|
|
* (at your option) any later version. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
|
* for more details. *
|
|
* *
|
|
****************************************************************************/
|
|
|
|
#include "camera.h"
|
|
|
|
namespace vcg {
|
|
|
|
template<class S>
|
|
Camera<S>::Camera() :
|
|
FocalMm(0.f),
|
|
ViewportPx(vcg::Point2<int>(0, 0)),
|
|
PixelSizeMm(vcg::Point2<S>(0.0, 0.0)),
|
|
CenterPx(vcg::Point2<S>(0.0, 0.0)),
|
|
DistorCenterPx(vcg::Point2<S>(0.0, 0.0)),
|
|
cameraType(PERSPECTIVE)
|
|
{
|
|
k[0] = k[1] = k[2] = k[3] = 0;
|
|
}
|
|
|
|
template<class S>
|
|
template<class Q>
|
|
Camera<S> Camera<S>::Construct(const Camera<Q>& t)
|
|
{
|
|
Camera n;
|
|
n.FocalMm = t.FocalMm;
|
|
n.ViewportPx.Import(t.ViewportPx);
|
|
n.PixelSizeMm.Import(t.PixelSizeMm);
|
|
n.CenterPx.Import(t.CenterPx);
|
|
n.DistorCenterPx.Import(t.DistorCenterPx);
|
|
n.cameraType = t.cameraType;
|
|
n.k[0] = t.k[0];
|
|
n.k[1] = t.k[1];
|
|
n.k[2] = t.k[2];
|
|
n.k[3] = t.k[3];
|
|
return n;
|
|
}
|
|
|
|
template<class S>
|
|
void Camera<S>::SetOrtho(S l, S r, S b, S t, vcg::Point2<int> viewport)
|
|
{
|
|
cameraType = ORTHO;
|
|
ViewportPx = viewport;
|
|
|
|
PixelSizeMm[0] = (r - l) / (S) ViewportPx[0];
|
|
PixelSizeMm[1] = (t - b) / (S) ViewportPx[1];
|
|
|
|
CenterPx[0] = -l / (r - l) * (S) ViewportPx[0];
|
|
CenterPx[1] = -b / (t - b) * (S) ViewportPx[1];
|
|
}
|
|
|
|
template<class S>
|
|
bool Camera<S>::IsOrtho() const
|
|
{
|
|
return (cameraType == ORTHO);
|
|
}
|
|
|
|
/**
|
|
* @brief Set the camera specifying the perspective view
|
|
*/
|
|
template<class S>
|
|
void Camera<S>::SetPerspective(S AngleDeg, S AspectRatio, S Focal, vcg::Point2<int> Viewport)
|
|
{
|
|
cameraType = PERSPECTIVE;
|
|
S halfsize[2];
|
|
|
|
halfsize[1] = tan(math::ToRad(AngleDeg / 2.0f)) * Focal;
|
|
halfsize[0] = halfsize[1] * AspectRatio;
|
|
|
|
SetFrustum(-halfsize[0], halfsize[0], -halfsize[1], halfsize[1], Focal, Viewport);
|
|
}
|
|
|
|
/**
|
|
* @brief set the camera specifying the cavalieri view
|
|
*/
|
|
template<class S>
|
|
void Camera<S>::SetCavalieri(S sx, S dx, S bt, S tp, S Focal, vcg::Point2<int> Viewport)
|
|
{
|
|
cameraType = CAVALIERI;
|
|
SetFrustum(sx, dx, bt, tp, Focal, Viewport);
|
|
}
|
|
|
|
/**
|
|
* @brief set the camera specifying the isometric view
|
|
*/
|
|
template<class S>
|
|
void Camera<S>::SetIsometric(S sx, S dx, S bt, S tp, S Focal, vcg::Point2<int> Viewport)
|
|
{
|
|
cameraType = ISOMETRIC;
|
|
SetFrustum(sx, dx, bt, tp, Focal, Viewport);
|
|
}
|
|
|
|
/**
|
|
* @brief set the camera specifying the frustum view
|
|
*/
|
|
template<class S>
|
|
void Camera<S>::SetFrustum(S sx, S dx, S bt, S tp, S Focal, vcg::Point2<int> Viewport)
|
|
{
|
|
S vp[2];
|
|
vp[0] = dx - sx;
|
|
vp[1] = tp - bt;
|
|
|
|
ViewportPx[0] = Viewport[0];
|
|
if (vp[1] != -1)
|
|
ViewportPx[1] = Viewport[1]; // the user specified the viewport
|
|
else
|
|
ViewportPx[1] = ViewportPx[0]; // default viewport
|
|
|
|
PixelSizeMm[0] = vp[0] / (S) Viewport[0];
|
|
PixelSizeMm[1] = vp[1] / (S) Viewport[1];
|
|
|
|
CenterPx[0] = -sx / vp[0] * (S) Viewport[0];
|
|
CenterPx[1] = -bt / vp[1] * (S) Viewport[1];
|
|
|
|
FocalMm = Focal;
|
|
}
|
|
|
|
/**
|
|
* @brief returns the projection matrix
|
|
* @param nearVal
|
|
* @param farVal
|
|
* @return
|
|
*/
|
|
template<class S>
|
|
vcg::Matrix44<S> Camera<S>::GetMatrix(S nearVal, S farVal)
|
|
{
|
|
S left, right, bottom, top, nr;
|
|
GetFrustum(left, right, bottom, top, nr);
|
|
|
|
if (cameraType == PERSPECTIVE) {
|
|
S ratio = nearVal / nr;
|
|
left *= ratio;
|
|
right *= ratio;
|
|
bottom *= ratio;
|
|
top *= ratio;
|
|
}
|
|
vcg::Matrix44<S> m;
|
|
m[0][0] = 2.0 * nearVal / (right - left);
|
|
m[0][1] = 0;
|
|
m[0][2] = (right + left) / (right - left);
|
|
m[0][3] = 0;
|
|
|
|
m[1][0] = 0;
|
|
m[1][1] = 2 * nearVal / (top - bottom);
|
|
m[1][2] = (top + bottom) / (top - bottom);
|
|
m[1][3] = 0;
|
|
|
|
m[2][0] = 0;
|
|
m[2][1] = 0;
|
|
m[2][2] = -(farVal + nearVal) / (farVal - nearVal);
|
|
m[2][3] = -2 * farVal * nearVal / (farVal - nearVal);
|
|
|
|
m[3][0] = 0;
|
|
m[3][1] = 0;
|
|
m[3][2] = -1;
|
|
m[3][3] = 0;
|
|
|
|
return m;
|
|
}
|
|
|
|
/**
|
|
* @brief returns the frustum
|
|
*/
|
|
template<class S>
|
|
void Camera<S>::GetFrustum(S& sx, S& dx, S& bt, S& tp, S& nr) const
|
|
{
|
|
dx = CenterPx.X() * PixelSizeMm.X(); // scaled center
|
|
sx = -((S) ViewportPx.X() - CenterPx.X()) * PixelSizeMm.X();
|
|
|
|
bt = -CenterPx.Y() * PixelSizeMm.Y();
|
|
tp = ((S) ViewportPx.Y() - CenterPx.Y()) * PixelSizeMm.Y();
|
|
|
|
nr = FocalMm;
|
|
}
|
|
|
|
/**
|
|
* @brief project a point from 3d CAMERA space to the camera's plane
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::Project(const vcg::Point3<S>& p) const
|
|
{
|
|
vcg::Point2<S> q = Point2<S>(p[0], p[1]);
|
|
|
|
if (!IsOrtho()) {
|
|
q[0] *= FocalMm / p.Z();
|
|
q[1] *= FocalMm / p.Z();
|
|
|
|
if (k[0] != 0) {
|
|
vcg::Point2<S> d;
|
|
d = UndistortedToDistorted(q);
|
|
q = d;
|
|
}
|
|
}
|
|
|
|
return q;
|
|
}
|
|
|
|
/**
|
|
* @brief unproject a point from the camera 2d plane [-1,-1]x[1,1] (plus depth) to 3d CAMERA space
|
|
*/
|
|
template<class S>
|
|
vcg::Point3<S> Camera<S>::UnProject(const vcg::Point2<S>& p, const S& d) const
|
|
{
|
|
vcg::Point3<S> np = Point3<S>(p[0], p[1], d);
|
|
|
|
if (!IsOrtho()) {
|
|
if (k[0] != 0) {
|
|
vcg::Point2<S> d = Point2<S>(p[0], p[1]);
|
|
vcg::Point2<S> u = DistortedToUndistorted(d);
|
|
np[0] = u[0];
|
|
np[1] = u[1];
|
|
}
|
|
|
|
np[0] /= FocalMm / d;
|
|
np[1] /= FocalMm / d;
|
|
}
|
|
|
|
return np;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms local plane coords to vieport (pixel) coords
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::LocalToViewportPx(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> np;
|
|
|
|
np[0] = (p[0] / PixelSizeMm.X()) + CenterPx.X();
|
|
np[1] = (p[1] / PixelSizeMm.Y()) + CenterPx.Y();
|
|
|
|
return np;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms vieport (pixel) coords to local plane coords
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::ViewportPxToLocal(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> ps;
|
|
ps[0] = (p[0] - CenterPx.X()) * PixelSizeMm.X();
|
|
ps[1] = (p[1] - CenterPx.Y()) * PixelSizeMm.Y();
|
|
return ps;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms vieport (pixel) coords to [-1 1] coords
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::ViewportPxTo_neg1_1(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> ps;
|
|
ps[0] =
|
|
2.0f * ((p[0] - CenterPx.X()) * PixelSizeMm.X()) / (PixelSizeMm.X() * (S) ViewportPx[0]);
|
|
ps[1] =
|
|
2.0f * ((p[1] - CenterPx.Y()) * PixelSizeMm.Y()) / (PixelSizeMm.Y() * (S) ViewportPx[1]);
|
|
return ps;
|
|
}
|
|
/**
|
|
* @brief transforms [-1 1] coords to vieport (pixel) coords MICHELE IO
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::Neg1_1ToViewportPx(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> ps;
|
|
ps[0] =
|
|
((PixelSizeMm.X() * (S) ViewportPx[0] * p[0]) / (2.0f * PixelSizeMm.X())) + CenterPx.X();
|
|
ps[1] =
|
|
((PixelSizeMm.Y() * (S) ViewportPx[1] * p[1]) / (2.0f * PixelSizeMm.Y())) + CenterPx.Y();
|
|
return ps;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms local plane coords to [0-1] coords
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::LocalTo_0_1(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> ps;
|
|
ps[0] = (p[0] / PixelSizeMm.X() + CenterPx.X()) / (S) ViewportPx[0];
|
|
ps[1] = (p[1] / PixelSizeMm.Y() + CenterPx.Y()) / (S) ViewportPx[1];
|
|
return ps;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms local plane coords to [-1 1] coords
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::LocalTo_neg1_1(const vcg::Point2<S>& p) const
|
|
{
|
|
vcg::Point2<S> ps;
|
|
ps[0] = 2.0f * p[0] / (PixelSizeMm.X() * (S) ViewportPx[0]);
|
|
ps[1] = 2.0f * p[1] / (PixelSizeMm.Y() * (S) ViewportPx[1]);
|
|
return ps;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms an undistorted 2D camera plane point in a distorted 2D camera plane point
|
|
*/
|
|
template<class Scalar>
|
|
vcg::Point2<Scalar> Camera<Scalar>::UndistortedToDistorted(vcg::Point2<Scalar> u) const
|
|
{
|
|
vcg::Point2<Scalar> dis;
|
|
vcg::Point2<Scalar> dc = ViewportPxTo_neg1_1(DistorCenterPx);
|
|
const Scalar SQRT3 = Scalar(1.732050807568877293527446341505872366943);
|
|
const Scalar CBRT = Scalar(0.33333333333333333333333);
|
|
Scalar Ru, Rd, lambda, c, d, Q, R, D, S, T, sinT, cosT;
|
|
|
|
if (((u[0] - dc[0]) == 0 && (u[1] - dc[1]) == 0) || k[0] == 0) {
|
|
dis[0] = u[0];
|
|
dis[1] = u[1];
|
|
return dis;
|
|
}
|
|
|
|
Ru = hypot((u[0] - dc[0]), (u[1] - dc[1])); /* SQRT(Xu*Xu+Yu*Yu) */
|
|
c = 1 / k[0];
|
|
d = -c * Ru;
|
|
|
|
Q = c / 3;
|
|
R = -d / 2;
|
|
if (R < 0)
|
|
D = pow(Q, 3) + sqrt(-R);
|
|
else
|
|
D = pow(Q, 3) + sqrt(R);
|
|
|
|
if (D >= 0) /* one real root */
|
|
{
|
|
D = sqrt(D);
|
|
S = pow((R + D), CBRT);
|
|
if (R >= D)
|
|
T = pow((R - D), CBRT);
|
|
else
|
|
T = -pow(abs((int) (R - D)), CBRT); // MODIFICATO DA ME
|
|
Rd = S + T;
|
|
|
|
if (Rd < 0)
|
|
Rd = sqrt(-1 / (3 * k[0]));
|
|
}
|
|
else /* three real roots */
|
|
{
|
|
D = sqrt(-D);
|
|
S = pow((Scalar) (hypot(R, D)), (Scalar) CBRT);
|
|
T = atan2(D, R) / 3;
|
|
// SinCos(T, sinT, cosT);
|
|
sinT = sin(T);
|
|
cosT = cos(T);
|
|
|
|
/* the larger positive root is 2*S*cos(T) */
|
|
/* the smaller positive root is -S*cos(T) + SQRT(3)*S*sin(T) */
|
|
/* the negative root is -S*cos(T) - SQRT(3)*S*sin(T) */
|
|
Rd = -S * cosT + SQRT3 * S * sinT; /* use the smaller positive root */
|
|
}
|
|
|
|
lambda = Rd / Ru;
|
|
|
|
dis[0] = u[0] * lambda;
|
|
dis[1] = u[1] * lambda;
|
|
|
|
return dis;
|
|
}
|
|
|
|
/**
|
|
* @brief transforms a distorted 2D camera plane point in an undistorted 2D camera plane point
|
|
*/
|
|
template<class S>
|
|
vcg::Point2<S> Camera<S>::DistortedToUndistorted(vcg::Point2<S> d) const
|
|
{
|
|
vcg::Point2<S> u;
|
|
vcg::Point2<S> dc = ViewportPxTo_neg1_1(DistorCenterPx);
|
|
S r = sqrt(pow((d[0] - dc[0]), 2) + pow((d[1] - dc[1]), 2));
|
|
u[0] = d[0] * (1 - k[0] * r * r);
|
|
u[1] = d[1] * (1 - k[0] * r * r);
|
|
|
|
return u;
|
|
}
|
|
|
|
template<class S>
|
|
bool Camera<S>::operator==(const Camera<S> &oth) const
|
|
{
|
|
return
|
|
FocalMm == oth.FocalMm &&
|
|
ViewportPx == oth.ViewportPx &&
|
|
PixelSizeMm == oth.PixelSizeMm &&
|
|
CenterPx == oth.CenterPx &&
|
|
DistorCenterPx == oth.DistorCenterPx &&
|
|
k == oth.k &&
|
|
cameraType == oth.cameraType;
|
|
}
|
|
|
|
template<class S>
|
|
bool Camera<S>::operator!=(const Camera<S> &oth) const
|
|
{
|
|
return !(*this == oth);
|
|
}
|
|
|
|
} // namespace vcg
|