public void Rotate(float angle, Vec3f v)
{
float a = angle;
float c = (float)Math.Cos(a);
float s = (float)Math.Sin(a);
Vec3f axis = v.unit();
Vec3f temp = new Vec3f((1.0f - c) * axis);
Mat4f Rotate = new Mat4f(0.0f);
Rotate.data[0][0] = c + temp[0] * axis[0];
Rotate.data[0][1] = temp[0] * axis[1] + s * axis[2];
Rotate.data[0][2] = temp[0] * axis[2] - s * axis[1];
Rotate.data[1][0] = temp[1] * axis[0] - s * axis[2];
Rotate.data[1][1] = c + temp[1] * axis[1];
Rotate.data[1][2] = temp[1] * axis[2] + s * axis[0];
Rotate.data[2][0] = temp[2] * axis[0] + s * axis[1];
Rotate.data[2][1] = temp[2] * axis[1] - s * axis[0];
Rotate.data[2][2] = c + temp[2] * axis[2];
data[0] = data[0] * Rotate.data[0][0] + data[1] * Rotate.data[0][1] + data[2] * Rotate.data[0][2];
data[1] = data[0] * Rotate.data[1][0] + data[1] * Rotate.data[1][1] + data[2] * Rotate.data[1][2];
data[2] = data[0] * Rotate.data[2][0] + data[1] * Rotate.data[2][1] + data[2] * Rotate.data[2][2];
}
public static Mat4f RotationMatrix(Mat4f m, float angle, Vec3f v)
{
float a = angle;
float c = (float)Math.Cos(a);
float s = (float)Math.Sin(a);
Vec3f axis = v.unit();
Vec3f temp = new Vec3f((1.0f - c) * axis);
Mat4f Rotate = new Mat4f(0.0f);
Rotate.data[0][0] = c + temp[0] * axis[0];
Rotate.data[0][1] = temp[0] * axis[1] + s * axis[2];
Rotate.data[0][2] = temp[0] * axis[2] - s * axis[1];
Rotate.data[1][0] = temp[1] * axis[0] - s * axis[2];
Rotate.data[1][1] = c + temp[1] * axis[1];
Rotate.data[1][2] = temp[1] * axis[2] + s * axis[0];
Rotate.data[2][0] = temp[2] * axis[0] + s * axis[1];
Rotate.data[2][1] = temp[2] * axis[1] - s * axis[0];
Rotate.data[2][2] = c + temp[2] * axis[2];
Mat4f Result = new Mat4f(0.0f);
Result[0] = m[0] * Rotate.data[0][0] + m[1] * Rotate.data[0][1] + m[2] * Rotate.data[0][2];
Result[1] = m[0] * Rotate.data[1][0] + m[1] * Rotate.data[1][1] + m[2] * Rotate.data[1][2];
Result[2] = m[0] * Rotate.data[2][0] + m[1] * Rotate.data[2][1] + m[2] * Rotate.data[2][2];
Result[3] = m[3];
return(Result);
}
public Mat4f transpose()
{
Mat4f transpMat = new Mat4f();
transpMat.data[0][0] = data[0][0];
transpMat.data[0][1] = data[1][0];
transpMat.data[0][2] = data[2][0];
transpMat.data[0][3] = data[3][0];
transpMat.data[1][0] = data[0][1];
transpMat.data[1][1] = data[1][1];
transpMat.data[1][2] = data[2][1];
transpMat.data[1][3] = data[3][1];
transpMat.data[2][0] = data[0][2];
transpMat.data[2][1] = data[1][2];
transpMat.data[2][2] = data[2][2];
transpMat.data[2][3] = data[3][2];
transpMat.data[3][0] = data[0][3];
transpMat.data[3][1] = data[1][3];
transpMat.data[3][2] = data[2][3];
transpMat.data[3][3] = data[3][3];
return(transpMat);
}
public Mat4f toMat()
{
Mat4f res = new Mat4f();
float wx, wy, wz, xx, yy, zz, yz, xy, xz, x2, y2, z2;
x2 = this.i + this.i;
y2 = this.j + this.j;
z2 = this.k + this.k;
xx = this.i * x2; xy = this.i * y2; xz = this.i * z2;
yy = this.j * y2; yz = this.j * z2; zz = this.k * z2;
wx = this.r * x2; wy = this.r * y2; wz = this.r * z2;
res.data[0][0] = 1.0f - (yy + zz); res.data[0][1] = xy - wz;
res.data[0][2] = xz + wy; res.data[0][3] = 0.0f;
res.data[1][0] = xy + wz; res.data[1][1] = 1.0f - (xx + zz);
res.data[1][2] = yz - wx; res.data[1][3] = 0.0f;
res.data[2][0] = xz - wy; res.data[2][1] = yz + wx;
res.data[2][2] = 1.0f - (xx + yy); res.data[2][3] = 0.0f;
res.data[3][0] = 0.0f; res.data[3][1] = 0.0f;
res.data[3][2] = 0.0f; res.data[3][3] = 1.0f;
return(res);
}
public Mat4f inverse()
{
float Coef00 = data[2][2] * data[3][3] - data[3][2] * data[2][3];
float Coef02 = data[1][2] * data[3][3] - data[3][2] * data[1][3];
float Coef03 = data[1][2] * data[2][3] - data[2][2] * data[1][3];
float Coef04 = data[2][1] * data[3][3] - data[3][1] * data[2][3];
float Coef06 = data[1][1] * data[3][3] - data[3][1] * data[1][3];
float Coef07 = data[1][1] * data[2][3] - data[2][1] * data[1][3];
float Coef08 = data[2][1] * data[3][2] - data[3][1] * data[2][2];
float Coef10 = data[1][1] * data[3][2] - data[3][1] * data[1][2];
float Coef11 = data[1][1] * data[2][2] - data[2][1] * data[1][2];
float Coef12 = data[2][0] * data[3][3] - data[3][0] * data[2][3];
float Coef14 = data[1][0] * data[3][3] - data[3][0] * data[1][3];
float Coef15 = data[1][0] * data[2][3] - data[2][0] * data[1][3];
float Coef16 = data[2][0] * data[3][2] - data[3][0] * data[2][2];
float Coef18 = data[1][0] * data[3][2] - data[3][0] * data[1][2];
float Coef19 = data[1][0] * data[2][2] - data[2][0] * data[1][2];
float Coef20 = data[2][0] * data[3][1] - data[3][0] * data[2][1];
float Coef22 = data[1][0] * data[3][1] - data[3][0] * data[1][1];
float Coef23 = data[1][0] * data[2][1] - data[2][0] * data[1][1];
Vec4f Fac0 = new Vec4f(Coef00, Coef00, Coef02, Coef03);
Vec4f Fac1 = new Vec4f(Coef04, Coef04, Coef06, Coef07);
Vec4f Fac2 = new Vec4f(Coef08, Coef08, Coef10, Coef11);
Vec4f Fac3 = new Vec4f(Coef12, Coef12, Coef14, Coef15);
Vec4f Fac4 = new Vec4f(Coef16, Coef16, Coef18, Coef19);
Vec4f Fac5 = new Vec4f(Coef20, Coef20, Coef22, Coef23);
Vec4f Vec0 = new Vec4f(data[1][0], data[0][0], data[0][0], data[0][0]);
Vec4f Vec1 = new Vec4f(data[1][1], data[0][1], data[0][1], data[0][1]);
Vec4f Vec2 = new Vec4f(data[1][2], data[0][2], data[0][2], data[0][2]);
Vec4f Vec3 = new Vec4f(data[1][3], data[0][3], data[0][3], data[0][3]);
Vec4f Inv0 = new Vec4f(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2);
Vec4f Inv1 = new Vec4f(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4);
Vec4f Inv2 = new Vec4f(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5);
Vec4f Inv3 = new Vec4f(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5);
Vec4f SignA = new Vec4f(+1, -1, +1, -1);
Vec4f SignB = new Vec4f(-1, +1, -1, +1);
Mat4f Inverse = new Mat4f(Inv0 * SignA, Inv1 * SignB, Inv2 * SignA, Inv3 * SignB);
Vec4f Row0 = new Vec4f(Inverse.data[0][0], Inverse.data[1][0], Inverse.data[2][0], Inverse.data[3][0]);
Vec4f Dot0 = new Vec4f(data[0] * Row0);
float Dot1 = (Dot0.x + Dot0.y) + (Dot0.z + Dot0.w);
float OneOverDeterminant = 1.0f / Dot1;
return(Inverse * OneOverDeterminant);
}
public static Mat4f operator +(Mat4f m1, Mat4f m2)
{
Mat4f res = new Mat4f();
res.data[0] = m1.data[0] + m2.data[0];
res.data[1] = m1.data[1] + m2.data[1];
res.data[2] = m1.data[2] + m2.data[2];
res.data[3] = m1.data[3] + m2.data[3];
return(res);
}
public static Mat4f lookAtLH(Vec3f eye, Vec3f center, Vec3f up)
{
Vec3f f = (center - eye).normal();
Vec3f s = (f * up).normal();
Vec3f u = s * f;
Mat4f Result = new Mat4f();
Result.data[0][0] = s.x;
Result.data[1][0] = s.y;
Result.data[2][0] = s.z;
Result.data[0][1] = u.x;
Result.data[1][1] = u.y;
Result.data[2][1] = u.z;
Result.data[0][2] = f.x;
Result.data[1][2] = f.y;
Result.data[2][2] = f.z;
Result.data[3][0] = -(s.dot(eye));
Result.data[3][1] = -(u.dot(eye));
Result.data[3][2] = -(f.dot(eye));
return(Result);
}
public void fromMat(Mat4f other)
{
}
