public static Vec4 operator *(Vec4 v, Mat4 a)
{
Vec4 result = new Vec4();
for (int i = 0; i < 4; ++i)
{
result[i] = Vec4.dot(a.column(i), v);
}
return(result);
}
public static Vec4 operator *(Mat4 a, Vec4 v)
{
Vec4 result = new Vec4();
for (int i = 0; i < 4; ++i)
{
result[i] = Vec4.dot(a.row(i), v);
}
return(result);
}
public Mat4(Vec4 v, Vec4 b, Vec4 c, Vec4 d)
{
for (int i = 0; i < 4; ++i)
{
a[i, 0] = v[i];
a[i, 1] = b[i];
a[i, 2] = c[i];
a[i, 3] = d[i];
}
}
public static Mat4 operator *(Mat4 a, Mat4 b)
{
Mat4 mat = new Mat4();
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
mat[i, j] = Vec4.dot(a.row(i), b.column(j));
}
}
return(mat);
}
public static Point3 operator *(Transform a, Point3 p)
{
Vec4 v = new Vec4(p.x, p.y, p.z, 1);
Vec4 result = a.m * v;
if (result.w == 0)
{
//报错
return(new Point3());
}
else
{
float inv = 1 / result.w;
return(new Point3(result.x * inv, result.y * inv, result.z * inv));
}
}
/// <summary>
/// 这个主要是用于将局部坐标系转到世界坐标系
/// </summary>
/// <param name="pos"></param>
/// <param name="x">局部坐标系x轴对应的世界坐标系的坐标</param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <returns></returns>
public static Transform toWorld(Point3 pos, Vec3 x, Vec3 y, Vec3 z)
{
Vec4[] r = new Vec4[3];
Vec4[] t = new Vec4[3];
t[0] = new Vec4(1, 0, 0, pos[0]);
t[1] = new Vec4(0, 1, 0, pos[1]);
t[2] = new Vec4(0, 0, 1, pos[2]);
r[0] = new Vec4(x.x, x.y, x.z, 0);
r[1] = new Vec4(y.x, y.y, y.z, 0);
r[2] = new Vec4(z.x, z.y, z.z, 0);
Mat4 rotate = new Mat4(r[0], r[1], r[2], new Vec4(0, 0, 0, 1));
Mat4 translate = new Mat4(t[0], t[1], t[2], new Vec4(0, 0, 0, 1));
Mat4 m = rotate.transpose() * translate;
return(new Transform(m));
}
public static float dot(Vec4 v1, Vec4 v2)
{
return(v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w);
}
