void TestLinAlg()
{
// Instantiate templates, check sizes, make sure operators compile etc.
static_assert(sizeof(Vec2f) == 8 , "Vec2f size test failed" );
static_assert(sizeof(Vec3f) == 12 , "Vec3f size test failed" );
static_assert(sizeof(Vec4f) == 16 , "Vec4f size test failed" );
static_assert(sizeof(Vec2d) == 16 , "Vec2d size test failed" );
static_assert(sizeof(Vec3d) == 24 , "Vec3d size test failed" );
static_assert(sizeof(Vec4d) == 32 , "Vec4d size test failed" );
static_assert(sizeof(Vec2i) == 8 , "Vec2i size test failed" );
static_assert(sizeof(Vec3i) == 12 , "Vec3i size test failed" );
static_assert(sizeof(Vec4i) == 16 , "Vec4i size test failed" );
static_assert(sizeof(Vec2ui) == 8 , "Vec2ui size test failed" );
static_assert(sizeof(Vec3ui) == 12 , "Vec3ui size test failed" );
static_assert(sizeof(Vec4ui) == 16 , "Vec4ui size test failed" );
static_assert(sizeof(Matrix44f) == 64 , "Matrix44f size test failed");
static_assert(sizeof(Matrix44d) == 128, "Matrix44d size test failed");
Vec2f vec2f(0.0f, 0.0f);
Vec3f vec3f(0.0f, 0.0f, 0.0f);
Vec4f vec4f(0.0f, 0.0f, 0.0f, 0.0f);
Vec2d vec2d(0.0, 0.0);
Vec3d vec3d(0.0, 0.0, 0.0);
Vec4d vec4d(0.0, 0.0, 0.0, 0.0);
Vec2i vec2i(-1, -1);
Vec3i vec3i(-1, -1, -1);
Vec4i vec4i(-1, -1, -1, -1);
Vec2ui vec2ui(0, 0);
Vec3ui vec3ui(0, 0, 0);
Vec4ui vec4ui(0, 0, 0, 0);
float f = 0.0f;
bool b = false;
b = (vec3f == vec3f);
b = (vec3f != vec3f);
vec3f = Vec3f(1.0f) + Vec3f(2.0f);
vec3f = Vec3f(1.0f) - Vec3f(2.0f);
vec3f = Vec3f(1.0f) * Vec3f(2.0f);
vec3f = Vec3f(1.0f) / Vec3f(2.0f);
vec3f = Vec3f(1.0f) * f;
vec3f = f * Vec3f(1.0f);
vec3f = Vec3f(1.0f) / f;
vec3f = -Vec3f(1.0f);
vec3f += Vec3f(1.0f);
vec3f -= Vec3f(1.0f);
vec3f *= Vec3f(1.0f);
vec3f /= Vec3f(1.0f);
vec3f *= f;
vec3f /= f;
f = vec3f[0];
vec3f[0] = f;
f = Length(vec3f);
f = LengthSquared(vec3f);
vec3f = Normalize(vec3f);
f = Dot(Vec3f(1.0f), Vec3f(2.0f));
vec3f = Vec3f(1.0f) ^ Vec3f(2.0f);
Matrix44f matf;
matf.Identity();
Matrix44d matd;
matf.RotationX(1);
matf.RotationY(1);
matf.RotationZ(1);
matf.Scaling(1);
b = matf == matf;
matf = matf * matf;
matf.BuildLookAtMatrix(Vec3f(0.0f, 10.0f, 10.0f), Vec3f(0.0f));
matf.BuildProjection(90.0f, 4.0f / 3.0f, 1.0f, 1000.0f);
Vec3f out;
matf.Transf3x3(vec3f, out);
matf.Transf4x4(vec3f, out);
matf.Transpose3x3();
matf.Transpose4x4();
matf.Invert();
}
mVector3f ArmDialog::CalculateEndEffectorPosition(){
Matrix44f mTmp;
mTmp.Identity(&mTmp);
mat0.Identity(&mat0);
mat1.Identity(&mat1);
mat2.Identity(&mat2);
mat3.Identity(&mat3);
mat4.Identity(&mat4);
mat5.Identity(&mat5);
mat6.Identity(&mat6);
mat7.Identity(&mat7);
mat8.Identity(&mat8);
// Initial
Matrix44f::Translation(&mTmp,6.0f,34.3f,armConfig->baseHeight);
mat0 *= mTmp;
Matrix44f::RotationZ(&mTmp,90.0f/180*3.14);
mat0 *= mTmp;
// 0 to 1
Matrix44f::RotationZ(&mTmp,armConfig->ceta1/180*3.14);
mat0 *= mTmp;
// 1 to 2
Matrix44f::RotationX(&mTmp,90.0f/180*3.14);
mat1 *= mTmp;
Matrix44f::RotationZ(&mTmp,(armConfig->ceta2)/180*3.14);
mat1 *= mTmp;
// 2 to 3
Matrix44f::Translation(&mTmp,armConfig->foreArmLength,0.0f,0.0f);
mat2 *= mTmp;
Matrix44f::RotationZ(&mTmp,(armConfig->ceta3)/180*3.14);
mat2 *= mTmp;
// 3 to 4
Matrix44f::Translation(&mTmp,armConfig->armLength,0.0f,0.0f);
mat3 *= mTmp;
Matrix44f::RotationZ(&mTmp,(armConfig->ceta4)/180*3.14);
mat3 *= mTmp;
// 4 to 5 (camera)
Matrix44f::Translation(&mTmp,armConfig->handLength,0,0);
mat4 *= mTmp;
Matrix44f::RotationZ(&mTmp,30.0f/180.0f*3.14f);
mat4 *= mTmp;
mVector3f position = mat0 * mat1 * mat2 * mat3 * mat4 * mVector3f(0,0,0.0f);
allMat = mat0 * mat1 * mat2 * mat3 * mat4;
char buf[30];
TCHAR tchar[30];
_snprintf(buf, 30, "x=%3.2f y=%3.2f z=%3.2f ", position.x,position.y,position.z);
ConvertCHARToTCHAR(buf,tchar,30);
SetDlgItemText(m_hDlg,IDC_POS,tchar);
return position;
}
