static Algerbra.Matrix<double> RotationMatrix(double azimuth, double elevation)
{
Algerbra.Matrix<double> R = new Algerbra.Matrix<double>(3, 3);
R.SetMatrix(new double[] { Math.Cos(azimuth), 0, -Math.Sin(azimuth),
Math.Sin(azimuth)*Math.Sin(elevation), Math.Cos(elevation), Math.Cos(azimuth)*Math.Sin(elevation),
Math.Cos(elevation)*Math.Sin(azimuth), -Math.Sin(elevation), Math.Cos(azimuth)*Math.Cos(elevation) });
return R;
}
static Algerbra.Matrix<double> GetMext(double azimuth, double elevation, double[] d_w)
{
Algerbra.Matrix<double> R = RotationMatrix(azimuth, elevation);
Algerbra.Matrix<double> dw = new Algerbra.Matrix<double>(3, 1);
dw.SetMatrix(d_w);
Algerbra.Matrix<double> Mext = R | (-R * dw);
return Mext;
}
static Algerbra.Matrix<double> GetMint(double s_x, double s_y, double f)
{
Algerbra.Matrix<double> Mint = new Algerbra.Matrix<double>(3, 3);
double o_x = s_x / 2;
double o_y = s_y / 2;
double a = 1;
Mint.SetMatrix(new double[] { f, 0, o_x, 0, f * a, o_y, 0, 0, 1 });
return Mint;
}
static public PointF Project(double[] x, double s_x, double s_y, double f, double[] d_w, double azimuth, double elevation)
{
Algerbra.Matrix<double> Mext = GetMext(azimuth, elevation, d_w);
Algerbra.Matrix<double> Mint = GetMint(s_x, s_y, f);
Algerbra.Matrix<double> X_h = new Algerbra.Matrix<double>(4, 1);
X_h.SetMatrix(new double[] { x[0], x[1], x[2], 1.0});
//Debug.Print((Mint * Mext).ToString());
Algerbra.Matrix<double> P = Mint * Mext * X_h;
return new PointF((float)(P.GetValByIndex(0, 0) / P.GetValByIndex(2, 0)), (float)(P.GetValByIndex(1, 0) / P.GetValByIndex(2, 0)));
}
static public PointF[] ProjectVector(List<double[]> x, double s_x, double s_y, double f, double[] d_w, double azimuth, double elevation)
{
Algerbra.Matrix<double> Mext = GetMext(azimuth, elevation, d_w);
Algerbra.Matrix<double> Mint = GetMint(s_x, s_y, f);
Algerbra.Matrix<double> X_h = new Algerbra.Matrix<double>(4, 1);
PointF[] Pvec = new PointF[x.Count];
for (int i = 0; i < x.Count; i++)
{
X_h.SetMatrix(new double[] { x[i][0], x[i][1], x[i][2], 1.0 });
Algerbra.Matrix<double> P = Mint * Mext * X_h;
Pvec[i] = new PointF((float)(P.GetValByIndex(0, 0) / P.GetValByIndex(2, 0)), (float)(P.GetValByIndex(1, 0) / P.GetValByIndex(2, 0)));
}
return Pvec;
}