void
vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
{
p1Ext = p1;
p2Ext = p2;
vpColVector ABC(3);
vpColVector V1(3);
vpColVector V2(3);
V1[0] = p1.get_oX();
V1[1] = p1.get_oY();
V1[2] = p1.get_oZ();
V2[0] = p2.get_oX();
V2[1] = p2.get_oY();
V2[2] = p2.get_oZ();
// Get the axis of the cylinder
ABC = V1-V2;
// Build our extremity circles
circle1.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p1.get_oX(),p1.get_oY(),p1.get_oZ(),r);
circle2.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p2.get_oX(),p2.get_oY(),p2.get_oZ(),r);
// Build our cylinder
cylinder.setWorldCoordinates(ABC[0],ABC[1],ABC[2],(p1.get_oX()+p2.get_oX())/2.0,(p1.get_oY()+p2.get_oY())/2.0,(p1.get_oZ()+p2.get_oZ())/2.0,r);
}
void txPointsOfCycle::prepSrcData(const QVector<QVector<double> > &srcdata) {
if ( srcdata.isEmpty() ) {
throw txError("Incorrect source data array!");
}
if ( srcdata[0].size() != SRCDATACAPTIONS_11.size() &&
srcdata[0].size() != SRCDATACAPTIONS_6.size() ) {
throw txError("Incorrect source data array!");
}
int currstd = m_calculationOptions->val_standard();
if ( currstd == STD_EU6 || currstd == STD_EU5 ||
currstd == STD_EU4 || currstd == STD_EU3 ) {
m_n_hi = srcdata[0][ 0];
m_n_lo = srcdata[0][ 1];
m_idle = srcdata[0][ 2];
m_n_rated = srcdata[0][ 3];
m_N_fan_rated = srcdata[0][ 4];
m_Ne_A = srcdata[0][ 5];
m_Ne_B = srcdata[0][ 6];
m_Ne_C = srcdata[0][ 7];
m_Ne_a1 = srcdata[0][ 8];
m_Ne_a2 = srcdata[0][ 9];
m_Ne_a3 = srcdata[0][10];
ABC(m_n_hi, m_n_lo, &m_A, &m_B, &m_C, &m_a1, &m_a2, &m_a3, &m_n_ref);
}
else if ( (currstd == STD_EU2) || (currstd == STD_EU1) || (currstd == STD_EU0) ||
(currstd == STD_OST3700123481) || (currstd == STD_GOST17220597) ||
(currstd == STD_R96E8) || (currstd == STD_R96F8) ||
(currstd == STD_R96G8) || (currstd == STD_R96D8) ||
(currstd == STD_R96E5) || (currstd == STD_R96F5) ||
(currstd == STD_R96G5) || (currstd == STD_R96D5) ||
(currstd == STD_R96H8) || (currstd == STD_R96I8) ||
(currstd == STD_R96J8) || (currstd == STD_R96K8) ||
(currstd == STD_R96H5) || (currstd == STD_R96I5) ||
(currstd == STD_R96J5) || (currstd == STD_R96K5) ||
(currstd == STD_C1) || (currstd == STD_D1) || (currstd == STD_D2) ||
(currstd == STD_E1) || (currstd == STD_E2) || (currstd == STD_E3) ||
(currstd == STD_E5) ||
(currstd == STD_F) || (currstd == STD_G1) || (currstd == STD_G2) ) {
m_idle = srcdata[0][ 0];
m_n_interim = srcdata[0][ 1];
m_n_rated = srcdata[0][ 2];
m_N_fan_rated = srcdata[0][ 3];
m_Ne_interim = srcdata[0][ 4];
m_Ne_rated = srcdata[0][ 5];
}
else {
throw txError("Unknown value of parameter \"standard\"!");
}
}
int
main(void)
{
double trailX [X_MAX];
double trailY [Y_MAX];
struct TestPoint testPoints[POINTS_MAX];
//found int traildata.o
init_trail (trailX, trailY);
//from testdata.o
read_points(testPoints, sizeof(testPoints) / sizeof(*testPoints));
//from linestuff.o
// test values
double A = 0.0, B = 0.0, C = 0.0;
ABC(8, -7.2, 8, 19.5, &A, &B, &C);
printf("A == %lf\tB == %lf\tC == %lf\n", A, B, C);
ABC(-2.44, 4.902, 16.3, -187.511, &A, &B, &C);
printf("A == %lf\tB == %lf\tC == %lf\n", A, B, C);
ABC(-42, -19.1, 17.03, -19.1, &A, &B, &C);
printf("A == %lf\tB == %lf\tC == %lf\n", A, B, C);
double x , y, distance;
//test values
ABC(-1, 3, 1, -1, &A, &B, &C);
x = -3;
y = -1;
distance = dist_to_line(A, B, C, x, y);
printf("distance to line segment 1 == %lf\n", distance);
ABC(7.0, -5.0, 6.0, -0.5, &A, &B, &C);
x = 7.0;
y = 5.1;
distance = dist_to_line(A, B, C, x, y);
printf("distance to line segment 2 == %lf\n", distance);
ABC(-7, -5, -7, -1, &A, &B, &C);
x = -3;
y = -1;
distance = dist_to_line(A, B, C, x, y);
printf("distance to line segment 3 == %lf\n", distance);
return 0;
}
// Validation Tests
void validation_tests () {
std::cout << std::endl << "borZoi Validation Tests" << std::endl;
// ***********************************************************
// SHA-1 Validation Tests
// ***********************************************************
std::cout << std::endl << "SHA-1 Validation Tests" << std::endl;
OCTETSTR ABC(3); // Message to be hashed
ABC[0] = 'a'; ABC[1] = 'b'; ABC[2] = 'c';
BigInt Hash = OS2IP (SHA1 (ABC));
BigInt Hash1 = hexto_BigInt("A9993E364706816ABA3E25717850C26C9CD0D89D");
std::cout << "SHA-1 Hash of {\'a\', \'b\', \'c\'}:" << Hash<< std::endl;
if (notValid (Hash == Hash1))
return;
Hash = OS2IP (SHA1 ("abc"));
std::cout << "SHA-1 Hash of \"abc\":" << Hash << std::endl;
if (notValid (Hash == Hash1))
return;
BigInt Hash2 = hexto_BigInt("84983E441C3BD26EBAAE4AA1F95129E5E54670F1");
Hash = OS2IP (SHA1 ("abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"));
std::cout << "SHA-1 Hash of \"abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq\":" << Hash << std::endl;
if (notValid (Hash == Hash2))
return;
OCTETSTR million_as(1000000);
for (unsigned long i=0; i<1000000; i++) {
million_as[i] = 'a';
}
BigInt Hash3 = hexto_BigInt("34AA973CD4C4DAA4F61EEB2BDBAD27316534016F");
Hash = OS2IP (SHA1 (million_as));
std::cout << "SHA-1 Hash of one million \'a\'s:" << Hash << std::endl;
if (notValid (Hash == Hash3))
return;
}
//Computes the ground plane
void GroundPlane::planar_fitting(CCloud &cpcl,
CPhotographs &photos,
float extend_boundary){
std::vector<cv::Point3f> plane_points;
int n_points = 0;
cv::Matx31f ABC, sumRight(0,0,0);
cv::Matx33f sumLeft(0,0,0,
0,0,0,
0,0,0);
//Planar surface approximation ---> output: (A, B, D) from Ax + By + D = 1z
float avgx = 0, avgy = 0, avgz = 0;
for(int i = 0; i < (int) photos.size(); i++){
cv::Matx34f P = photos[i].P;
cv::Matx33f R(P(0,0), P(0,1), P(0,2),
P(1,0), P(1,1), P(1,2),
P(2,0), P(2,1), P(2,2));
cv::Matx31f T(P(0,3),
P(1,3),
P(2,3));
cv::Matx31f C = (-R.t()) * T;
avgx += C(0,0);
avgy += C(1,0);
avgz += C(2,0);
}
avgx /= (int) photos.size();
avgy /= (int) photos.size();
avgz /= (int) photos.size();
for(int i = 0; i < (int) photos.size(); i++){
cv::Matx34f P = photos[i].P;
cv::Matx33f R(P(0,0), P(0,1), P(0,2),
P(1,0), P(1,1), P(1,2),
P(2,0), P(2,1), P(2,2));
cv::Matx31f T(P(0,3),
P(1,3),
P(2,3));
cv::Matx31f Cold = (-R.t()) * T;
cv::Matx31f C = cv::Matx31f(Cold(0,0)-avgx,
Cold(1,0)-avgy,
Cold(2,0)-avgz);
//plane_points.push_back(cv::Point3f(C(0,0),C(1,0), C(2,0)));
n_points++;
sumLeft(0,0) += pow(C(0,0),2.0);
sumLeft(0,1) += C(0,0) * C(2,0);
sumLeft(0,2) += C(0,0);
sumLeft(1,0) += C(0,0) * C(2,0);
sumLeft(1,1) += pow(C(2,0),2.0);
sumLeft(1,2) += C(2,0);
sumLeft(2,0) += C(0,0);
sumLeft(2,1) += C(2,0);
sumLeft(2,2) += 1.0;
sumRight(0,0) += C(0,0) * C(1,0);
sumRight(1,0) += C(2,0) * C(1,0);
sumRight(2,0) += C(1,0);
}
ABC = sumLeft.inv() * sumRight;
plane_normal = cv::Point3f(ABC(0,0),
-1.0,
ABC(1,0)
//(-1.0)
);
cv::Point3f vecx(1,0,0);
float angle = acos(vecx.dot(plane_normal));
if(angle < 0){
plane_normal.x = -plane_normal.x;
plane_normal.y = -plane_normal.y;
plane_normal.z = -plane_normal.z;
}
normalize_vector(plane_normal);
/*float length = sqrt(pow(plane_normal.x,2.0) + pow(plane_normal.y,2.0) + pow(plane_normal.z,2.0));
plane_normal.x /= length;
plane_normal.y /= length;
plane_normal.z /= length;
*/
//Point from plane: p.n = -D
plane_point = cv::Point3f(0.0,
ABC(2,0),
0.0
);
//Search Upper and lower bounds and center of cloud
cv::Point3f cloud_center(0,0,0);
cv::Point3f cloud_upper_bound(-9000,-9000,-9000), cloud_lower_bound(9000,9000,9000);
for(int i = 0; i < (int) cpcl.size(); i++){
cloud_center.x = cloud_center.x + cpcl[i].pos.x;
cloud_center.y = cloud_center.y + cpcl[i].pos.y;
cloud_center.z = cloud_center.z + cpcl[i].pos.z;
if(cpcl[i].pos.x < cloud_lower_bound.x)
cloud_lower_bound.x = cpcl[i].pos.x;
if(cpcl[i].pos.y < cloud_lower_bound.y)
cloud_lower_bound.y = cpcl[i].pos.y;
if(cpcl[i].pos.z < cloud_lower_bound.z)
cloud_lower_bound.z = cpcl[i].pos.z;
if(cpcl[i].pos.x > cloud_upper_bound.x)
cloud_upper_bound.x = cpcl[i].pos.x;
if(cpcl[i].pos.y > cloud_upper_bound.y)
cloud_upper_bound.y = cpcl[i].pos.y;
if(cpcl[i].pos.z > cloud_upper_bound.z)
cloud_upper_bound.z = cpcl[i].pos.z;
}
cloud_center.x /= cpcl.size();
cloud_center.y /= cpcl.size();
cloud_center.z /= cpcl.size();
//Streatch boundaries
cloud_lower_bound.x -= extend_boundary;
cloud_lower_bound.y -= extend_boundary;
cloud_lower_bound.z -= extend_boundary;
cloud_upper_bound.x += extend_boundary;
cloud_upper_bound.y += extend_boundary;
cloud_upper_bound.z += extend_boundary;
//Project both boudaries and center to the computed plane
cv::Point3f v = cloud_center - plane_point;
float distance = v.dot(plane_normal);
cloud_center = cloud_center - (distance * plane_normal);
v = cloud_lower_bound - plane_point;
distance = v.dot(plane_normal);
lower_bound = cloud_lower_bound - distance * plane_normal;
v = cloud_upper_bound - plane_point;
distance = v.dot(plane_normal);
higher_bound = cloud_upper_bound - distance * plane_normal;
}
static const scm_t_uint32 subr_stub_code[] = {
/* C-u 1 0 M-x generate-bytecodes RET */
/* 0 arguments */
A(0),
/* 1 arguments */
A(1), B(1),
C(),
/* 2 arguments */
A(2), AB(1,1), B(2),
AC(1), BC(1),
/* 3 arguments */
A(3), AB(2,1), AB(1,2), B(3),
AC(2), ABC(1,1), BC(2),
/* 4 arguments */
A(4), AB(3,1), AB(2,2), AB(1,3), B(4),
AC(3), ABC(2,1), ABC(1,2), BC(3),
/* 5 arguments */
A(5), AB(4,1), AB(3,2), AB(2,3), AB(1,4), B(5),
AC(4), ABC(3,1), ABC(2,2), ABC(1,3), BC(4),
/* 6 arguments */
A(6), AB(5,1), AB(4,2), AB(3,3), AB(2,4), AB(1,5), B(6),
AC(5), ABC(4,1), ABC(3,2), ABC(2,3), ABC(1,4), BC(5),
/* 7 arguments */
A(7), AB(6,1), AB(5,2), AB(4,3), AB(3,4), AB(2,5), AB(1,6), B(7),
