public static CDVMatrix InvertZ()
{
CDVMatrix vret = new CDVMatrix(Identity);
vret.M33 = -1d;
return(vret);
}
public static CDVMatrix BuildModulationMatrix(float X, float Y, float DX, float DY)
{
CDVMatrix vret = null;
try
{
CDVMatrix _vret = new CDVMatrix();
double x1 = X;
double y1 = Y;
double x2 = X + DX;
double y2 = Y + DY;
_vret.M11 = 1 / (x2 - x1);
_vret.M12 = 0;
_vret.M13 = 0;
_vret.M14 = -x1 / (x2 - x1);
_vret.M21 = 0;
_vret.M22 = 1 / (y2 - y1);
_vret.M23 = 0;
_vret.M24 = -y1 / (y2 - y1);
vret = _vret;
}
finally
{
}
return(vret);
}
public void Copy(CDVMatrix pCDVMatrix)
{
if (pCDVMatrix == null)
{
return;
}
M11 = pCDVMatrix.M11;
M12 = pCDVMatrix.M12;
M13 = pCDVMatrix.M13;
M14 = pCDVMatrix.M14;
M21 = pCDVMatrix.M21;
M22 = pCDVMatrix.M22;
M23 = pCDVMatrix.M23;
M24 = pCDVMatrix.M24;
M31 = pCDVMatrix.M31;
M32 = pCDVMatrix.M32;
M33 = pCDVMatrix.M33;
M34 = pCDVMatrix.M34;
M41 = pCDVMatrix.M41;
M42 = pCDVMatrix.M42;
M43 = pCDVMatrix.M43;
M44 = pCDVMatrix.M44;
}
public CDVMatrix(CDVMatrix pDVMatrixS)
{
if (pDVMatrixS == null)
{
return;
}
M11 = pDVMatrixS.M11;
M12 = pDVMatrixS.M12;
M13 = pDVMatrixS.M13;
M14 = pDVMatrixS.M14;
M21 = pDVMatrixS.M21;
M22 = pDVMatrixS.M22;
M23 = pDVMatrixS.M23;
M24 = pDVMatrixS.M24;
M31 = pDVMatrixS.M31;
M32 = pDVMatrixS.M32;
M33 = pDVMatrixS.M33;
M34 = pDVMatrixS.M34;
M41 = pDVMatrixS.M41;
M42 = pDVMatrixS.M42;
M43 = pDVMatrixS.M43;
M44 = pDVMatrixS.M44;
}
bool interp_step(int octv, int intvl, int r, int c, out double xi, out double xr, out double xc)
{
xi = xr = xc = 0;
double _dx = 0;
double _dy = 0;
double _ds = 0;
deriv_3D(octv, intvl, r, c, out _dx, out _dy, out _ds);
CDVMatrix H = hessian_3D(octv, intvl, r, c);
if (H == null)
{
return(false);
}
CDVMatrix H_inv = CDVMatrix.Inversion(H);
if (H_inv == null)
{
return(false);
}
H_inv.MultiplyVector(_dx, _dy, _ds, out xi, out xr, out xc);
return(true);
}
public void Add(CDVMatrix pCDVMatrix)
{
if (pCDVMatrix == null)
{
return;
}
M11 += pCDVMatrix.M11;
M12 += pCDVMatrix.M12;
M13 += pCDVMatrix.M13;
M14 += pCDVMatrix.M14;
M21 += pCDVMatrix.M21;
M22 += pCDVMatrix.M22;
M23 += pCDVMatrix.M23;
M24 += pCDVMatrix.M24;
M31 += pCDVMatrix.M31;
M32 += pCDVMatrix.M32;
M33 += pCDVMatrix.M33;
M34 += pCDVMatrix.M34;
M41 += pCDVMatrix.M41;
M42 += pCDVMatrix.M42;
M43 += pCDVMatrix.M43;
M44 += pCDVMatrix.M44;
}
public static CDVMatrix Inversion(CDVMatrix pCDVMatrix)
{
if (pCDVMatrix == null)
{
return(null);
}
CDVMatrix vret = null;
try
{
double[,] Mij = pCDVMatrix.Mij();
double[,] mij = Invert(Mij);
if (mij == null)
{
return(null);
}
vret = new CDVMatrix(mij);
}
finally
{
}
return(vret);
}
/**
* - Heading_Tilt_Roll retourne la CDVMatrix représentant la suite d'opérations:
* - rotation de Heading / Z
* - rotation de Tilt / X
* - rotation de Roll / Z
* - Heading/Tilt/Roll: en radians
**/
public static CDVMatrix Heading_Tilt_Roll(double Heading, double Tilt, double Roll)
{
List <CDVMatrix> apCDVMatrix = new List <CDVMatrix>();
apCDVMatrix.Add(CDVMatrix.RotationZ(Heading));
apCDVMatrix.Add(CDVMatrix.RotationX(Tilt));
apCDVMatrix.Add(CDVMatrix.RotationZ(Roll));
return(CDVMatrix.Multiplication(apCDVMatrix, true));
}
CDVMatrix hessian_3D(int octv, int intvl, int r, int c)
{
CDVMatrix vret = new CDVMatrix();
double v, dxx, dyy, dss, dxy, dxs, dys;
int step = init_sample * COpenSURF.cvRound(COpenSURF.pow(2.0f, octv));
v = getValLowe(octv, intvl, r, c);
dxx = (getValLowe(octv, intvl, r, c + step) +
getValLowe(octv, intvl, r, c - step) - 2 * v);
dyy = (getValLowe(octv, intvl, r + step, c) +
getValLowe(octv, intvl, r - step, c) - 2 * v);
dss = (getValLowe(octv, intvl + 1, r, c) +
getValLowe(octv, intvl - 1, r, c) - 2 * v);
dxy = (getValLowe(octv, intvl, r + step, c + step) -
getValLowe(octv, intvl, r + step, c - step) -
getValLowe(octv, intvl, r - step, c + step) +
getValLowe(octv, intvl, r - step, c - step)) / 4.0;
dxs = (getValLowe(octv, intvl + 1, r, c + step) -
getValLowe(octv, intvl + 1, r, c - step) -
getValLowe(octv, intvl - 1, r, c + step) +
getValLowe(octv, intvl - 1, r, c - step)) / 4.0;
dys = (getValLowe(octv, intvl + 1, r + step, c) -
getValLowe(octv, intvl + 1, r - step, c) -
getValLowe(octv, intvl - 1, r + step, c) +
getValLowe(octv, intvl - 1, r - step, c)) / 4.0;
/***
* cvmSet( H, 0, 0, dxx );
* cvmSet( H, 0, 1, dxy );
* cvmSet( H, 0, 2, dxs );
*
* cvmSet( H, 1, 0, dxy );
* cvmSet( H, 1, 1, dyy );
* cvmSet( H, 1, 2, dys );
*
* cvmSet( H, 2, 0, dxs );
* cvmSet( H, 2, 1, dys );
* cvmSet( H, 2, 2, dss );
***/
vret.M11 = dxx;
vret.M12 = dxy;
vret.M13 = dxs;
vret.M21 = dxy;
vret.M22 = dyy;
vret.M23 = dys;
vret.M31 = dxs;
vret.M32 = dys;
vret.M33 = dss;
return(vret);
}
public static CDVMatrix Multiplication(CDVMatrix pCDVMatrix1, CDVMatrix pCDVMatrix2)
{
if (pCDVMatrix1 == null || pCDVMatrix2 == null)
{
return(null);
}
CDVMatrix vret = new CDVMatrix(pCDVMatrix1);
vret.Multiply(pCDVMatrix2);
return(vret);
}
public static double Determinant(CDVMatrix pCDVMatrix)
{
double vret = 0;
if (pCDVMatrix == null)
{
return(vret);
}
vret = pCDVMatrix.M11 * (pCDVMatrix.M22 * pCDVMatrix.M33 - pCDVMatrix.M32 * pCDVMatrix.M23)
- pCDVMatrix.M12 * (pCDVMatrix.M21 * pCDVMatrix.M33 - pCDVMatrix.M31 * pCDVMatrix.M23)
+ pCDVMatrix.M13 * (pCDVMatrix.M21 * pCDVMatrix.M32 - pCDVMatrix.M31 * pCDVMatrix.M22);
return(vret);
}
/***
* void cvGEMM( const CvArr* src1, const CvArr* src2, double alpha,
* const CvArr* src3, double beta, CvArr* dst, int tABC=0 );
#define cvMatMulAdd( src1, src2, src3, dst ) cvGEMM( src1, src2, 1, src3, 1, dst, 0 )
#define cvMatMul( src1, src2, dst ) cvMatMulAdd( src1, src2, 0, dst )
*
* src1
* The first source array.
* src2
* The second source array.
* src3
* The third source array (shift). Can be NULL, if there is no shift.
* dst
* The destination array.
* tABC
* The operation flags that can be 0 or combination of the following values:
* CV_GEMM_A_T - transpose src1
* CV_GEMM_B_T - transpose src2
* CV_GEMM_C_T - transpose src3
* for example, CV_GEMM_A_T+CV_GEMM_C_T corresponds to
*
* alpha*src1T*src2 + beta*srcT
*
* The function cvGEMM performs generalized matrix multiplication:
*
* dst = alpha*op(src1)*op(src2) + beta*op(src3), where op(X) is X or XT
***/
internal static void cvGEMM(CDVMatrix src1, CDVMatrix src2, double alpha, CDVMatrix src3, double beta, CDVMatrix dst, int tABC)
{
CDVMatrix matrix1 = new CDVMatrix(src1);
matrix1.Multiply(alpha);
matrix1.Multiply(src2);
CDVMatrix matrix2 = new CDVMatrix(src3);
matrix2.Multiply(beta);
dst.Copy(matrix1);
dst.Add(matrix2);
}
public void RotateXYZ(double AngleX, double AngleY, double AngleZ)
{
if (AngleX != 0)
{
Multiply(CDVMatrix.RotationX(AngleX));
}
if (AngleY != 0)
{
Multiply(CDVMatrix.RotationY(AngleY));
}
if (AngleZ != 0)
{
Multiply(CDVMatrix.RotationZ(AngleZ));
}
}
public static CDVMatrix Multiplication(List <CDVMatrix> apCDVMatrix, bool bRevertOrder)
{
if (apCDVMatrix == null || apCDVMatrix.Count == 0)
{
return(null);
}
CDVMatrix vret = null;
if (bRevertOrder == false)
{
for (int i = 0; i < apCDVMatrix.Count; i++)
{
if (apCDVMatrix[i] == null)
{
continue;
}
if (vret == null)
{
vret = new CDVMatrix((CDVMatrix)apCDVMatrix[i]);
}
else
{
vret = Multiplication((CDVMatrix)apCDVMatrix[i], vret);
}
}
}
else
{
for (int i = apCDVMatrix.Count - 1; i >= 0; i--)
{
if (apCDVMatrix[i] == null)
{
continue;
}
if (vret == null)
{
vret = new CDVMatrix((CDVMatrix)apCDVMatrix[i]);
}
else
{
vret = Multiplication((CDVMatrix)apCDVMatrix[i], vret);
}
}
}
return(vret);
}
public void Multiply(CDVMatrix pCDVMatrix)
{
if (pCDVMatrix == null)
{
return;
}
double m11 = M11 * pCDVMatrix.M11 + M12 * pCDVMatrix.M21 + M13 * pCDVMatrix.M31 + M14 * pCDVMatrix.M41;
double m12 = M11 * pCDVMatrix.M12 + M12 * pCDVMatrix.M22 + M13 * pCDVMatrix.M32 + M14 * pCDVMatrix.M42;
double m13 = M11 * pCDVMatrix.M13 + M12 * pCDVMatrix.M23 + M13 * pCDVMatrix.M33 + M14 * pCDVMatrix.M43;
double m14 = M11 * pCDVMatrix.M14 + M12 * pCDVMatrix.M24 + M13 * pCDVMatrix.M34 + M14 * pCDVMatrix.M44;
double m21 = M21 * pCDVMatrix.M11 + M22 * pCDVMatrix.M21 + M23 * pCDVMatrix.M31 + M24 * pCDVMatrix.M41;
double m22 = M21 * pCDVMatrix.M12 + M22 * pCDVMatrix.M22 + M23 * pCDVMatrix.M32 + M24 * pCDVMatrix.M42;
double m23 = M21 * pCDVMatrix.M13 + M22 * pCDVMatrix.M23 + M23 * pCDVMatrix.M33 + M24 * pCDVMatrix.M43;
double m24 = M21 * pCDVMatrix.M14 + M22 * pCDVMatrix.M24 + M23 * pCDVMatrix.M34 + M24 * pCDVMatrix.M44;
double m31 = M31 * pCDVMatrix.M11 + M32 * pCDVMatrix.M21 + M33 * pCDVMatrix.M31 + M34 * pCDVMatrix.M41;
double m32 = M31 * pCDVMatrix.M12 + M32 * pCDVMatrix.M22 + M33 * pCDVMatrix.M32 + M34 * pCDVMatrix.M42;
double m33 = M31 * pCDVMatrix.M13 + M32 * pCDVMatrix.M23 + M33 * pCDVMatrix.M33 + M34 * pCDVMatrix.M43;
double m34 = M31 * pCDVMatrix.M14 + M32 * pCDVMatrix.M24 + M33 * pCDVMatrix.M34 + M34 * pCDVMatrix.M44;
double m41 = M41 * pCDVMatrix.M11 + M42 * pCDVMatrix.M21 + M43 * pCDVMatrix.M31 + M44 * pCDVMatrix.M41;
double m42 = M41 * pCDVMatrix.M12 + M42 * pCDVMatrix.M22 + M43 * pCDVMatrix.M32 + M44 * pCDVMatrix.M42;
double m43 = M41 * pCDVMatrix.M13 + M42 * pCDVMatrix.M23 + M43 * pCDVMatrix.M33 + M44 * pCDVMatrix.M43;
double m44 = M41 * pCDVMatrix.M14 + M42 * pCDVMatrix.M24 + M43 * pCDVMatrix.M34 + M44 * pCDVMatrix.M44;
M11 = m11;
M12 = m12;
M13 = m13;
M14 = m14;
M21 = m21;
M22 = m22;
M23 = m23;
M24 = m24;
M31 = m31;
M32 = m32;
M33 = m33;
M34 = m34;
M41 = m41;
M42 = m42;
M43 = m43;
M44 = m44;
}
public static CDVMatrix RotationAlphaBetaGamma(double alpha, double beta, double gamma)
{
CDVMatrix vret = new CDVMatrix();
CDVMatrix mbetaalpha = CDVMatrix.RotationAlphaBeta(alpha, beta);
mbetaalpha.Transpose();
CDVMatrix mgamma = CDVMatrix.RotationX(gamma);
mgamma.Transpose();
vret = CDVMatrix.Multiplication(mgamma, mbetaalpha);
vret.Transpose();
return(vret);
}
public static CDVMatrix Translation(double x, double y, double z)
{
CDVMatrix vret = new CDVMatrix();
vret.M11 = 1;
vret.M12 = 0;
vret.M13 = 0;
vret.M14 = x;
vret.M21 = 0;
vret.M22 = 1;
vret.M23 = 0;
vret.M24 = y;
vret.M31 = 0;
vret.M32 = 0;
vret.M33 = 1;
vret.M34 = z;
return(vret);
}
public static CDVMatrix Scaling(double fx, double fy, double fz)
{
CDVMatrix vret = new CDVMatrix();
vret.M11 = fx;
vret.M12 = 0;
vret.M13 = 0;
vret.M14 = 0;
vret.M21 = 0;
vret.M22 = fy;
vret.M23 = 0;
vret.M24 = 0;
vret.M31 = 0;
vret.M32 = 0;
vret.M33 = fz;
vret.M34 = 0;
return(vret);
}
public static CDVMatrix RotationZ(double angle)
{
CDVMatrix vret = new CDVMatrix();
double cos = (double)Math.Cos(angle);
double sin = (double)Math.Sin(angle);
vret.M11 = cos;
vret.M12 = -sin;
vret.M13 = 0;
vret.M21 = sin;
vret.M22 = cos;
vret.M23 = 0;
vret.M31 = 0;
vret.M32 = 0;
vret.M33 = 1;
return(vret);
}
public static CDVMatrix BuildWindowMatrix(double Xs, double Ys, double DXs, double DYs, double Xd, double Yd, double DXd, double DYd)
{
if (DXs == 0 || DYs == 0)
{
return(null);
}
CDVMatrix vret = new CDVMatrix();
vret.M11 = DXd / DXs;
vret.M12 = 0;
vret.M13 = 0;
vret.M14 = (Xd - (Xs * DXd) / DXs);
vret.M21 = 0;
vret.M22 = DYd / DYs;
vret.M23 = 0;
vret.M24 = (Yd - (Ys * DYd) / DYs);
return(vret);
}
public static CDVMatrix RotationAlphaBeta(double alpha, double beta)
{
CDVMatrix vret = new CDVMatrix();
double cosalpha = (double)Math.Cos(alpha);
double sinalpha = (double)Math.Sin(alpha);
double cosbeta = (double)Math.Cos(beta);
double sinbeta = (double)Math.Sin(beta);
vret.M11 = cosalpha * cosbeta;
vret.M12 = -sinalpha;
vret.M13 = -cosalpha * sinbeta;
vret.M21 = sinalpha * cosbeta;
vret.M22 = cosalpha;
vret.M23 = -sinalpha * sinbeta;
vret.M31 = sinbeta;
vret.M32 = 0;
vret.M33 = cosbeta;
return(vret);
}
public bool Compare(CDVMatrix pDVMatrix)
{
if (pDVMatrix == null)
{
return(false);
}
if (M11 != pDVMatrix.M11 || M12 != pDVMatrix.M12 || M13 != pDVMatrix.M13 || M14 != pDVMatrix.M14)
{
return(false);
}
if (M21 != pDVMatrix.M21 || M22 != pDVMatrix.M22 || M23 != pDVMatrix.M23 || M24 != pDVMatrix.M24)
{
return(false);
}
if (M31 != pDVMatrix.M31 || M32 != pDVMatrix.M32 || M33 != pDVMatrix.M33 || M34 != pDVMatrix.M34)
{
return(false);
}
if (M41 != pDVMatrix.M41 || M42 != pDVMatrix.M42 || M43 != pDVMatrix.M43 || M44 != pDVMatrix.M44)
{
return(false);
}
return(true);
}
internal static CDVMatrix cvInvert(CDVMatrix pCDVMatrix)
{
return(pCDVMatrix != null ? CDVMatrix.Inversion(pCDVMatrix) : null);
}
internal static CDVMatrix cvCreateMat()
{
CDVMatrix vret = new CDVMatrix();
return(vret);
}
