private List <PointOnGrid> ComputeProjection(StateVector stateVec, List <PointOnGrid> origPoints)
{ /*
* rotation_matrix = yaw_rotation_matrix * pitch_rotation_matrix * roll_rotation_matrix;
* world_matrix = translation_matrix * rotation_matrix;
*/
List <PointOnGrid> projPoints = new List <PointOnGrid>();
double[,] T_mat = Transltions.CreateTranslationMatrix(new double[]
{ stateVec.XCG, stateVec.YCG, stateVec.ZCG });
double[,] R_Gen = Rotations.CreateRotationThetaPsiPhi(Rotations.ToRadians(stateVec.PitchAngle),
Rotations.ToRadians(stateVec.YawAngle), Rotations.ToRadians(stateVec.RollAngle));
double[,] World_mat = BLAS.Multiply(T_mat, R_Gen);
/*
* [u,v]= Cam_Project_mat*world_matrix*[x y z]
*/
double[,] Cam_Project_mat = Camera.CreateProjectionMatrix();
double[,] Launch_to_Camera = Camera.CreateAxisTranformMatrix();
double[,] FinalMat = BLAS.Multiply(Cam_Project_mat, BLAS.Multiply(Launch_to_Camera, World_mat));
for (int ii = 0; ii < origPoints.Count; ii++)
{
// Apply transformation to original position
double[] vec = new double[] { origPoints[ii].X, origPoints[ii].Y, origPoints[ii].Z, 1 };
double[] transf_camera = BLAS.Multiply(FinalMat, vec);
transf_camera[0] /= transf_camera[2];
transf_camera[1] /= transf_camera[2];
transf_camera[2] /= transf_camera[2];
//double[] transfScaled = BLAS.Multiply(Scales.ScaleMat, transf_camera);
if (transf_camera[0] > 0 && transf_camera[1] > 0 && transf_camera[0] < Camera.CameraSettings.SensorWidth && transf_camera[1] < Camera.CameraSettings.SensorHeight)
{
projPoints.Add(new PointOnGrid((float)transf_camera[0], (float)transf_camera[1], 0, ii, origPoints[ii].Intensity, origPoints[ii].Component));
}
}
return(projPoints);
}
public void ComputeProjection(double[] CenterOfMassCartesian)
{ /*
* rotation_matrix = yaw_rotation_matrix * pitch_rotation_matrix * roll_rotation_matrix;
* world_matrix = translation_matrix * rotation_matrix;
*/
ImagePoints = new ShapePoint2D[OriginalPoints.Length];
#region Debug
#warning DEBUG
//var TestVec = new double[] {-0.5, 0.5, 0.7071, 1};
//RollAngle = 0;
//PitchAngle = 45;
//YawAngle = 45;
//CenterOfMassCartesian = new double[] { 0, 50, 100 };
#endregion
//double[,] R_roll = Rotations.CreateRotationMatrixRoll(Rotations.ToRadians(RollAngle)); //Rx
//double[,] R_pitch = Rotations.CreateRotationMatrixPitch(Rotations.ToRadians(PitchAngle)); //Ry
//double[,] R_yaw = Rotations.CreateRotationMatrixYaw(Rotations.ToRadians(YawAngle)); //Rz
//double[,] temp = BLAS.Multiply(R_roll, R_yaw);
//double[,] R_Gen = BLAS.Multiply(R_pitch, temp);
double[,] T_mat = Transltions.CreateTranslationMatrix(CenterOfMassCartesian);
//R_Gen = Rotations.CreateRotationPsiThetaPhi(Rotations.ToRadians(YawAngle), Rotations.ToRadians(PitchAngle), Rotations.ToRadians(RollAngle));
double[,] R_Gen = Rotations.CreateRotationThetaPsiPhi(Rotations.ToRadians(PitchAngle), Rotations.ToRadians(YawAngle), Rotations.ToRadians(RollAngle));
double[,] World_mat = BLAS.Multiply(T_mat, R_Gen);
/*
* [u,v]= Cam_Project_mat*world_matrix*[x y z]
*/
double[,] Cam_Project_mat = Camera.CreateProjectionMatrix();
double[,] Launch_to_Camera = Camera.CreateAxisTranformMatrix();
double[,] FinalMat = BLAS.Multiply(Cam_Project_mat, BLAS.Multiply(Launch_to_Camera, World_mat));
#region Debug
#warning DEBUG
//double[] Testtransf = BLAS.Multiply(FinalMat, TestVec);
//Testtransf[0] /= Testtransf[2];
//Testtransf[1] /= Testtransf[2];
//Testtransf[2] /= Testtransf[2];
#endregion
SetLedSize(FinalMat);
for (int ii = 0; ii < OriginalPoints.Length; ii++)
{
// Apply transformation to original position
double[] vec = new double[] { OriginalPoints[ii].X, OriginalPoints[ii].Y, OriginalPoints[ii].Z, 1 };
double[] transf_camera = BLAS.Multiply(FinalMat, vec);
transf_camera[0] /= transf_camera[2];
transf_camera[1] /= transf_camera[2];
transf_camera[2] /= transf_camera[2];
double[] transfScaled = BLAS.Multiply(Scales.ScaleMat, transf_camera);
if (transfScaled[0] > 0 && transfScaled[1] > 0 && transfScaled[0] < Camera.CameraSettings.SensorWidth && transfScaled[1] < Camera.CameraSettings.SensorHeight)
{
ImagePoints[ii] = new ShapePoint2D(transfScaled[0], transfScaled[1], OriginalPoints[ii].isLED);
}
else
{
ImagePoints[ii] = new ShapePoint2D(-10, -10, false);
}
}
}