Exemplo n.º 1
0
bool UnitTest::TestAll() {
    cout << endl << "Testing all modules" << endl;

    bool temp_result;

    temp_result = TestMath();
    cout << endl << "Math test: ";
    if (temp_result) {
        cout << "succeeded" << endl << endl;
    } else {
        cout << "failed" << endl << endl;
        return false;
    }

    temp_result = Test2D();
    cout << endl << "2D test: ";
    if (temp_result) {
        cout << "succeeded" << endl << endl;
    } else {
        cout << "failed" << endl << endl;
        return false;
    }

    temp_result = Test3D();
    cout << endl << "3D test: ";
    if (temp_result) {
        cout << "succeeded" << endl << endl;
    } else {
        cout << "failed" << endl << endl;
        return false;
    }

    return true;
}
Exemplo n.º 2
0
int main()
{	
  Test3D();
  Test2D();

  TestIntersection3D();
  TestIntersection2D();

	return 0;
}
Exemplo n.º 3
0
void CMotionControl::UnitTest()
{
#ifdef DO_UNITTEST

	InitConversion(ToMm1000_1_1000, ToMachine_1_1000);

	// matrix test

	float msrc[NUM_AXISXYZ][NUM_AXISXYZ] = { { 3 , 2 , 3 }, { 4, 5, 6}, { 7, 8, 9,} } ;
	float mdest[3][3];
	float mdest2[3][3];

	CMatrix3x3<float>::Invert(msrc,mdest);
	CMatrix3x3<float>::Invert(mdest,mdest2);

	float v1[3] = { 1.0, 2.0, 3.0 };
	float v2[3];

	CMatrix3x3<float>::Mul(mdest2,v1,v2);

	// see: http://www.math.kit.edu/iag2/~globke/media/koordinaten.pdf
	// see: http://de.wikipedia.org/wiki/Denavit-Hartenberg-Transformation

	float theta = (float)(15.0 * 180 / M_PI);	// phi ... einer Rotation \theta_n(Gelenkwinkel) um die z_{ n - 1 }-Achse, damit die x_{ n - 1 }-Achse parallel zu der x_n - Achse liegt
	float alpha = (float)(10.0 * 180 / M_PI);	// alpha ... einer Rotation \alpha_n (Verwindung) um die x_n-Achse, um die z_{n - 1}-Achse in die z_n-Achse zu überführen
	float di = 10;								// d ... einer Translation d_n (Gelenkabstand) entlang der z_{n - 1}- Achse bis zu dem Punkt, wo sich z_{n - 1} und x_n schneiden
	float ai = 5;								// a ... einer Translation a_n (Armelementlänge) entlang der x_n-Achse, um die Ursprünge der Koordinatensysteme in Deckung zu bringen

	float A1[4][4] = 
	{ 
		{ cos(theta),	-sin(theta)*cos(alpha),		sin(theta)*sin(alpha),		ai*cos(theta) },
		{ sin(theta),	cos(theta)*cos(alpha),		-cos(theta)*sin(alpha),		ai*sin(theta) },
		{ 0,			sin(alpha),					cos(alpha),					di },
		{ 0,			0,						0,						1	}
	};

	float A2[4][4];

	CMatrix4x4<float>::InitDenavitHartenberg(A2,alpha,theta,ai,di);


	float ps[4] = { 1, 2, 3, 1 };
	float pd[4];

	CMatrix4x4<float>::Mul(A1, ps, pd);

	// 3d Test

	mm1000_t ofs[3] = { 1000,2000,71000 };

	mm1000_t srcX[3] = { 1000,0,0 };
	mm1000_t srcY[3] = { 0,1000,0 };
	mm1000_t srcZ[3] = { 0,0,1000 };
	mm1000_t srcXY[3] = { 1000,2000,3000 };
	mm1000_t dest[3] = { 0,0,0 };

	mm1000_t vectX[3] = { 100,0,0 };
	mm1000_t vectY[3] = { 0,100,0 };
	mm1000_t vectZ[3] = { 0,0,100 };

	mm1000_t vectXY[3] = { 100,100,0 };
	mm1000_t vectXZ[3] = { 100,0,100 };
	mm1000_t vectYZ[3] = { 0,100,100 };

	mm1000_t vectXYZ[3] = { 100,100,100 };

	float angle=(float)(M_PI/6);
	//angle=0.001;

	Test3D(srcX,ofs,dest,vectX,angle,true);
	Test3D(srcX,ofs,dest,vectY,angle,true);
	Test3D(srcX,ofs,dest,vectZ,angle,true);

	Test3D(srcX,ofs,dest,vectXY,angle,true);
	Test3D(srcX,ofs,dest,vectXZ,angle,true);
	Test3D(srcX,ofs,dest,vectYZ,angle,true);

	Test3D(srcX,ofs,dest,vectXYZ,angle,true);

	Test3D(srcY,ofs,dest,vectX,angle,true);
	Test3D(srcY,ofs,dest,vectY,angle,true);
	Test3D(srcY,ofs,dest,vectZ,angle,true);
			
	Test3D(srcY,ofs,dest,vectXY,angle,true);
	Test3D(srcY,ofs,dest,vectXZ,angle,true);
	Test3D(srcY,ofs,dest,vectYZ,angle,true);
			
	Test3D(srcY,ofs,dest,vectXYZ,angle,true);

	Test3D(srcZ,ofs,dest,vectX,angle,true);
	Test3D(srcZ,ofs,dest,vectY,angle,true);
	Test3D(srcZ,ofs,dest,vectZ,angle,true);
			
	Test3D(srcZ,ofs,dest,vectXY,angle,true);
	Test3D(srcZ,ofs,dest,vectXZ,angle,true);
	Test3D(srcZ,ofs,dest,vectYZ,angle,true);
			
	Test3D(srcZ,ofs,dest,vectXYZ,angle,true);

	Test3D(srcXY,ofs,dest,vectX,angle,true);
	Test3D(srcXY,ofs,dest,vectY,angle,true);
	Test3D(srcXY,ofs,dest,vectZ,angle,true);
			
	Test3D(srcXY,ofs,dest,vectXY,angle,true);
	Test3D(srcXY,ofs,dest,vectXZ,angle,true);
	Test3D(srcXY,ofs,dest,vectYZ,angle,true);
			
	Test3D(srcXY,ofs,dest,vectXYZ,angle,true);

	ClearRotate();

	// 2D Test

	float angle2dX[3]={ angle,0,0 };
	float angle2dY[3]={ 0,angle, 0 };
	float angle2dZ[3]={ 0,0, angle };
	float angle2d[3]={ angle,angle, angle };

	Test2D(srcXY,ofs,dest,angle2dX,true);
	Test2D(srcXY,ofs,dest,angle2dY,true);
	Test2D(srcXY,ofs,dest,angle2dZ,true);

	Test2D(srcXY,ofs,dest,angle2d,true);

	//2d+3D

	SetRotate(angle,vectXYZ,ofs);
	Test2D(srcXY,ofs,dest,angle2d,true);

	ClearRotate2D();
	ClearRotate();

#endif
}