float Behaviour::calcAngleRef(cv::Point3f p1Ini, cv::Point3f p1End,
cv::Point3f p2Ini, cv::Point3f p2End){
vec3 v1= vec3(p1End.x -p1Ini.x, p1End.y - p1Ini.y, p1End.z - p1Ini.z);
vec3 v2= vec3(p2End.x -p2Ini.x, p2End.y - p2Ini.y, p2End.z - p2Ini.z);
vec3 vRef= vec3(v2);
float angle=signed_angle(v1,v2, vRef)*180 / 3.1415926;//radians to degrees
return angle;
}
float calcAngleRef(vec3 p1Ini,vec3 p1End,
vec3 p2Ini,vec3 p2End) {
vec3 v1 = vec3(p1End.x - p1Ini.x, p1End.y - p1Ini.y, p1End.z - p1Ini.z);
vec3 v2 = vec3(p2End.x - p2Ini.x, p2End.y - p2Ini.y, p2End.z - p2Ini.z);
vec3 vRef = vec3(v2);
float angle = signed_angle(v1, v2, vRef)*180 / 3.1415926; //radians to degrees
return angle;
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
dihedral_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3, Eigen::Matrix<float,3,1> &coor4)
{
float da ;
Eigen::Matrix<float,3,1> r1 ; r1 = coor1 - coor2 ;
Eigen::Matrix<float,3,1> r2 ; r2 = coor3 - coor2 ;
Eigen::Matrix<float,3,1> r3 ; r3 = -1.0*r2 ;
Eigen::Matrix<float,3,1> r4 ; r4 = coor4 - coor3 ;
Eigen::Matrix<float,3,1> n1 ; n1 = r1.cross(r2) ;
Eigen::Matrix<float,3,1> n2 ; n2 = r3.cross(r4) ;
da = signed_angle(n1,n2,r2) ;
return da ;
}
void get_km_phase_space(double* jvec, double* zvec)
{
double p0vec[3];
double p1vec[3];
double p2vec[3];
double p3vec[3];
double avec[3];
double bvec[3];
double tempvec[3];
double temp_norm = 0.0;
double p1 = 0.0, p2 = 0.0;
double q1 = 0.0, q2 = 0.0;
int i;
for(i = 0; i < 3; i++){
p0vec[i] = jvec[i+1];
p1vec[i] = p0vec[i] + jvec[(4+i)];
p2vec[i] = p1vec[i] + jvec[(7+i)];
p3vec[i] = p2vec[i] + jvec[(10+i)];
}
p1 = three_vec_norm(p1vec);
p2 = three_vec_norm(p2vec);
three_vec_cross(p0vec, p1vec, tempvec);
temp_norm = three_vec_norm(tempvec);
three_vec_cross(p0vec, p1vec, avec);
avec[0] = avec[0] / temp_norm;
avec[1] = avec[1] / temp_norm;
avec[2] = avec[2] / temp_norm;
three_vec_cross(p1vec, p2vec, tempvec);
temp_norm = three_vec_norm(tempvec);
three_vec_cross(p1vec, p2vec, bvec);
bvec[0] = bvec[0] / temp_norm;
bvec[1] = bvec[1] / temp_norm;
bvec[2] = bvec[2] / temp_norm;
q1 = signed_angle(avec, bvec, p1vec);
three_vec_cross(p1vec, p2vec, tempvec);
temp_norm = three_vec_norm(tempvec);
three_vec_cross(p1vec, p2vec, avec);
avec[0] = avec[0] / temp_norm;
avec[1] = avec[1] / temp_norm;
avec[2] = avec[2] / temp_norm;
three_vec_cross(p2vec, p3vec, tempvec);
temp_norm = three_vec_norm(tempvec);
three_vec_cross(p2vec, p3vec, bvec);
bvec[0] = bvec[0] / temp_norm;
bvec[1] = bvec[1] / temp_norm;
bvec[2] = bvec[2] / temp_norm;
q2 = signed_angle(avec, bvec, p2vec);
zvec[0] = q1;
zvec[1] = q2;
zvec[2] = p1;
zvec[3] = p2;
}
