/* generate random float signal */ static void genRandFloats( float *buf, size_t numSamples) { unsigned bufSize = RRAND_BUFSIZE; if(bufSize > numSamples) { bufSize = numSamples; } /* fill up our buffer */ float *inBuf = (float *)malloc(bufSize * sizeof(float)); float *inp = inBuf; for(size_t dex=0; dex<bufSize; dex++) { *inp++ = getRandomDouble(); } /* copy to caller's buffer */ float *outp = buf; inp = inBuf; float *inEnd = inBuf + bufSize; for(unsigned dex=0; dex<numSamples; dex++) { *outp++ = *inp++; if(inp == inEnd) { inp = inBuf; } } free(inBuf); }
vector<double> generateTestData() { //Generates a data point either in the range (0.75,0.75) -> (1,1) or (0,0) -> (.25,.25) double g = getRandomDouble(0, 1); double lb; double ub; if (g < 0.5) { lb = 0.75; ub = 1.00; } else { lb = 0.00; ub = 0.25; } vector<double> generatedVector = { getRandomDouble(lb, ub), getRandomDouble(lb, ub) }; generatedVector.shrink_to_fit(); return generatedVector; }
void publishNewGoal() { double x; double y; // Check if the point is unoccupied bool collision = true; while (collision) { collision = false; x = getRandomDouble(x_min, x_max, 2.00); y = getRandomDouble(y_min, y_max, 2.00); for (long unsigned i = 0; i < robot_poses.size(); i++) { pixel_position = (unsigned int)(x / 0.05) + (unsigned int)(y / 0.05) * 200; // First check for the costmap values if (costmap_there && current_costmap.data.at(pixel_position) > 10) { collision = true; } // Now check for dynamic obstacles if (dist(x, y, robot_poses.at(i).pose.pose.position.x, robot_poses.at(i).pose.pose.position.y) < 0.8) { collision = true; } } } geometry_msgs::PoseStamped pose_stamped; pose_stamped.pose.position.z = 0.0; pose_stamped.pose.position.x = x; pose_stamped.pose.position.y = y; pose_stamped.pose.orientation.z = 1.0; pose_stamped.pose.orientation.w = o; pose_stamped.header.frame_id = "map"; move_base_goal_pub.publish(pose_stamped); }
int main(void) { int i; for(i=0; i<20; i++) { printf("Die loesung ist %d\n", getRandom(12,14)); } for(i=0; i<20; i++) { printf("Die loesung ist %f\n", getRandomDouble(1.1,2.3)); } system("PAUSE"); }
static Vector4<double> getRandomVector4(double range) { return Vector4<double>(getRandomDouble(range), getRandomDouble(range), getRandomDouble(range)); }
/** * Generates a random number (float) between the minimum and * maximum range. * * @param min - minimum * @param max - maximum */ float FaultInjector::getRandomFloat(float min, float max) { int rd = (int) getRandomDouble((int) (min * 10), (int) (max * 10) + 1); return (float) rd / 10; }
int testTriangle() { int numErr = 0; logMessage(_T("TESTING - class GM_3dTriangle ...\n\n")); // Default constructor, triangle must be invalid GM_3dTriangle tr; if (tr.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid triangle\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid triangle\n")); } // Get/Set GM_3dPoint v1(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint v2(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint v3(getRandomDouble(), getRandomDouble(), getRandomDouble()); tr[0] = GM_3dLine(v1, v2); tr[1] = GM_3dLine(v2, v3); tr[2] = GM_3dLine(v3, v1); if (!tr.isValid() || tr[0].begin() != v1 || tr[0].end() != v2 || tr[1].begin() != v2 || tr[1].end() != v3 || tr[2].begin() != v3 || tr[2].end() != v1) { logMessage(_T("\tERROR - Get/Set not working\n")); numErr++; } else { logMessage(_T("\tOK - Get/Set working\n")); } // Copy constructor GM_3dTriangle tr1(tr); if (!tr1.isValid() || tr[0] != tr1[0] || tr[1] != tr1[1] || tr[2] != tr1[2]) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Constructor (points) GM_3dTriangle tr2(v1, v2, v3); if (!tr1.isValid() || tr[0] != tr2[0] || tr[1] != tr2[1] || tr[2] != tr2[2]) { logMessage(_T("\tERROR - Constructor (points) not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor (points) working\n")); } // Constructor (lines) GM_3dTriangle tr3(tr[0], tr[1], tr[2]); if (!tr1.isValid() || tr[0] != tr3[0] || tr[1] != tr3[1] || tr[2] != tr3[2]) { logMessage(_T("\tERROR - Constructor (lines) not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor (lines) working\n")); } // Inversion tr1.invert(); if (!tr1.isValid() || tr1[0].begin() != tr[0].end() || tr1[0].end() != tr[0].begin() || tr1[1].begin() != tr[1].end() || tr1[1].end() != tr[1].begin() || tr1[2].begin() != tr[2].end() || tr1[2].end() != tr[2].begin()) { logMessage(_T("\tERROR - Triangle inversion not working\n")); numErr++; } else { logMessage(_T("\tOK - Triangle inversion working\n")); } // Vertical check GM_3dPoint vPoint = GM_3dLine(v1, v2).center(); vPoint.z(vPoint.z() + getRandomDouble()); GM_3dTriangle VTr(v1, v2, vPoint); if (!VTr.isValid() || !VTr.isVertical()) { logMessage(_T("\tERROR - Vertical check not working\n")); numErr++; } else { logMessage(_T("\tOK - Vertical check working\n")); } // Horizontal check GM_3dTriangle HTr(tr); double z = getRandomDouble(); for (unsigned int i = 0 ; i < 3 ; i++) { HTr[i].begin().z(z); HTr[i].end().z(z); } if (!HTr.isValid() || !HTr.isHorizontal()) { logMessage(_T("\tERROR - Horizontal check not working\n")); numErr++; } else { logMessage(_T("\tOK - Horizontal check working\n")); } // Connection check GM_3dTriangle discTr(tr); discTr[0].begin().x(discTr[0].begin().x() + 2.0 * GM_DIFF_TOLERANCE); if (!discTr.isValid() || discTr.isConnected() || !tr.isConnected()) { logMessage(_T("\tERROR - Connection check not working\n")); numErr++; } else { logMessage(_T("\tOK - Connection check working\n")); } // Max/Min z double maxZ = tr.maxZ(); double minZ = tr.minZ(); double checkMaxZ = -DBL_MAX; double checkMinZ = DBL_MAX; for (unsigned int i = 0 ; i < 3 ; i++) { if (tr[i].begin().z() > checkMaxZ) checkMaxZ = tr[i].begin().z(); if (tr[i].begin().z() < checkMinZ) checkMinZ = tr[i].begin().z(); if (tr[i].end().z() > checkMaxZ) checkMaxZ = tr[i].end().z(); if (tr[i].end().z() < checkMinZ) checkMinZ = tr[i].end().z(); } if (checkMinZ != minZ || checkMaxZ != maxZ) { logMessage(_T("\tERROR - Min/Max z not working\n")); numErr++; } else { logMessage(_T("\tOK - Min/Max z working\n")); } return numErr; }
int testBasis() { int numErr = 0; logMessage(_T("TESTING - class GM_3dBasis ...\n\n")); // Default constructor, basis must be invalid GM_3dBasis b; if (b.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid basis\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid basis\n")); } // Constructor from three vectors GM_3dVector v1(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v2(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v3(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dBasis b1(v1, v2, v3); if (!b1.isValid()) { logMessage(_T("\tERROR - Constructor from three vectors not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor from three vectors working\n")); } // Copy constructor GM_3dBasis bCopy(b1); if (!bCopy.isValid() || bCopy[0] != b1[0] || bCopy[1] != b1[1] || bCopy[2] != b1[2]) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Linear independency check GM_3dBasis bIndep(GM_3dVector(getRandomDouble(), 0.0, 0.0), GM_3dVector(0.0, getRandomDouble(), 0.0), GM_3dVector(0.0, 0.0, getRandomDouble())); GM_3dBasis bDep(v1, v2, v1*getRandomDouble() + v2*getRandomDouble()); double det = bIndep[0].x() * bIndep[1].y() * bIndep[2].z(); if (!bDep.isValid() || !bIndep.isValid() || bDep.isLinearlyInd() || !bIndep.isLinearlyInd()) { logMessage(_T("\tERROR - Linear independency check not working\n")); numErr++; } else { logMessage(_T("\tOK - Linear independency check working\n")); } return numErr; }
int testPlane() { int numErr = 0; logMessage(_T("TESTING - class GM_3dPlane ...\n\n")); // Default constructor, plane must be invalid GM_3dPlane p; if (p.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid plane\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid plane\n")); } // Constructor (coeff) GM_3dPlane p1(getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPlane pNull(0.0, 0.0, 0.0, getRandomDouble()); if (!p1.isValid() || pNull.isValid()) { logMessage(_T("\tERROR - Coeff. constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Coeff. constructor working\n")); } // Copy constructor GM_3dPlane copyPlane(p1); if (!copyPlane.isValid() || copyPlane != p1) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Constructor (coeff vector) double pointsVect[4]; for (int i = 0 ; i < 4 ; i++) { pointsVect[i] = getRandomDouble(); } GM_3dPlane p2(pointsVect); if (!p2.isValid()) { logMessage(_T("\tERROR - Coeff. vector constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Coeff. vector constructor working\n")); } // Constructor (points) GM_3dPoint pt1(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint pt2(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint pt3(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dLine ln(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint ptLn1 = ln.begin(); GM_3dPoint ptLn2 = ln.center(); GM_3dPoint ptLn3 = ln.end(); GM_3dPlane p3(pt1, pt2, pt3); GM_3dPlane pLine(ptLn2, ptLn2, ptLn3); double distPt1 = pt1.x()*p3[0] + pt1.y()*p3[1] + pt1.z()*p3[2] + p3[3]; double distPt2 = pt2.x()*p3[0] + pt2.y()*p3[1] + pt2.z()*p3[2] + p3[3]; double distPt3 = pt3.x()*p3[0] + pt3.y()*p3[1] + pt3.z()*p3[2] + p3[3]; if (!p3.isValid() || pLine.isValid() || fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE || fabs(distPt3) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Points constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Points constructor working\n")); } // Point distance GM_3dPlane p4(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble()); GM_3dPoint checkPoint(getRandomDouble(), getRandomDouble(), getRandomDouble()); if (fabs(p4[0]) > GM_NULL_TOLERANCE) { checkPoint.x(-(checkPoint.y()*p4[1] + checkPoint.z()*p4[2] + p4[3]) / p4[0]); } else if (fabs(p4[1]) > GM_NULL_TOLERANCE) { checkPoint.y(-(checkPoint.x()*p4[0] + checkPoint.z()*p4[2] + p4[3]) / p4[1]); } else if (fabs(p4[2]) > GM_NULL_TOLERANCE) { checkPoint.z(-(checkPoint.x()*p4[0] + checkPoint.y()*p4[1] + p4[3]) / p4[2]); } else { p4.invalidate(); } double checkSignedDist = getRandomDouble(); double checkDist = fabs(checkSignedDist); GM_3dVector p4Norm = p4.normalVector(); checkPoint = (GM_3dVector)checkPoint + (p4Norm * checkSignedDist); GM_3dPoint checkPointOnPlane1; GM_3dPoint checkPointOnPlane2; double dist = p4.pointDistance(checkPoint); double signedDist = p4.pointDistanceSgn(checkPoint); double signedDistOnPlane = p4.pointDistanceSgn(checkPoint, checkPointOnPlane1); double distOnPlane = p4.pointDistance(checkPoint, checkPointOnPlane2); distPt1 = checkPointOnPlane1.x()*p4[0] + checkPointOnPlane1.y()*p4[1] + checkPointOnPlane1.z()*p4[2] + p4[3]; distPt2 = checkPointOnPlane2.x()*p4[0] + checkPointOnPlane2.y()*p4[1] + checkPointOnPlane2.z()*p4[2] + p4[3]; if (!p4.isValid() || !checkPointOnPlane1.isValid() || !checkPointOnPlane2.isValid() || fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE || fabs(distOnPlane - checkDist) > GM_NULL_TOLERANCE || fabs(signedDistOnPlane - checkSignedDist) > GM_NULL_TOLERANCE || fabs(dist - checkDist) > GM_NULL_TOLERANCE || fabs(signedDist - checkSignedDist) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Point distance computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Point distance computation working\n")); } // XY Angle double angle = ((((double)rand()) / ((double)RAND_MAX)) * GM_PI) - (GM_PI / 2.0); GM_3dVector normVector(angle/* + GM_HALFPI*/); normVector.z(normVector.y()); normVector.y(0.0); GM_3dPlane angleP(normVector, GM_3dPoint(getRandomDouble(), getRandomDouble(), getRandomDouble())); double checkAngle = angleP.xyAngle(); if (checkAngle > GM_PI) { checkAngle -= 2.0 * GM_PI; } if (!angleP.isValid() || fabs(angle - checkAngle) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - XY angle computation not working\n")); numErr++; } else { logMessage(_T("\tOK - XY angle computation working\n")); } return numErr; }
int testPoint() { int numErr = 0; logMessage(_T("TESTING - class GM_3dPoint ...\n\n")); // Default constructor, point must be invalid GM_3dPoint pt; if (pt.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid points\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid points\n")); } // Get/Set points coordinates double x = getRandomDouble(); double y = getRandomDouble(); double z = getRandomDouble(); pt.x(x); pt.y(y); pt.z(z); if (pt.x() != x || pt.y() != y || pt.z() != z) { logMessage(_T("\tERROR - Get/Set not working\n")); numErr++; } else { logMessage(_T("\tOK - Get/Set working\n")); } // Copy constructor GM_3dPoint pt1(pt); if (pt.isValid() != pt1.isValid() || pt1.x() != pt.x() || pt1.y() != pt.y() || pt1.z() != pt.z()) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Constructor (coordinates) x = getRandomDouble(); y = getRandomDouble(); z = getRandomDouble(); GM_3dPoint pt2(x, y, z); if (!pt2.isValid() || pt2.x() != x || pt2.y() != y || pt2.z() != z) { logMessage(_T("\tERROR - Coordinate constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Coordinate constructor working\n")); } // Constructor (angle) double ang = getRandomAngle(); GM_3dPoint pt3(ang); if (!pt2.isValid() || pt3.x() != cos(ang) || pt3.y() != sin(ang) || pt3.z() != 0.0) { logMessage(_T("\tERROR - XY angle constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - XY angle constructor working\n")); } // Invalidation pt.invalidate(); if (pt.isValid()) { logMessage(_T("\tERROR - Invalidation not working\n")); numErr++; } else { logMessage(_T("\tOK - Invalidation working\n")); } // Inversion GM_3dPoint invPt(pt2); invPt.xyInvert(); if (invPt.isValid() != pt2.isValid() || invPt.x() != -pt2.x() || invPt.y() != -pt2.y() || invPt.z() != pt2.z()) { logMessage(_T("\tERROR - Inversion not working\n")); numErr++; } else { logMessage(_T("\tOK - Inversion working\n")); } // Origin check GM_3dPoint origPt(GM_NULL_TOLERANCE / 2.0, -GM_NULL_TOLERANCE / 2.0, 0.0); if (pt2.isOrigin() || !origPt.isOrigin()) { logMessage(_T("\tERROR - Origin check not working\n")); numErr++; } else { logMessage(_T("\tOK - Origin check working\n")); } // Distance GM_3dPoint startPt(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint endPt(getRandomDouble(), getRandomDouble(), getRandomDouble()); double dx = startPt.x() - endPt.x(); double dy = startPt.y() - endPt.y(); double dz = startPt.z() - endPt.z(); double dist = sqrt(dx*dx + dy*dy + dz*dz); if (!startPt.isValid() || !endPt.isValid() || startPt.distFrom(endPt) != dist) { logMessage(_T("\tERROR - Distance computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Distance computation working\n")); } // Equality GM_3dPoint ptEq1(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint ptEq2(ptEq1); GM_3dPoint ptEq3(ptEq1.x() + getRandomDouble(), ptEq1.y() + getRandomDouble(), ptEq1.z() + getRandomDouble()); if (ptEq1 != ptEq2 || ptEq1 == ptEq3) { logMessage(_T("\tERROR - Equality check not working\n")); numErr++; } else { logMessage(_T("\tOK - Equality check working\n")); } return numErr; }
int testLine() { int numErr = 0; logMessage(_T("TESTING - class GM_3dLine ...\n\n")); // Default constructor, line must be invalid GM_3dLine ln; if (ln.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid line\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid line\n")); } // Get/Set line coordinates double xStart = getRandomDouble(); double yStart = getRandomDouble(); double zStart = getRandomDouble(); double xEnd = getRandomDouble(); double yEnd = getRandomDouble(); double zEnd = getRandomDouble(); ln.begin().x(xStart); ln.begin().y(yStart); ln.begin().z(zStart); ln.end().x(xEnd); ln.end().y(yEnd); ln.end().z(zEnd); if (!ln.isValid() || ln.begin().x() != xStart || ln.begin().y() != yStart || ln.begin().z() != zStart || ln.end().x() != xEnd || ln.end().y() != yEnd || ln.end().z() != zEnd) { logMessage(_T("\tERROR - Get/Set not working\n")); numErr++; } else { logMessage(_T("\tOK - Get/Set working\n")); } // Copy constructor GM_3dLine ln1(ln); if (ln1.isValid() != ln.isValid() || ln1.begin().x() != ln.begin().x() || ln1.begin().y() != ln.begin().y() || ln1.begin().z() != ln.begin().z() || ln1.end().x() != ln.end().x() || ln1.end().y() != ln.end().y() || ln1.end().z() != ln.end().z()) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Constructor (points) GM_3dPoint startPt(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dPoint endPt(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dLine ln2(startPt, endPt); if (!ln2.isValid() || ln2.begin().x() != startPt.x() || ln2.begin().y() != startPt.y() || ln2.begin().z() != startPt.z() || ln2.end().x() != endPt.x() || ln2.end().y() != endPt.y() || ln2.end().z() != endPt.z()) { logMessage(_T("\tERROR - Points constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Points constructor working\n")); } // Constructor (coordinates) xStart = getRandomDouble(); yStart = getRandomDouble(); zStart = getRandomDouble(); xEnd = getRandomDouble(); yEnd = getRandomDouble(); zEnd = getRandomDouble(); GM_3dLine ln3(xStart, yStart, zStart, xEnd, yEnd, zEnd); if (!ln3.isValid() || ln3.begin().x() != xStart || ln3.begin().y() != yStart || ln3.begin().z() != zStart || ln3.end().x() != xEnd || ln3.end().y() != yEnd || ln3.end().z() != zEnd) { logMessage(_T("\tERROR - Coordinate constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Coordinate constructor working\n")); } // dx, dy, dz double dx = ln2.dx(); double dy = ln2.dy(); double dz = ln2.dz(); if (dx != ln2.end().x()-ln2.begin().x() || dy != ln2.end().y()-ln2.begin().y() || dz != ln2.end().z()-ln2.begin().z()) { logMessage(_T("\tERROR - dx, dy or dz not working\n")); numErr++; } else { logMessage(_T("\tOK - dx, dy or dz working\n")); } // Dot product double dotProduct = ln2 * ln3; double dxLn2 = ln2.dx(); double dyLn2 = ln2.dy(); double dzLn2 = ln2.dz(); double dxLn3 = ln3.dx(); double dyLn3 = ln3.dy(); double dzLn3 = ln3.dz(); double checkDotProduct = dxLn2*dxLn3 + dyLn2*dyLn3 + dzLn2*dzLn3; if (dotProduct != checkDotProduct) { logMessage(_T("\tERROR - Dot product not working\n")); numErr++; } else { logMessage(_T("\tOK - Dot product working\n")); } // Cross product GM_3dLine crossProdLn = ln2 ^ ln3; double checkCrossProdX = ln2.begin().x() + (dyLn2*dzLn3 - dzLn2*dyLn3); double checkCrossProdY = ln2.begin().y() + (dzLn2*dxLn3 - dxLn2*dzLn3); double checkCrossProdZ = ln2.begin().z() + (dxLn2*dyLn3 - dyLn2*dxLn3); if (!crossProdLn.isValid() || crossProdLn.end().x() != checkCrossProdX || crossProdLn.end().y() != checkCrossProdY || crossProdLn.end().z() != checkCrossProdZ) { logMessage(_T("\tERROR - Cross product not working\n")); numErr++; } else { logMessage(_T("\tOK - Cross product working\n")); } // Center GM_3dPoint ln2Center = ln2.center(); if (!ln2Center.isValid() || ln2Center.x() != ln2.begin().x() + (ln2.end().x()-ln2.begin().x())/2.0 || ln2Center.y() != ln2.begin().y() + (ln2.end().y()-ln2.begin().y())/2.0 || ln2Center.z() != ln2.begin().z() + (ln2.end().z()-ln2.begin().z())/2.0) { logMessage(_T("\tERROR - Center point computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Center point computation working\n")); } // Invert GM_3dLine ln2Copy(ln2); ln2.invert(); if (!ln2Copy.isValid() || ln2Copy.begin().x() != ln2.end().x() || ln2Copy.begin().y() != ln2.end().y() || ln2Copy.begin().z() != ln2.end().z() || ln2Copy.end().x() != ln2.begin().x() || ln2Copy.end().y() != ln2.begin().y() || ln2Copy.end().z() != ln2.begin().z()) { logMessage(_T("\tERROR - Inversion not working\n")); numErr++; } else { logMessage(_T("\tOK - Inversion working\n")); } // Length double checkLen = sqrt(ln2.dx()*ln2.dx() + ln2.dy()*ln2.dy() + ln2.dz()*ln2.dz()); if (ln2.length() != checkLen) { logMessage(_T("\tERROR - Length computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Length computation working\n")); } // Null check GM_3dLine nullLn(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); bool checkNull1 = nullLn.isNull(); nullLn.begin().x(10.0); bool checkNull2 = nullLn.isNull(); if (!nullLn.isValid() || !checkNull1 || checkNull2) { logMessage(_T("\tERROR - Null line check not working\n")); numErr++; } else { logMessage(_T("\tOK - Null line check working\n")); } // Vertical line check double xCoord = getRandomDouble(); double yCoord = getRandomDouble(); GM_3dLine vertLn(xCoord, yCoord, getRandomDouble(), xCoord, yCoord, getRandomDouble()); bool checkVert1 = vertLn.isVertical(); vertLn.begin().x(vertLn.begin().x() + 10.0); bool checkVert2 = vertLn.isVertical(); if (!vertLn.isValid() || !checkVert1 || checkVert2) { logMessage(_T("\tERROR - Vertical line check not working\n")); numErr++; } else { logMessage(_T("\tOK - Vertical line check working\n")); } // Horizontal line check double zCoord = getRandomDouble(); GM_3dLine horLn(getRandomDouble(), getRandomDouble(), zCoord, getRandomDouble(), getRandomDouble(), zCoord); bool checkHor1 = horLn.isHorizontal(); horLn.begin().z(horLn.begin().z() + 10.0); bool checkHor2 = horLn.isHorizontal(); if (!horLn.isValid() || !checkHor1 || checkVert2) { logMessage(_T("\tERROR - Horizontal line check not working\n")); numErr++; } else { logMessage(_T("\tOK - Horizontal line check working\n")); } // Z min/max double minZ = ln2.minZ(); double maxZ = ln2.maxZ(); double checkMin = ln2.begin().z() < ln2.end().z() ? ln2.begin().z() : ln2.end().z(); double checkMax = ln2.begin().z() > ln2.end().z() ? ln2.begin().z() : ln2.end().z(); if (minZ != checkMin || maxZ != checkMax) { logMessage(_T("\tERROR - min/max Z not working\n")); numErr++; } else { logMessage(_T("\tOK - min/max Z working\n")); } // Point on line at Z double sec = getRandom01Double(); double dX = ln2.dx(); double dY = ln2.dy(); double dZ = ln2.dz(); double xCheck = ln2.begin().x() + (sec * dX); double yCheck = ln2.begin().y() + (sec * dY); double zCheck = ln2.begin().z() + (sec * dZ); GM_3dPoint pointAtZ = ln2.pointAtZ(zCheck); if (!pointAtZ.isValid() || fabs(pointAtZ.x()-xCheck) > GM_NULL_TOLERANCE || fabs(pointAtZ.y()-yCheck) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Point on line at Z not working\n")); numErr++; } else { logMessage(_T("\tOK - Point on line at Z working\n")); } // Nomal vector on XY GM_3dVector normalVectorR = ln2.normalXYVector(true); GM_3dVector normalVectorL = ln2.normalXYVector(false); GM_3dVector ln2Dir(ln2); double dotPR = normalVectorR * ln2Dir; double dotPL = normalVectorL * ln2Dir; bool RSide = ln2Dir.isAtLeftOnXY(normalVectorR); bool LSide = ln2Dir.isAtLeftOnXY(normalVectorL); if (!normalVectorR.isValid() || !normalVectorL.isValid() || fabs(dotPR) > GM_NULL_TOLERANCE || fabs(dotPL) > GM_NULL_TOLERANCE || !RSide || LSide) { logMessage(_T("\tERROR - XY normal vector computation not working\n")); numErr++; } else { logMessage(_T("\tOK - XY normal vector computation working\n")); } // Point from section double section = getRandom01Double(); GM_3dPoint sectionPoint = ln2.pointFromSection(section); double ln2Len = ln2.length(); double pointSectionLen = ln2.begin().distFrom(sectionPoint); if (fabs((pointSectionLen / ln2Len) - section) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Point from section not working\n")); numErr++; } else { logMessage(_T("\tOK - Point from section working\n")); } // Section from point double checkSection = ln2.sectionFromPoint(sectionPoint); if (fabs(checkSection - section) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Section from point not working\n")); numErr++; } else { logMessage(_T("\tOK - Section from point working\n")); } // Point distance ln2Dir.normalize(); GM_3dVector ln2NormDir(ln2Dir); section = getRandom01Double(); sectionPoint = ln2.pointFromSection(section); if (fabs(ln2Dir.x()) > GM_NULL_TOLERANCE) { ln2NormDir.x(getRandomDouble()); ln2NormDir.y(getRandomDouble()); ln2NormDir.z(getRandomDouble()); ln2NormDir.normalize(); ln2NormDir.x(-(ln2Dir.y()*ln2NormDir.y() + ln2Dir.z()*ln2NormDir.z()) / ln2Dir.x()); } else if (fabs(ln2Dir.y()) > GM_NULL_TOLERANCE) { ln2NormDir.x(getRandomDouble()); ln2NormDir.y(getRandomDouble()); ln2NormDir.z(getRandomDouble()); ln2NormDir.normalize(); ln2NormDir.y(-(ln2Dir.x()*ln2NormDir.x() + ln2Dir.z()*ln2NormDir.z()) / ln2Dir.y()); } else if (fabs(ln2Dir.z()) > GM_NULL_TOLERANCE) { ln2NormDir.x(getRandomDouble()); ln2NormDir.y(getRandomDouble()); ln2NormDir.z(getRandomDouble()); ln2NormDir.normalize(); ln2NormDir.z(-(ln2Dir.x()*ln2NormDir.x() + ln2Dir.y()*ln2NormDir.y()) / ln2Dir.z()); } else { ln2NormDir.invalidate(); } double dist = 0.0; double checkDist = 0.0; GM_3dPoint pointOnLine; if (ln2NormDir.isValid()) { ln2NormDir.normalize(); double dotProd = ln2NormDir * ln2Dir; dist = fabs(getRandomDouble()); GM_3dPoint externPoint(sectionPoint); externPoint = (GM_3dVector)externPoint + (ln2NormDir * dist); checkDist = ln2.pointDistance(externPoint, pointOnLine); } if (!pointOnLine.isValid() || fabs(dist - checkDist) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Point distance not working\n")); numErr++; } else { logMessage(_T("\tOK - Point distance working\n")); } return numErr; }
int testVector() { int numErr = 0; logMessage(_T("TESTING - class GM_3dVector ...\n\n")); // Default constructor, vector must be invalid GM_3dVector v; if (v.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid vector\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid vector\n")); } // Get/Set vector coordinates double x = getRandomDouble(); double y = getRandomDouble(); double z = getRandomDouble(); v.x(x); v.y(y); v.z(z); if (!v.isValid() || v.x() != x || v.y() != y || v.z() != z) { logMessage(_T("\tERROR - Get/Set not working\n")); numErr++; } else { logMessage(_T("\tOK - Get/Set working\n")); } // Copy constructor GM_3dVector v1(v); if (v.isValid() != v1.isValid() || v1.x() != v.x() || v1.y() != v.y() || v1.z() != v.z()) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Constructor (point) GM_3dPoint pt(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v2(pt); if (!v2.isValid() || v2.x() != pt.x() || v2.y() != pt.y() || v2.z() != pt.z()) { logMessage(_T("\tERROR - Constructor from point not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor from point working\n")); } // Constructor (line) GM_3dLine line(getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v3(line); if (!v3.isValid() || v3.x() != line.end().x() - line.begin().x() || v3.y() != line.end().y() - line.begin().y() || v3.z() != line.end().z() - line.begin().z()) { logMessage(_T("\tERROR - Constructor from line not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor from line working\n")); } // Constructor (angle) double ang = getRandomAngle(); GM_3dVector v4(ang); if (!v4.isValid() || v4.x() != cos(ang) || v4.y() != sin(ang) || v4.z() != 0.0) { logMessage(_T("\tERROR - XY angle constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - XY angle constructor working\n")); } // Module double module = v.mod(); if (module != sqrt(v.x()*v.x() + v.y()*v.y() + v.z()*v.z())) { logMessage(_T("\tERROR - Module computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Module computation working\n")); } // Normalization v.normalize(); if (fabs(v.mod()-1.0) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Normalization not working\n")); numErr++; } else { logMessage(_T("\tOK - Normalization working\n")); } // xy Angle double checkAng = v4.xyAngle(); if (checkAng > GM_PI) { checkAng -= 2.0 * GM_PI; } if (fabs(checkAng - ang) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - xy angle computation not working\n")); numErr++; } else { logMessage(_T("\tOK - xy angle computation working\n")); } // Angle between vectors ang = getRandomAngle(); GM_3dVector v5(ang); GM_3dVector baseVect(0.0); checkAng = baseVect.xyAngle(v5); if (checkAng > GM_PI) { checkAng -= 2.0 * GM_PI; } double checkAng1 = v5.xyAngle(baseVect); if (checkAng1 > GM_PI) { checkAng1 -= 2.0 * GM_PI; } if (fabs(checkAng - ang) > GM_NULL_TOLERANCE || fabs(-checkAng1 - ang) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Angle between vectors computation not working\n")); numErr++; } else { logMessage(_T("\tOK - Angle between vectors computation working\n")); } // At left ang = getRandomAngle(); if (ang < 0.0) { ang = 2.0 * GM_PI + ang; } double ang1 = getRandomAngle(); if (ang1 < 0.0) { ang1 = 2.0 * GM_PI + ang1; } GM_3dVector v6(ang); GM_3dVector v7(ang1); bool v6AtLeftv7 = v6.isAtLeftOnXY(v7); bool v7AtLeftv6 = v7.isAtLeftOnXY(v6); double checkv6Ang = v6.xyAngle(); double checkv7Ang = v7.xyAngle(); bool checkv6AtLeftv7 = ang > ang1 ? true : false; if (fabs(ang - checkv6Ang) > GM_NULL_TOLERANCE || fabs(ang1 - checkv7Ang) > GM_NULL_TOLERANCE || checkv6AtLeftv7 != v6AtLeftv7 || v6AtLeftv7 == v7AtLeftv6) { logMessage(_T("\tERROR - At left not working\n")); numErr++; } else { logMessage(_T("\tOK - At left working\n")); } // Dot product GM_3dVector v8(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v9(getRandomDouble(), getRandomDouble(), getRandomDouble()); double dotProd = v8 * v9; double dotProd1 = v9 * v8; double checkDotProd = v8.x()*v9.x() + v8.y()*v9.y() + v8.z()*v9.z(); if (dotProd != checkDotProd || dotProd1 != checkDotProd) { logMessage(_T("\tERROR - Dot product not working\n")); numErr++; } else { logMessage(_T("\tOK - Dot product working\n")); } // Cross product GM_3dVector crossProd = v8 ^ v9; GM_3dVector crossProd1 = v9 ^ v8; double checkCrossProdX = v8.y()*v9.z() - v8.z()*v9.y(); double checkCrossProdY = v8.z()*v9.x() - v8.x()*v9.z(); double checkCrossProdZ = v8.x()*v9.y() - v8.y()*v9.x(); if (crossProd.x() != checkCrossProdX || crossProd.y() != checkCrossProdY || crossProd.z() != checkCrossProdZ || crossProd1.x() != -checkCrossProdX || crossProd1.y() != -checkCrossProdY || crossProd1.z() != -checkCrossProdZ) { logMessage(_T("\tERROR - Cross product not working\n")); numErr++; } else { logMessage(_T("\tOK - Cross product working\n")); } // Scale double factor = getRandomDouble(); GM_3dVector v8Scaled = v8 * factor; if (v8Scaled.x() != v8.x()*factor || v8Scaled.y() != v8.y()*factor || v8Scaled.z() != v8.z()*factor) { logMessage(_T("\tERROR - Scaling not working\n")); numErr++; } else { logMessage(_T("\tOK - Scaling working\n")); } // Sum GM_3dVector sum = v8 + v9; if (sum.x() != v8.x()+v9.x() || sum.y() != v8.y()+v9.y() || sum.z() != v8.z()+v9.z()) { logMessage(_T("\tERROR - Sum not working\n")); numErr++; } else { logMessage(_T("\tOK - Sum working\n")); } // Difference GM_3dVector diff = v8 - v9; if (diff.x() != v8.x()-v9.x() || diff.y() != v8.y()-v9.y() || diff.z() != v8.z()-v9.z()) { logMessage(_T("\tERROR - Difference not working\n")); numErr++; } else { logMessage(_T("\tOK - Difference working\n")); } return numErr; }
int testMatrix() { int numErr = 0; logMessage(_T("TESTING - class GM_Matrix ...\n\n")); // Default constructor, matrix must be invalid GM_Matrix m; if (m.isValid()) { logMessage(_T("\tERROR - Default constructor creates valid matrix\n")); numErr++; } else { logMessage(_T("\tOK - Default constructor creates invalid matrix\n")); } // Constructor of a non square matrix GM_Matrix mQuad(3, 5); if (!mQuad.isValid() || mQuad.isQuad() || mQuad.getNumCol() != 3 || mQuad.getNumRow() != 5) { logMessage(_T("\tERROR - Constructor of a non square matrix not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor of a non square matrix working\n")); } // Constructor from 3d basis GM_3dVector v1(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v2(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dVector v3(getRandomDouble(), getRandomDouble(), getRandomDouble()); GM_3dBasis b(v1, v2, v3); GM_Matrix mb(b); if (!mb.isValid() || !mb.isQuad() || mb[0][0] != b[0].x() || mb[1][0] != b[0].y() || mb[2][0] != b[0].z() || mb[0][1] != b[1].x() || mb[1][1] != b[1].y() || mb[2][1] != b[1].z() || mb[0][2] != b[2].x() || mb[1][2] != b[2].y() || mb[2][2] != b[2].z()) { logMessage(_T("\tERROR - Constructor from basis not working\n")); numErr++; } else { logMessage(_T("\tOK - Constructor from basis working\n")); } // Copy constructor GM_Matrix mbCopy(mb); bool okCopy = true; for (unsigned short i = 0 ; i < 3 && okCopy ; i++) { for (unsigned short j = 0 ; j < 3 && okCopy ; j++) { if (mbCopy[i][j] != mb[i][j]) okCopy = false; } } if (!mbCopy.isValid() || !okCopy) { logMessage(_T("\tERROR - Copy constructor not working\n")); numErr++; } else { logMessage(_T("\tOK - Copy constructor working\n")); } // Get/Set int col = rand() % 3; int row = rand() % 3; double val = getRandomDouble(); mb[row][col] = val; if (mb[row][col] != val) { logMessage(_T("\tERROR - Get/Set not working\n")); numErr++; } else { logMessage(_T("\tOK - Get/Set working\n")); } // Equality Operators GM_Matrix mbCopy1(mbCopy); if (mbCopy != mbCopy1 || mbCopy == mb) { logMessage(_T("\tERROR - Equality operators not working\n")); numErr++; } else { logMessage(_T("\tOK - Equality operators working\n")); } // Assignment operator GM_Matrix mbCopy2; mbCopy2 = mb; if (!mbCopy2.isValid() || mbCopy2 != mb) { logMessage(_T("\tERROR - Assignment operator not working\n")); numErr++; } else { logMessage(_T("\tOK - Assignment operator working\n")); } // Matrix product unsigned short numRow = 3 + rand() % 10; unsigned short numCol = 3 + rand() % 10; unsigned short numRow1 = numCol; unsigned short numCol1 = 3 + rand() % 10; GM_Matrix m1(numCol, numRow); GM_Matrix m2(numCol1, numRow1); for (unsigned short i = 0 ; i < numRow ; i++) { for (unsigned short j = 0 ; j < numCol ; j++) { m1[i][j] = getRandomDouble(); } } for (unsigned short i = 0 ; i < numRow1 ; i++) { for (unsigned short j = 0 ; j < numCol1 ; j++) { m2[i][j] = getRandomDouble(); } } GM_Matrix prodM = m1 * m2; if (!prodM.isValid() || prodM.getNumRow() != m1.getNumRow() || prodM.getNumCol() != m2.getNumCol()) { logMessage(_T("\tERROR - Matrix product not working\n")); numErr++; } else { bool okProd = true; for (unsigned short i = 0 ; i < numRow && okProd ; i++) { for (unsigned short j = 0 ; j < numCol1 && okProd ; j++) { double val = 0.0; for (unsigned short j1 = 0 ; j1 < numCol ; j1++) { val += m1[i][j1] * m2[j1][j]; } if (val != prodM[i][j]) { okProd = false; } } } if (!okProd) { logMessage(_T("\tERROR - Matrix product not working\n")); numErr++; } else { logMessage(_T("\tOK - Matrix product working\n")); } } // Matrix transposition GM_Matrix mT = mb.transpose(); if (!mT.isValid() || mT.getNumCol() != mb.getNumRow() || mT.getNumRow() != mb.getNumCol()) { logMessage(_T("\tERROR - Matrix trasposition not working\n")); numErr++; } else { bool okTras = true; for (unsigned short i = 0 ; i < mb.getNumRow() && okTras ; i++) { for (unsigned short j = 0 ; j < mb.getNumCol() && okTras ; j++) { if (mb[i][j] != mT[j][i]) { okTras = false; } } } if (!okTras) { logMessage(_T("\tERROR - Matrix trasposition not working\n")); numErr++; } else { logMessage(_T("\tOK - Matrix trasposition working\n")); } } // Matrix inversion unsigned short numRowCol = 3;//3 + rand() % 10; GM_Matrix mToInv(numRowCol, numRowCol); for (unsigned short i = 0 ; i < numRowCol ; i++) { for (unsigned short j = 0 ; j < numRowCol ; j++) { mToInv[i][j] = getRandomDouble(); } } double mToInvDet = mToInv.determinant(); GM_Matrix mInv = mToInv.inverse(); if (!mInv.isValid() && fabs(mToInvDet) > GM_NULL_TOLERANCE) { logMessage(_T("\tERROR - Matrix inversion not working\n")); numErr++; } else { GM_Matrix mCheck = mInv * mToInv; GM_Matrix mCheck1 = mToInv * mInv; if (!mCheck.isValid() || !mCheck.isIdentity() || !mCheck1.isValid() || !mCheck1.isIdentity()) { logMessage(_T("\tERROR - Matrix inversion not working\n")); numErr++; } else { logMessage(_T("\tOK - Matrix inversion working\n")); } } return numErr; }