public static Vec4 Invert(Vec4 a) { var dot = a.X * a.X + a.Y * a.Y + a.Z * a.Z + a.W * a.W; var invDot = dot != 0 ? 1f / dot : 0f; return(new Vec4( -a.X * invDot, -a.Y * invDot, -a.Z * invDot, a.W * invDot )); }
// q это кватернион public static Vec3 TransformQuat(Vec3 a, Vec4 q) { var ix = q.W * a.X + q.Y * a.Z - q.Z * a.Y; var iy = q.W * a.Y + q.Z * a.X - q.X * a.Z; var iz = q.W * a.Z + q.X * a.Y - q.Y * a.X; var iw = -q.X * a.X - q.Y * a.Y - q.Z * a.Z; return(new Vec3( ix * q.W + iw * -q.X + iy * -q.Z - iz * -q.Y, iy * q.W + iw * -q.Y + iz * -q.X - ix * -q.Z, iz * q.W + iw * -q.Z + ix * -q.Y - iy * -q.X )); }
public static Vec4 QuaternionFromMatrix(Mat4 m) { var q = new Vec4(); q.W = (float)Math.Sqrt(Math.Max(0, 1 + m.a00 + m.a11 + m.a22)) / 2f; q.X = (float)Math.Sqrt(Math.Max(0, 1 + m.a00 - m.a11 - m.a22)) / 2f; q.Y = (float)Math.Sqrt(Math.Max(0, 1 - m.a00 + m.a11 - m.a22)) / 2f; q.Z = (float)Math.Sqrt(Math.Max(0, 1 - m.a00 - m.a11 + m.a22)) / 2f; q.X *= Math.Sign(q.X * (m.a21 - m.a12)); q.Y *= Math.Sign(q.Y * (m.a02 - m.a20)); q.Z *= Math.Sign(q.Z * (m.a10 - m.a01)); return(q); }
public WhVertex(BinaryReader r) { Position = new Vec3(r.ReadSingle(), r.ReadSingle(), r.ReadSingle()); Normal = new Vec4(r.ReadSingle(), r.ReadSingle(), r.ReadSingle(), 0); U = r.ReadSingle(); V = r.ReadSingle(); U2 = r.ReadSingle(); V2 = r.ReadSingle(); Weights = new ByteVec4(r.ReadByte(), r.ReadByte(), r.ReadByte(), r.ReadByte()); Bones = new ByteVec4(r.ReadByte(), r.ReadByte(), r.ReadByte(), r.ReadByte()); //self.transPosition = vec3.clone(self.position); //self.transNormal = vec4.clone(self.normal) Frame = 0; }
public static Vec4 FromMat3(Mat3 m) { var fTrace = m[0] + m[4] + m[8]; float fRoot; var res = new Vec4(); if (fTrace > 0) { fRoot = (float)Math.Sqrt(fTrace + 1); res[3] = 0.5f * fRoot; fRoot = 0.5f / fRoot; res[0] = (m[7] - m[5]) * fRoot; res[1] = (m[2] - m[6]) * fRoot; res[2] = (m[3] - m[1]) * fRoot; } else { var i = 0; if (m[4] > m[0]) { i = 1; } if (m[8] > m[i * 3 + i]) { i = 2; } var j = (i + 1) % 3; var k = (i + 2) % 3; fRoot = (float)Math.Sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + 1); res[i] = 0.5f * fRoot; fRoot = 0.5f / fRoot; res[3] = (m[k * 3 + j] - m[j * 3 + k]) * fRoot; res[j] = (m[j * 3 + i] + m[i * 3 + j]) * fRoot; res[k] = (m[k * 3 + i] + m[i * 3 + k]) * fRoot; } return(res); }
public static Vec4 RotateZ(Vec4 a, float rad) { rad *= 0.5f; var ax = a[0]; var ay = a[1]; var az = a[2]; var aw = a[3]; var bz = (float)Math.Sin(rad); var bw = (float)Math.Cos(rad); var res = new Vec4(); res[0] = ax * bw + ay * bz; res[1] = ay * bw - ax * bz; res[2] = az * bw + aw * bz; res[3] = aw * bw - az * bz; return(res); }
public static Vec4 Multiply(Vec4 a, Vec4 b) { var ax = a[0]; var ay = a[1]; var az = a[2]; var aw = a[3]; var bx = b[0]; var by = b[1]; var bz = b[2]; var bw = b[3]; var res = new Vec4(); res[0] = ax * bw + aw * bx + ay * bz - az * by; res[1] = ay * bw + aw * by + az * bx - ax * bz; res[2] = az * bw + aw * bz + ax * by - ay * bx; res[3] = aw * bw - ax * bx - ay * by - az * bz; return(res); }
public static Vec4 Slerp(Vec4 a, Vec4 b, float t) { var cosom = a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; if (cosom < 0) { cosom = -cosom; b.X = -b.X; b.Y = -b.Y; b.Z = -b.Z; b.W = -b.W; } float scale0; float scale1; if (1f - cosom > 1e-6f) { var omega = (float)Math.Acos(cosom); var sinom = (float)Math.Sin(omega); scale0 = (float)Math.Sin((1f - t) * omega) / sinom; scale1 = (float)Math.Sin(t * omega) / sinom; } else { scale0 = 1f - t; scale1 = t; } return(new Vec4( scale0 * a.X + scale1 * b.X, scale0 * a.Y + scale1 * b.Y, scale0 * a.Z + scale1 * b.Z, scale0 * a.W + scale1 * b.W )); }