$rot_mat1=eul2mat($data_doc1[$i],$data_doc1[$i+1],$data_doc1[$i+2]);
+#Catch the reference to the first matrix
$rot_mat2=eul2mat($data_doc2[$i],$data_doc2[$i+1],$data_doc2[$
+i+2]); #Catch the reference to the second matrix
$cbn_mat=mat_multiply($rot_mat1,$rot_mat2); #Multiply the two
+matrices
@cbn_angles=mat2eul($cbn_mat); #Convert the matrix back to eul
+er
sub eul2mat {
my $phi_rad=deg2rad($_[0]);
my $theta_rad=deg2rad($_[1]);
my $psi_rad=deg2rad($_[2]);
#print "$phi_rad $theta_rad $psi_rad\n";
my @rot_mat;
$rot_mat[0][0]=cos($phi_rad)*cos($theta_rad)*cos($psi_rad)-sin
+($phi_rad)*sin($psi_rad);
$rot_mat[0][1]=cos($phi_rad)*cos($theta_rad)*sin($psi_rad)+sin
+($phi_rad)*cos($psi_rad);
$rot_mat[0][2]=-cos($phi_rad)*sin($theta_rad);
$rot_mat[1][0]=-sin($phi_rad)*cos($theta_rad)*cos($psi_rad)-si
+n($psi_rad)*cos($phi_rad);
$rot_mat[1][1]=-sin($phi_rad)*cos($theta_rad)*sin($psi_rad)+co
+s($phi_rad)*cos($psi_rad);
$rot_mat[1][2]=sin($phi_rad)*sin($theta_rad);
$rot_mat[2][0]=sin($theta_rad)*cos($psi_rad);
$rot_mat[2][1]=sin($theta_rad)*sin($psi_rad);
$rot_mat[2][2]=cos($theta_rad);
say Dumper \@rot_mat;
return \@rot_mat;
}
sub mat_multiply {
my ($mat1,$mat2)=@_;
my ($i,$j,$k);
my $result_mat=[];
for $i (0..2) {
for $j (0..2) {
for $k (0..2) {
$result_mat->[$i][$j]+=$mat1->[$i][$k]
+ * $mat2->[$k][$j];
}
}
}
return $result_mat;
}
sub mat2eul {
print "$_[2][0]\n";
my $theta_deg=rad2deg(atan2((sqrt($_[2][0]**2+$_[2][1]**2)),$_
+[2][2]));
my $phi_deg=rad2deg(atan2(($_[1][2]/sin(deg2rad($theta_deg))),
+(-$_[0][2]/sin(deg2rad($theta_deg)))));
my $psi_deg=rad2deg(atan2(($_[2][1]/sin(deg2rad($theta_deg))),
+($_[2][0]/sin(deg2rad($theta_deg)))));
return ($theta_deg,$phi_deg,$psi_deg);
}
I now have a bunch of uninitialized values error in the sub mat2eul. My guess is it is most probably because of the way I am accessing the values from $_. What do you think? |