public void ConstructorValuesAreAccessibleByIndexer() { Matrix3x4 matrix3x4; matrix3x4 = new Matrix3x4(); for (int x = 0; x < matrix3x4.Columns; x++) { for (int y = 0; y < matrix3x4.Rows; y++) { Assert.Equal(0, matrix3x4[x, y], Epsilon); } } double value = 33.33; matrix3x4 = new Matrix3x4(value); for (int x = 0; x < matrix3x4.Columns; x++) { for (int y = 0; y < matrix3x4.Rows; y++) { Assert.Equal(value, matrix3x4[x, y], Epsilon); } } GenerateFilledMatrixWithValues(out matrix3x4); for (int y = 0; y < matrix3x4.Rows; y++) { for (int x = 0; x < matrix3x4.Columns; x++) { Assert.Equal(y * matrix3x4.Columns + x, matrix3x4[x, y], Epsilon); } } }
public void ConstantValuesAreCorrect() { Matrix3x4 matrix3x4 = new Matrix3x4(); Assert.Equal(3, matrix3x4.Columns); Assert.Equal(4, matrix3x4.Rows); Assert.Equal(Matrix3x4.ColumnCount, matrix3x4.Columns); Assert.Equal(Matrix3x4.RowCount, matrix3x4.Rows); }
private static Envelopes LoadEVP1FromStream(EndianBinaryReader reader, long chunkStart) { Envelopes envelopes = new Envelopes(); ushort numEnvelopes = reader.ReadUInt16(); reader.ReadUInt16(); // Padding // numEnvelope many uint8 - each one describes how many bones belong to this index. uint boneCountOffset = reader.ReadUInt32(); // "sum over all bytes in boneCountOffset many shorts (index into some joint stuff? into matrix table?)" uint indexDataOffset = reader.ReadUInt32(); // Bone Weights (as many floats here as there are ushorts at indexDataOffset) uint weightOffset = reader.ReadUInt32(); // Matrix Table (3x4 float array) - Inverse Bind Pose uint boneMatrixOffset = reader.ReadUInt32(); // - Is this the number of bones which influence the vert? reader.BaseStream.Position = chunkStart + boneCountOffset; for (int b = 0; b < numEnvelopes; b++) envelopes.numBonesAffecting.Add(reader.ReadByte()); // ??? reader.BaseStream.Position = chunkStart + indexDataOffset; for (int m = 0; m < envelopes.numBonesAffecting.Count; m++) { for (int j = 0; j < envelopes.numBonesAffecting[m]; j++) { envelopes.indexRemap.Add(reader.ReadUInt16()); } } // Bone Weights reader.BaseStream.Position = chunkStart + weightOffset; for (int w = 0; w < envelopes.numBonesAffecting.Count; w++) { for (int j = 0; j < envelopes.numBonesAffecting[w]; j++) { envelopes.weights.Add(reader.ReadSingle()); } } // Inverse Bind Pose Matrices reader.BaseStream.Position = chunkStart + boneMatrixOffset; for (int w = 0; w < numEnvelopes; w++) { Matrix3x4 matrix = new Matrix3x4(); for (int j = 0; j < 3; j++) { for (int k = 0; k < 4; k++) matrix[j, k] = reader.ReadSingle(); } envelopes.inverseBindPose.Add(matrix); } return envelopes; }
public void IndexerGetAndSetValuesCorrectly() { Matrix3x4 matrix3x4 = new Matrix3x4(); for (int x = 0; x < matrix3x4.Columns; x++) { for (int y = 0; y < matrix3x4.Rows; y++) { matrix3x4[x, y] = y * matrix3x4.Columns + x; } } for (int y = 0; y < matrix3x4.Rows; y++) { for (int x = 0; x < matrix3x4.Columns; x++) { Assert.Equal(y * matrix3x4.Columns + x, matrix3x4[x, y], Epsilon); } } }
public void AccessorThrowsWhenOutOfBounds() { Matrix3x4 matrix3x4 = new Matrix3x4(); try { matrix3x4[-1, 0] = 0; Assert.Fail("Matrix3x4[-1, 0] did not throw when it should have."); } catch (ArgumentOutOfRangeException) { } try { matrix3x4[0, -1] = 0; Assert.Fail("Matrix3x4[0, -1] did not throw when it should have."); } catch (ArgumentOutOfRangeException) { } try { matrix3x4[3, 0] = 0; Assert.Fail("Matrix3x4[3, 0] did not throw when it should have."); } catch (ArgumentOutOfRangeException) { } try { matrix3x4[0, 4] = 0; Assert.Fail("Matrix3x4[0, 4] did not throw when it should have."); } catch (ArgumentOutOfRangeException) { } }
public void Set(ref Matrix3x4 value, bool transpose) { this.Uniform.Set(ref value, transpose); }
public void SetUniform(string uniform, Matrix3x4 value) { GL.UniformMatrix3x4(GL.GetUniformLocation(program, uniform), false, ref value); }
public void MuliplyByMatrix3x4ProducesMatrix3x3() { Matrix4x3 matrix1 = new Matrix4x3(3); Matrix3x4 matrix2 = new Matrix3x4(2); Matrix3x3 result = matrix1 * matrix2; Matrix3x3 expected = new Matrix3x3(24, 24, 24, 24, 24, 24, 24, 24, 24); Assert.Equal(expected, result); }
public void Set(ref Matrix3x4 value) { this.Uniform.Set(ref value); }
/// <summary> /// Sets position, rotation and scale of an entity slot, ba a matrix. /// </summary> /// <param name="slot">Slot.</param> /// <param name="mx">Mx.</param> public void SetGeometrySlotLocalTransform(int slot, Matrix3x4 mx) { NativeHandle.SetSlotLocalTM(slot, mx); }
public void LoadEVP1FromStream(EndianBinaryReader reader, long tagStart) { ushort envelopeCount = reader.ReadUInt16(); Trace.Assert(reader.ReadUInt16() == 0xFFFF); // Padding uint boneInfluenceCountOffset = reader.ReadUInt32(); // This points to an array which is envelopeCount many long bytes which specify how many bones influence that particular envelope. uint boneIndexDataOffset = reader.ReadUInt32(); // For each influence of each envelope, which bone index is the envelope referring to uint weightDataOffset = reader.ReadUInt32(); // For each influence of each envelope, a float indicating how much weight the envelope has. uint boneMatrixOffset = reader.ReadUInt32(); // Matrix Table (3x4 float array) - Skeleton Inverse Bind Pose. You have to get the highest index from boneIndex to know how many to read. byte[] numBoneInfluences = new byte[envelopeCount]; InverseBindPose = new List <Matrix4>(); Envelopes = new List <Envelope>(); // How many bones influence the given index reader.BaseStream.Position = tagStart + boneInfluenceCountOffset; for (int i = 0; i < envelopeCount; i++) { numBoneInfluences[i] = reader.ReadByte(); } // For each influence, an index remap? int numMatrices = 0; reader.BaseStream.Position = tagStart + boneIndexDataOffset; for (int m = 0; m < envelopeCount; m++) { Envelope env = new Envelope(); env.NumBones = numBoneInfluences[m]; env.BoneWeights = new float[env.NumBones]; env.BoneIndexes = new ushort[env.NumBones]; Envelopes.Add(env); for (int j = 0; j < numBoneInfluences[m]; j++) { ushort val = reader.ReadUInt16(); env.BoneIndexes[j] = val; numMatrices = Math.Max(numMatrices, val + 1); } } // For each influence, how much does that influence have an affect. reader.BaseStream.Position = tagStart + weightDataOffset; for (int m = 0; m < envelopeCount; m++) { Envelope env = Envelopes[m]; for (int j = 0; j < numBoneInfluences[m]; j++) { float val = reader.ReadSingle(); env.BoneWeights[j] = val; } } // For each envelope index, what is the Inverse Bind Pose matrix? The Inverse Bind Pose matrix will transform // a vertex from being in model space into local space around its bone. reader.BaseStream.Position = tagStart + boneMatrixOffset; for (int m = 0; m < numMatrices; m++) { Matrix3x4 matrix = new Matrix3x4(); for (int j = 0; j < 3; j++) { for (int k = 0; k < 4; k++) { matrix[j, k] = reader.ReadSingle(); } } Matrix4 bindPoseMatrix = new Matrix4(matrix.Row0, matrix.Row1, matrix.Row2, new Vector4(0, 0, 0, 1)); // We transpose this matrix for use in OpenTK so it lines up with existing joint-matrix calculations. bindPoseMatrix.Transpose(); InverseBindPose.Add(bindPoseMatrix); } }
private void GenerateFilledMatrixWithValues(out Matrix3x4 matrix) { matrix = new Matrix3x4( 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11); }
public void TestMethods() { //we need 3 orthonormal vectors for equivalence test to pass Vector3 right = new Vec3(1f, 0f, -1f); right = right.Normalized; Vector3 forward = new Vec3(1f, 1.4142136f, 1f); forward = forward.Normalized; Vector3 up = new Vec3(1f, -1.4142136f, 1f); up = up.Normalized; //native Matrix34 nativematrix = new Matrix34(0f, 3f, 6f, 9f, 1f, 4f, 7f, 10f, 2f, 5f, 8f, 11f); Quat nativeQuat = Quat.CreateQuatFromMatrix(nativematrix); //managed Matrix3x4 managedmatrix = new Matrix3x4(0f, 3f, 6f, 9f, 1f, 4f, 7f, 10f, 2f, 5f, 8f, 11f); Quaternion managedQuat = Quat.CreateQuatFromMatrix(managedmatrix); float radians = 1.57f; //1 normalize { Quaternion test1a = managedQuat; test1a.Normalize(); Quat test1b = nativeQuat; test1b.Normalize(); Assert.IsTrue(Quat.IsEquivalent(test1a, test1b), "Normalize Equivalence test failed"); } //2 Dot //3 Difference //4 CreateFromVectors { Matrix3x3 matrix33 = Matrix33.CreateFromVectors(right, forward, up); Quat quat2a = Quat.CreateQuatFromMatrix(matrix33); Vector3 right2 = new Vector3(right.x, right.y, right.z); Vector3 forward2 = new Vector3(forward.x, forward.y, forward.z); Vector3 up2 = new Vector3(up.x, up.y, up.z); Quaternion quat2b = Quaternion.CreateFromVectors(right2, forward2, up2); Assert.IsTrue(quat2a == quat2b, "CreateFromVectors equality failed"); Assert.IsTrue(Quat.IsEquivalent(quat2a, quat2b), "CreateFromVectors equivalence failed"); } //5 SetLookOrientation //5a { //compare to SetRotationVDir, up vector = (0,0,1) //get 2 orthogonal vectors Vector3 up5 = new Vector3(0f, 0f, 1f); Vector3 forward5 = new Vector3(1f, 1.4142136f, 0f); forward5 = forward5.Normalized; Vector3 right5 = forward5.Cross(up5); Quat nativeQuaternion2 = Quat.CreateIdentity(); nativeQuaternion2.SetRotationVDir(forward5); Matrix33 nativeMatrix = Matrix33.CreateMatrix33(right5, forward5, up5); Quat nativeQuaternion3 = Quat.CreateQuatFromMatrix(nativeMatrix); Assert.IsTrue(Quat.IsEquivalent(nativeQuaternion2, nativeQuaternion3), "Native Quaternion constructor failed comparison with SetRotationVDir. Expected :" + ((Quaternion)nativeQuaternion2).ToString() + ", Actual :" + ((Quaternion)nativeQuaternion3).ToString()); } //5b { // test new C# SetLookOrientation, DEV-3691 // rotate forward around x-y plane, z=0 // i) 20 values for x-y plane // ii) 20 values for y-z plane Vec3[] testVectors = new Vec3[40]; for (uint i = 0; i < 20; ++i) { radians = ((float)System.Math.PI) / 20.0f * i; testVectors[i] = new Vec3((float)System.Math.Cos(radians), (float)System.Math.Sin(radians), 0f); } for (uint i = 20; i < 40; ++i) { radians = ((float)System.Math.PI) / 20.0f * i; testVectors[i] = new Vec3(0f, (float)System.Math.Cos(radians), (float)System.Math.Sin(radians)); } const float test_margin_error = 0.05f; for (uint i = 0; i < testVectors.Length; ++i) { Vec3 forward5c = testVectors[i]; forward5c = forward5c.normalized(); Quat nativeQuat5c = Quat.CreateIdentity(); nativeQuat5c.SetRotationVDir(forward5c); float dtPrdt1 = MathHelpers.Clamp(forward5c.Dot(nativeQuat5c.GetColumn1()), -1f, 1f); Quaternion managedQuat5c = Quaternion.Identity; Vector3 upManaged = Vector3.Up; managedQuat5c.SetLookOrientation(forward5c, upManaged); float dtPrdt2 = MathHelpers.Clamp(forward5c.Dot(managedQuat5c.Forward), -1f, 1f); float diffFromNative = (float)(System.Math.Acos(dtPrdt1) * (180f / System.Math.PI)); float diffFromManaged = (float)(System.Math.Acos(dtPrdt2) * (180f / System.Math.PI)); float absoluteDiff = System.Math.Abs(diffFromManaged - diffFromNative); Assert.IsTrue(absoluteDiff <= test_margin_error, "SetLookOrientation failed at loop index " + i + ".Absolute Difference:" + absoluteDiff + " Expected (Native) :" + diffFromNative + ", Actual (Managed) :" + diffFromManaged + ", Forward : " + NativeExtensions.PrintString(forward5c) + ", Up :" + upManaged.ToString()); } { //boundary case where axis are flipped when comparing native to managed Quaternion quatManaged = Quaternion.Identity; Vector3 upManaged = Vector3.Up; Vector3 forwardManaged = new Vector3(-8.126793f, 3.401123f, -1.644333f); forwardManaged = forwardManaged.Normalized; quatManaged.SetLookOrientation(forwardManaged, upManaged); Quat quatNative = Quat.CreateIdentity(); Vec3 forwardNative = new Vec3(-8.126793f, 3.401123f, -1.644333f); forwardNative = forwardNative.normalized(); quatNative.SetRotationVDir(forwardNative); bool isEqui1 = Quat.IsEquivalent(quatManaged, quatNative, 0.00999999776f); Assert.IsTrue(isEqui1, String.Format("Native Quaternion {0} and Managed Quaternion {1} are not equivalent", ((Quaternion)quatNative).ToString(), quatManaged)); } } //6 SetFromTORotation(Vector3 , Vector3) { Vec3 fromVec = new Vec3(0.5f, 0.5f, 0.5f); Vec3 toVec = new Vec3(0.5f, -0.5f, -0.5f); Quat quat6a = Quat.CreateIdentity(); quat6a.SetRotationV0V1(fromVec, toVec); Vector3 fromVec2 = new Vector3(fromVec.x, fromVec.y, fromVec.z); Vector3 toVec2 = new Vector3(toVec.x, toVec.y, toVec.z); Quaternion quat6b = Quaternion.Identity; quat6b.SetFromToRotation(fromVec2, toVec2); Assert.IsTrue(Quat.IsEquivalent(quat6a, quat6b), "SetFromToRotation failed"); } //7 CreateRotationX(float) { Quat quat7a = Quat.CreateRotationX(radians); Quaternion quat7b = Quaternion.CreateRotationX(radians); Assert.IsTrue(Quat.IsEquivalent(quat7a, quat7b), "CreateRotationX failed"); } //8 CreateRotationY(float) { Quat quat8a = Quat.CreateRotationY(radians + 0.1f); Quaternion quat8b = Quaternion.CreateRotationY(radians + 0.1f); Assert.IsTrue(Quat.IsEquivalent(quat8a, quat8b), "CreateRotationY failed"); } //9 CreateRotationZ(float) { Quat quat9a = Quat.CreateRotationZ(radians + 0.1f); Quaternion quat9b = Quaternion.CreateRotationZ(radians + 0.1f); Assert.IsTrue(Quat.IsEquivalent(quat9a, quat9b), "CreateRotationZ failed"); } //10 CreateRotationXYZ(Angles3) { Angles3 angles = new Vec3(radians, radians, radians); Quat quat10a = Quat.CreateRotationXYZ(angles); Quaternion quat10b = Quaternion.CreateRotationXYZ(angles); Assert.IsTrue(Quat.IsEquivalent(quat10a, quat10b), "CreateRotationXYZ equivalence failed"); } //11 Slerp(Quaternion, Quaternion, float) { float timeRatio = 0.5f; Quat quat11a = Quat.CreateRotationX(radians); Quat quat11b = Quat.CreateRotationY(radians); Quat quat11c = Quat.CreateIdentity(); quat11c.SetSlerp(quat11a, quat11b, timeRatio); Quaternion quat11d = Quaternion.CreateRotationX(radians); Quaternion quat11e = Quaternion.CreateRotationY(radians); Quaternion quat11f = Quaternion.Slerp(quat11d, quat11e, timeRatio); Assert.IsTrue(Quat.IsEquivalent(quat11c, quat11f), "Slerp equivalence failed"); } //12 Lerp(Quaternion, Quaternion, float) { float timeRatio = 0.5f; Quat quat12a = Quat.CreateRotationX(radians); Quat quat12b = Quat.CreateRotationY(radians); Quat quat12c = Quat.CreateIdentity(); quat12c.SetNlerp(quat12a, quat12b, timeRatio); Quaternion quat12d = Quaternion.CreateRotationX(radians); Quaternion quat12e = Quaternion.CreateRotationY(radians); Quaternion quat12f = Quaternion.Lerp(quat12d, quat12e, timeRatio); Assert.IsTrue(Quat.IsEquivalent(quat12c, quat12f), "Lerp equivalence failed"); } //13 Properties { Matrix3x3 matrix33 = Matrix33.CreateFromVectors(right, forward, up); Quat quat13a = Quat.CreateQuatFromMatrix(matrix33); Vector3 right2 = new Vector3(right.x, right.y, right.z); Vector3 forward2 = new Vector3(forward.x, forward.y, forward.z); Vector3 up2 = new Vector3(up.x, up.y, up.z); Quaternion quat13b = Quaternion.CreateFromVectors(right2, forward2, up2); Assert.IsTrue(quat13a == quat13b, "Quaternions equality test failed"); Assert.IsTrue(Quat.IsEquivalent(quat13a, quat13b), "Quaternions equivalence test failed"); Assert.IsTrue(Vec3.IsEquivalent(right2, quat13b.Right), "Right equivalence test failed"); Assert.IsTrue(Vec3.IsEquivalent(forward2, quat13b.Forward), "Forward equivalence test failed"); Assert.IsTrue(Vec3.IsEquivalent(up2, quat13b.Up), "Up equivalence test failed"); //inversion Quat invertNative = quat13a.GetInverted(); Quaternion invertManaged = quat13b.Inverted; Assert.IsTrue(Quat.IsEquivalent(invertNative, invertManaged), "Inversion equivalence test failed"); //normalization Quat normalNative = quat13a.GetNormalized(); Quaternion normalManaged = quat13b.Normalized; Assert.IsTrue(Quat.IsEquivalent(normalNative, normalManaged), "Normalization test failed"); //length float lengthNative = quat13a.GetLength(); float lengthManaged = quat13b.Length; Assert.IsTrue(System.Math.Abs(lengthNative - lengthManaged) <= Single.Epsilon, "Length test failed"); //IsIdentity Quaternion managedIdentity = Quaternion.Identity; Assert.IsTrue(managedIdentity.IsIdentity, "Managed Identity test failed"); Quat nativeIdentity = new Quat(); nativeIdentity.SetIdentity(); Assert.IsTrue(nativeIdentity.IsIdentity(), "Native Identity test failed"); Assert.IsTrue(managedIdentity == nativeIdentity, "Identity comparison failed"); // yaw pitch roll } }
/// <summary> /// Sets the value of the uniform. /// </summary> /// <param name="value">The value.</param> /// <param name="transpose">Indicates whether to transpose the matrix value before transmitting it to the shader.</param> public unsafe void Set(ref Matrix3x4 value, bool transpose) { this.VerifyAccess(); fixed (Matrix3x4* pValue = &value) { GL.ProgramUniformMatrix3x4(this.Program, this.Location, 1, transpose, (float*)pValue); } }
/// <summary> /// Sets the value of the uniform. /// </summary> /// <remarks>The value will not be transposed.</remarks> /// <param name="value">The value.</param> public void Set(ref Matrix3x4 value) { this.Set(ref value, false); }
public bool Equals(Matrix3x4 other) { return(m00.Equals(other.m00) && m01.Equals(other.m01) && m02.Equals(other.m02) && m03.Equals(other.m03) && m10.Equals(other.m10) && m11.Equals(other.m11) && m12.Equals(other.m12) && m13.Equals(other.m13) && m20.Equals(other.m20) && m21.Equals(other.m21) && m22.Equals(other.m22) && m23.Equals(other.m23)); }
/// <summary> /// Set a float, Vector or Matrix attribute stored in a variant. /// </summary> public bool SetVectorVariant(string name, Matrix3x4 value) { Runtime.ValidateObject(this); return(XmlElement_SetVectorVariant8(handle, name, ref value)); }
internal static extern bool XmlElement_SetVectorVariant8(IntPtr handle, string name, ref Matrix3x4 value);
/// <summary> /// Set a variant attribute excluding the type. /// </summary> public bool SetVariantValue(Matrix3x4 value) { Runtime.ValidateObject(this); return(XmlElement_SetVariantValue8(handle, ref value)); }
public void SimpleSubtractionGeneratesCorrectValues() { Matrix3x4 value1 = new Matrix3x4(100); Matrix3x4 value2 = new Matrix3x4(1); Matrix3x4 result = value1 - value2; for (int y = 0; y < Matrix3x4.RowCount; y++) { for (int x = 0; x < Matrix3x4.ColumnCount; x++) { Assert.Equal(100 - 1, result[x, y], Epsilon); } } }
public void AccessorThrowsWhenOutOfBounds() { Matrix3x4 matrix3x4 = new Matrix3x4(); Assert.Throws<ArgumentOutOfRangeException>(() => { matrix3x4[-1, 0] = 0; }); Assert.Throws<ArgumentOutOfRangeException>(() => { matrix3x4[0, -1] = 0; }); Assert.Throws<ArgumentOutOfRangeException>(() => { matrix3x4[3, 0] = 0; }); Assert.Throws<ArgumentOutOfRangeException>(() => { matrix3x4[0, 4] = 0; }); }
public bool Write(Chunked model, Stream output, bool keepOpen = false) { culture.NumberFormat.NumberDecimalSeparator = "."; System.Threading.Thread.CurrentThread.CurrentCulture = culture; IChunk chunk = model.FindNextChunk("lksm").Value; if (chunk == null) { return(false); } lksm skeleton = (lksm)chunk; short[] hierarchy = (short[])skeleton.Hierarchy.Clone(); HashSet <short> weightedParNodes = new HashSet <short>(); Dictionary <int, HTLC.ClothNode> nodeMap = new Dictionary <int, HTLC.ClothNode>(); HTLC cloth = model.FindNextChunk("HTLC").Value as HTLC; if (cloth != null) { uint clothIndex = 0; foreach (HTLC.ClothNode[] nodeCollection in cloth.Nodes) { if (nodeCollection == null) { continue; } int nodeIndex = 0; foreach (HTLC.ClothNode node in nodeCollection) { int parentRaw = node.VerticalParent; if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex) && cloth.NodeBones[clothIndex].ContainsKey(parentRaw)) { // good code: if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = cloth.NodeBones[clothIndex][parentRaw]; if (cloth.NodeBones[clothIndex][parentRaw] == -1) { HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; weightedParNodes.Add(cloth.NodeBones[clothIndex][nodeIndex]); } } // else: on subskele // todo: add subskelebones? } else { if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) // if on main skele // good code: { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = -1; HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; weightedParNodes.Add(cloth.NodeBones[clothIndex][nodeIndex]); } // else: on subskele // todo: add subskelebones? } } if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) { // good code: nodeMap[cloth.NodeBones[clothIndex][nodeIndex]] = node; } nodeIndex++; } clothIndex++; } } using (StreamWriter writer = new StreamWriter(output, Encoding.Default, 512, keepOpen)) { writer.WriteLine("{0}", skeleton.Data.bonesAbs); writer.WriteLine("version 1"); writer.WriteLine("nodes"); for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.WriteLine("{0} \"bone_{1:X4}\" {2}", i, skeleton.IDs[i], hierarchy[i]); } writer.WriteLine("end"); writer.WriteLine("skeleton"); writer.WriteLine("time 0"); for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { Matrix3x4 bone = skeleton.Matrices34Inverted[i]; Quaternion3D quat = new Quaternion3D(bone[0, 3], bone[0, 0], bone[0, 1], bone[0, 2]); Vector3D rot = C3D.ToEulerAngles(quat); Vector3 scale = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); if (nodeMap.ContainsKey(i)) { HTLC.ClothNode thisNode = nodeMap[i]; if (weightedParNodes.Contains((short)i)) { Vector3 pos2 = GetGlobalPos(skeleton.Matrices34Inverted, hierarchy[i], hierarchy); pos.X = thisNode.X - pos2.X; pos.Y = thisNode.Y - pos2.Y; pos.Z = thisNode.Z - pos2.Z; } else if (nodeMap.ContainsKey(hierarchy[i])) { HTLC.ClothNode parentNode = nodeMap[hierarchy[i]]; pos.X = thisNode.X - parentNode.X; pos.Y = thisNode.Y - parentNode.Y; pos.Z = thisNode.Z - parentNode.Z; } else { pos.X = thisNode.X; pos.Y = thisNode.Y; pos.Z = thisNode.Z; } } if (rot.X == -3.14159274f && rot.Y == 0 && rot.Z == 0) { rot = new Vector3D(0, 3.14159274f, 3.14159274f); // effectively the same but you know, eulers. } writer.WriteLine(String.Format(CultureInfo.InvariantCulture, "{0} {1:0.000000} {2:0.000000} {3:0.000000} {4:0.000000} {5:0.000000} {6:0.000000} {7:0.000000} {8:0.000000} {9:0.000000}", i, pos.X, pos.Y, pos.Z, rot.X, rot.Y, rot.Z, scale.X, scale.Y, scale.Z)); } } return(true); }
internal static extern bool XmlElement_SetVariantValue8(IntPtr handle, ref Matrix3x4 value);
void RenderSplash(Matrix3x4 transform) { particleNode.SetTransform(transform); emitter.Effect = Main.Instance.ResourceCache.GetParticleEffect("Particle/Blood.xml"); emitter.Emitting = true; }
protected static void MulAddPoint3x4_XYZW(ref Vector3 result, ref Matrix3x4 mat, Vector4 vec) { result.x += mat.m00 * vec.x + mat.m01 * vec.y + mat.m02 * vec.z + mat.m03 * vec.w; result.y += mat.m10 * vec.x + mat.m11 * vec.y + mat.m12 * vec.z + mat.m13 * vec.w; result.z += mat.m20 * vec.x + mat.m21 * vec.y + mat.m22 * vec.z + mat.m23 * vec.w; }
private void DrawModelRecursive(ref Matrix4[] jointMatrix, SceneGraph curNode, BaseRenderer renderer, bool bSelectedPass) { switch (curNode.NodeType) { case J3DFormat.HierarchyDataTypes.Material: if (!bSelectedPass) { GL.BindTexture(TextureTarget.Texture2D, GetGLTexIdFromCache(curNode.DataIndex)); } break; case J3DFormat.HierarchyDataTypes.Batch: /* For each batch, we're going to enable the * appropriate Vertex Attributes for that batch * and set default values for vertex attribs that * the batch doesn't use, then draw all primitives * within it.*/ if (bSelectedPass) { #region Selected float[] front_face_wireframe_color = { 1.0f, 1.0f, 1.0f, 1.0f }; float[] back_face_wireframe_color = { 0.7f, 0.7f, 0.7f, 0.7f }; GL.LineWidth(1); GL.Enable(EnableCap.CullFace); //GL.Disable(EnableCap.DepthTest); GL.PolygonMode(MaterialFace.FrontAndBack, PolygonMode.Line); GL.EnableVertexAttribArray((int)BaseRenderer.ShaderAttributeIds.Position); // 1. Draw the back-faces with a darker color: GL.CullFace(CullFaceMode.Back); GL.VertexAttrib4((int)BaseRenderer.ShaderAttributeIds.Color, back_face_wireframe_color); foreach (var packet in _renderList[curNode.DataIndex].Packets) { int vertexIndex = 0; foreach (var primitive in packet.PrimList) { //Uhh... ushort drawIndex = packet.DrawIndexes[primitive.PosMatrixIndex[vertexIndex] / 3]; bool isWeighted = _file.Draw.IsWeighted(drawIndex); if (isWeighted) { } else { var jnt = _file.Joints.GetJoint(curNode.DataIndex); Vector3 jntRot = jnt.GetRotation().ToDegrees(); Vector3 trans = jnt.GetTranslation(); Matrix4 trnMatrix = Matrix4.CreateTranslation(trans); Matrix4 rtMatrix = Matrix4.CreateRotationX(jntRot.X) * Matrix4.CreateRotationY(jntRot.Y) * Matrix4.CreateRotationZ(jntRot.Z); Matrix4 sclMatrix = Matrix4.CreateScale(jnt.GetScale()); Matrix4 final = trnMatrix * rtMatrix * sclMatrix; //renderer.SetModelMatrix(Matrix4.Identity); renderer.SetModelMatrix(final); } GL.DrawArrays(primitive.DrawType, primitive.VertexStart, primitive.VertexCount); vertexIndex++; } } // 2. Draw the front-faces with a lighter color: GL.CullFace(CullFaceMode.Front); GL.VertexAttrib4((int)BaseRenderer.ShaderAttributeIds.Color, front_face_wireframe_color); /*foreach (var primitive in _renderList[curNode.DataIndex]) * { * GL.DrawArrays(primitive.DrawType, primitive.VertexStart, primitive.VertexCount); * }*/ GL.DisableVertexAttribArray((int)BaseRenderer.ShaderAttributeIds.Position); GL.PolygonMode(MaterialFace.FrontAndBack, PolygonMode.Fill); //GL.Enable(EnableCap.DepthTest); GL.LineWidth(1); #endregion } else { SetVertexAttribArraysForBatch(true, curNode.DataIndex); //GL.CullFace(CullFaceMode.Front); for (int packetIndex = 0; packetIndex < _renderList[curNode.DataIndex].Packets.Count; packetIndex++) { RenderPacket packet = _renderList[curNode.DataIndex].Packets[packetIndex]; foreach (var primitive in packet.PrimList) { renderer.SetModelMatrix(Matrix4.Identity); if (primitive.PosMatrixIndex.Count > 0) { var transformedData = new J3DRenderer.VertexFormatLayout[primitive.VertexCount]; //For each vertex within this primitive, we're going to get its id. for (int vertexIndex = 0; vertexIndex < primitive.VertexCount; vertexIndex++) { ushort vertIndex = primitive.PosMatrixIndex[vertexIndex]; ushort drawIndex = packet.DrawIndexes[vertIndex / 3]; //ehh int seriously = 0; while (drawIndex == 0xFFFF) { RenderPacket prevPacket = _renderList[curNode.DataIndex].Packets[packetIndex - seriously]; drawIndex = prevPacket.DrawIndexes[vertIndex / 3]; seriously++; } bool isWeighted = _file.Draw.IsWeighted(drawIndex); if (isWeighted) { ushort numBonesAffecting = _file.Envelopes.GetCount(_file.Draw.GetIndex(drawIndex)); //Much WTFs ushort offset = 0; for (ushort i = 0; i < _file.Draw.GetIndex(drawIndex); i++) { offset += _file.Envelopes.GetCount(i); } offset *= 2; Matrix4 finalTransform = Matrix4.Identity; for (ushort i = 0; i < numBonesAffecting; i++) { ushort boneIndex = _file.Envelopes.GetIndexOffset((ushort)(offset + (i * 0x2))); float boneWeight = _file.Envelopes.GetWeight((ushort)((offset / 2) + i)); Matrix3x4 envMatrix = _file.Envelopes.GetMatrix(boneIndex); Matrix4 newEnvelopeMtx = new Matrix4(envMatrix.Row0, envMatrix.Row1, envMatrix.Row2, new Vector4(0, 0, 0, 1)); SkeletonJoint joint = _skeleCopy[boneIndex]; //Matrix4 transMatrix = Matrix4.CreateTranslation(joint.Position); //We need to use the bone's matrix from EVP1 to get the joint matrix Matrix4 jointMtx = joint.Rotation * newEnvelopeMtx; //finalTransform = Matrix4.Mult(jointMtx * newEnvelopeMtx, boneWeight) * finalTransform; //ToDo: This is the wrong scale. //AddScaleMatrix(ref finalTransform, Matrix4.Mult(jointMtx, newEnvelopeMtx), 1f); } transformedData[vertexIndex] = _vertDataBind[primitive.VertexStart + vertexIndex]; Vector3 vertPosition = transformedData[vertexIndex].Position; transformedData[vertexIndex].Position = Vector3.TransformPosition(vertPosition, finalTransform); } else { //If the vertex is not weighted, we're just going to use the position //from the bone matrix. Something like this. Vector3 vertPosition = _vertDataBind[primitive.VertexStart + vertexIndex].Position; transformedData[vertexIndex] = _vertDataBind[primitive.VertexStart + vertexIndex]; SkeletonJoint joint = _skeleCopy[_file.Draw.GetIndex(drawIndex)]; Matrix4 transMatrix = Matrix4.CreateTranslation(joint.Position); Matrix4 final = joint.Rotation * transMatrix; transformedData[vertexIndex].Position = Vector3.TransformPosition(vertPosition, final); } } //Re-upload the subsection to the buffer. GL.BindBuffer(BufferTarget.ArrayBuffer, _glVbo); GL.BufferSubData(BufferTarget.ArrayBuffer, (IntPtr)(primitive.VertexStart * (9 * 4)), (IntPtr)(primitive.VertexCount * (9 * 4)), transformedData); float[] front_face_wireframe_color = { 1.0f, 1.0f, 1.0f, 1.0f }; GL.VertexAttrib4((int)BaseRenderer.ShaderAttributeIds.Color, front_face_wireframe_color); } GL.DrawArrays(primitive.DrawType, primitive.VertexStart, primitive.VertexCount); } } SetVertexAttribArraysForBatch(false, curNode.DataIndex); } break; } foreach (SceneGraph subNode in curNode.Children) { DrawModelRecursive(ref jointMatrix, subNode, renderer, bSelectedPass); } }
public void SetMatrix3x4(StringHash key, Matrix3x4 value) { Urho3D_Object_Event_SetMatrix3x4(_map, key.Hash, ref value); }
public void MuliplyByMatrix3x1ProducesMatrix3x4() { Matrix1x4 matrix1 = new Matrix1x4(3); Matrix3x1 matrix2 = new Matrix3x1(2); Matrix3x4 result = matrix1 * matrix2; Matrix3x4 expected = new Matrix3x4(6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6); Assert.AreEqual(expected, result); }
public void MuliplyByMatrix3x2ProducesMatrix3x4() { Matrix2x4 matrix1 = new Matrix2x4(3); Matrix3x2 matrix2 = new Matrix3x2(2); Matrix3x4 result = matrix1 * matrix2; Matrix3x4 expected = new Matrix3x4(12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12); Assert.Equal(expected, result); }
private static void Set(int location, Matrix3x4 value) => GL.UniformMatrix3x4(location, false, ref value);
internal override void UpdateTransform(CommandBuffer updateCB, bool starting) { if (!m_initialized) { Initialize(); return; } Profiler.BeginSample("Skinned.Update"); if (!starting && m_wasVisible) { m_prevLocalToWorld = m_currLocalToWorld; } bool isVisible = m_renderer.isVisible; if (!m_error && (isVisible || starting)) { UpdateBones(); m_starting = !m_wasVisible || starting; if (!m_useFallback) { if (!m_useGPU) { m_asyncUpdateSignal.Reset(); m_asyncUpdateTriggered = true; m_owner.Instance.WorkerPool.EnqueueAsyncUpdate(this); } else { UpdateVerticesGPU(updateCB, m_starting); } } else { UpdateVerticesFallback(m_starting); } } if (!m_useFallback) { m_currLocalToWorld = m_transform.localToWorldMatrix; } else { #if UNITY_2017_1_OR_NEWER m_currLocalToWorld = m_transform.localToWorldMatrix; #else m_currLocalToWorld = Matrix4x4.TRS(m_transform.position, m_transform.rotation, Vector3.one); #endif } if (starting || !m_wasVisible) { m_prevLocalToWorld = m_currLocalToWorld; } m_wasVisible = isVisible; Profiler.EndSample(); }
private static Envelopes LoadEVP1FromStream(EndianBinaryReader reader, long chunkStart) { Envelopes envelopes = new Envelopes(); ushort numEnvelopes = reader.ReadUInt16(); reader.ReadUInt16(); // Padding // numEnvelope many uint8 - each one describes how many bones belong to this index. uint boneCountOffset = reader.ReadUInt32(); // "sum over all bytes in boneCountOffset many shorts (index into some joint stuff? into matrix table?)" uint indexDataOffset = reader.ReadUInt32(); // Bone Weights (as many floats here as there are ushorts at indexDataOffset) uint weightOffset = reader.ReadUInt32(); // Matrix Table (3x4 float array) - Inverse Bind Pose uint boneMatrixOffset = reader.ReadUInt32(); // - Is this the number of bones which influence the vert? reader.BaseStream.Position = chunkStart + boneCountOffset; for (int b = 0; b < numEnvelopes; b++) { envelopes.numBonesAffecting.Add(reader.ReadByte()); } // ??? reader.BaseStream.Position = chunkStart + indexDataOffset; for (int m = 0; m < envelopes.numBonesAffecting.Count; m++) { for (int j = 0; j < envelopes.numBonesAffecting[m]; j++) { envelopes.indexRemap.Add(reader.ReadUInt16()); } } // Bone Weights reader.BaseStream.Position = chunkStart + weightOffset; for (int w = 0; w < envelopes.numBonesAffecting.Count; w++) { for (int j = 0; j < envelopes.numBonesAffecting[w]; j++) { envelopes.weights.Add(reader.ReadSingle()); } } // Inverse Bind Pose Matrices reader.BaseStream.Position = chunkStart + boneMatrixOffset; for (int w = 0; w < numEnvelopes; w++) { Matrix3x4 matrix = new Matrix3x4(); for (int j = 0; j < 3; j++) { for (int k = 0; k < 4; k++) { matrix[j, k] = reader.ReadSingle(); } } envelopes.inverseBindPose.Add(matrix); } return(envelopes); }
public void MemberGetAndSetValuesCorrectly() { Matrix3x4 matrix3x4 = new Matrix3x4(); matrix3x4.M11 = 0; matrix3x4.M21 = 1; matrix3x4.M31 = 2; matrix3x4.M12 = 3; matrix3x4.M22 = 4; matrix3x4.M32 = 5; matrix3x4.M13 = 6; matrix3x4.M23 = 7; matrix3x4.M33 = 8; matrix3x4.M14 = 9; matrix3x4.M24 = 10; matrix3x4.M34 = 11; Assert.Equal(0, matrix3x4.M11, Epsilon); Assert.Equal(1, matrix3x4.M21, Epsilon); Assert.Equal(2, matrix3x4.M31, Epsilon); Assert.Equal(3, matrix3x4.M12, Epsilon); Assert.Equal(4, matrix3x4.M22, Epsilon); Assert.Equal(5, matrix3x4.M32, Epsilon); Assert.Equal(6, matrix3x4.M13, Epsilon); Assert.Equal(7, matrix3x4.M23, Epsilon); Assert.Equal(8, matrix3x4.M33, Epsilon); Assert.Equal(9, matrix3x4.M14, Epsilon); Assert.Equal(10, matrix3x4.M24, Epsilon); Assert.Equal(11, matrix3x4.M34, Epsilon); Assert.Equal(matrix3x4[0, 0], matrix3x4.M11, Epsilon); Assert.Equal(matrix3x4[1, 0], matrix3x4.M21, Epsilon); Assert.Equal(matrix3x4[2, 0], matrix3x4.M31, Epsilon); Assert.Equal(matrix3x4[0, 1], matrix3x4.M12, Epsilon); Assert.Equal(matrix3x4[1, 1], matrix3x4.M22, Epsilon); Assert.Equal(matrix3x4[2, 1], matrix3x4.M32, Epsilon); Assert.Equal(matrix3x4[0, 2], matrix3x4.M13, Epsilon); Assert.Equal(matrix3x4[1, 2], matrix3x4.M23, Epsilon); Assert.Equal(matrix3x4[2, 2], matrix3x4.M33, Epsilon); Assert.Equal(matrix3x4[0, 3], matrix3x4.M14, Epsilon); Assert.Equal(matrix3x4[1, 3], matrix3x4.M24, Epsilon); Assert.Equal(matrix3x4[2, 3], matrix3x4.M34, Epsilon); }
bool _SolveThumb( ref Matrix3x4 parentTransform ) { _FingerBranch fingerBranch = _fingerBranches[(int)FingerType.Thumb]; if( fingerBranch == null || fingerBranch.fingerLinks.Length != 3 ) { return false; } _FingerLink fingerLink0 = fingerBranch.fingerLinks[0]; _FingerLink fingerLink1 = fingerBranch.fingerLinks[1]; _FingerLink fingerLink2 = fingerBranch.fingerLinks[2]; _ThumbLink thumbLink0 = _thumbBranch.thumbLinks[0]; _ThumbLink thumbLink1 = _thumbBranch.thumbLinks[1]; _ThumbLink thumbLink2 = _thumbBranch.thumbLinks[2]; bool isRight = (_fingerIKType == FingerIKType.RightWrist); { Vector3 fingerLinkPosition0 = parentTransform * (fingerLink0.bone._defaultPosition - _parentBone._defaultPosition); var endEffector = fingerBranch.effector; Vector3 effectorPosition = _GetEffectorPosition( _internalValues, _parentBone, fingerLink0.bone, endEffector, fingerBranch.link0ToEffectorLength, ref parentTransform ); Vector3 effectorTranslate = effectorPosition - fingerLinkPosition0; float effectorLength = effectorTranslate.magnitude; if( effectorLength < IKEpsilon || fingerBranch.link0ToEffectorLength < IKEpsilon ) { return false; } Vector3 effectorDirection = effectorTranslate * (1.0f / effectorLength); if( effectorLength > fingerBranch.link0ToEffectorLength ) { effectorLength = fingerBranch.link0ToEffectorLength; effectorTranslate = effectorDirection * fingerBranch.link0ToEffectorLength; effectorPosition = fingerLinkPosition0 + effectorTranslate; } { // thumb0 (1st pass.) // Simply, compute direction thumb0 to effector. Vector3 dirX = effectorDirection; // Limit yaw pitch for thumb0 to effector. if( _thumbBranch.thumb0_isLimited ) { Matrix3x3 beginToEndBasis; SAFBIKMatMult( out beginToEndBasis, ref parentTransform.basis, ref fingerBranch.boneToSolvedBasis ); Vector3 localEffectorDirection; SAFBIKMatMultVecInv( out localEffectorDirection, ref beginToEndBasis, ref dirX ); if( _LimitYZ( isRight, ref localEffectorDirection, _thumbBranch.thumb0_lowerLimit, _thumbBranch.thumb0_upperLimit, _thumbBranch.thumb0_innerLimit, _thumbBranch.thumb0_outerLimit ) ) { SAFBIKMatMultVec( out dirX, ref beginToEndBasis, ref localEffectorDirection ); // Local to world. } } Vector3 dirY; SAFBIKMatMultVec( out dirY, ref parentTransform.basis, ref thumbLink0.thumb_boneToSolvedBasis.column1 ); if( !SAFBIKComputeBasisFromXYLockX( out fingerLink0.boneTransform.basis, isRight ? dirX : -dirX, dirY ) ) { return false; } fingerLink0.boneTransform.origin = fingerLinkPosition0; SAFBIKMatMultRet0( ref fingerLink0.boneTransform.basis, ref thumbLink0.thumb_solvedToBoneBasis ); } // thumb0 / Limit length based thumb1/2 (Type3) { Vector3 fingerLinkPosition1 = fingerLink0.boneTransform * (fingerLink1.bone._defaultPosition - fingerLink0.bone._defaultPosition); Vector3 effectorTranslate1to3 = effectorPosition - fingerLinkPosition1; float effectorLength1to3 = effectorTranslate1to3.magnitude; if( effectorLength1to3 < _thumbBranch.linkLength1to3 - IKEpsilon ) { Vector3 effectorTranslate0to3 = effectorPosition - fingerLink0.boneTransform.origin; float effectorLength0to3Sq; float effectorLength0to3 = SAFBIKVecLengthAndLengthSq( out effectorLength0to3Sq, ref effectorTranslate0to3 ); float baseTheta = 1.0f; if( effectorLength0to3 > IKEpsilon ) { Vector3 baseDirection0to1 = fingerLinkPosition1 - fingerLink0.boneTransform.origin; if( SAFBIKVecNormalize( ref baseDirection0to1 ) ) { Vector3 effectorDirection0to3 = effectorTranslate0to3 * (1.0f / effectorLength0to3); baseTheta = Vector3.Dot( effectorDirection0to3, baseDirection0to1 ); } } float moveLenA = _thumbBranch.linkLength0to1; float moveLenASq = _thumbBranch.linkLength0to1Sq; float moveLenB = effectorLength0to3; float moveLenBSq = effectorLength0to3Sq; float moveLenC = effectorLength1to3 + (_thumbBranch.linkLength1to3 - effectorLength1to3) * 0.5f; // 0.5f = Magic number.(Balancer) float moveLenCSq = moveLenC * moveLenC; float moveTheta = _ComputeTriangleTheta( moveLenA, moveLenB, moveLenC, moveLenASq, moveLenBSq, moveLenCSq ); if( moveTheta < baseTheta ) { float newAngle = SAFBIKAcos( moveTheta ) - SAFBIKAcos( baseTheta ); if( newAngle > 0.01f * Mathf.Deg2Rad ) { // moveLenAtoAD = Move length thumb1 origin with bending thumb0. float moveLenASq2 = moveLenASq * 2.0f; float moveLenAtoAD = SAFBIKSqrt( moveLenASq2 * (1.0f - SAFBIKCos( newAngle )) ); if( moveLenAtoAD > IKEpsilon ) { Vector3 solveDirection; SAFBIKMatMultVec( out solveDirection, ref fingerLink0.boneTransform.basis, ref _thumbBranch.thumbSolveZ ); fingerLinkPosition1 += solveDirection * moveLenAtoAD; Vector3 newX = fingerLinkPosition1 - fingerLink0.boneTransform.origin; if( SAFBIKVecNormalize( ref newX ) ) { Vector3 dirY; SAFBIKMatMultVec( out dirY, ref fingerLink0.boneTransform.basis, ref fingerLink0.boneToSolvedBasis.column1 ); Matrix3x3 solveBasis0; if( SAFBIKComputeBasisFromXYLockX( out solveBasis0, isRight ? newX : -newX, dirY ) ) { SAFBIKMatMult( out fingerLink0.boneTransform.basis, ref solveBasis0, ref fingerLink0.solvedToBoneBasis ); } } } } } } } { // thumb1 { Vector3 fingerLinkPosition1 = fingerLink0.boneTransform * (fingerLink1.bone._defaultPosition - fingerLink0.bone._defaultPosition); // Simply, compute direction thumb1 to effector. // (Compute push direction for thumb1.) Vector3 dirX = effectorPosition - fingerLinkPosition1; if( !SAFBIKVecNormalize( ref dirX ) ) { return false; } Vector3 dirY; SAFBIKMatMultVec( out dirY, ref fingerLink0.boneTransform.basis, ref thumbLink1.thumb_boneToSolvedBasis.column1 ); if( !SAFBIKComputeBasisFromXYLockX( out fingerLink1.boneTransform.basis, isRight ? dirX : -dirX, dirY ) ) { return false; } fingerLink1.boneTransform.origin = fingerLinkPosition1; SAFBIKMatMultRet0( ref fingerLink1.boneTransform.basis, ref thumbLink1.thumb_solvedToBoneBasis ); } Vector3 effectorTranslate1to3 = effectorPosition - fingerLink1.boneTransform.origin; float effectorLength1to3Sq = effectorTranslate1to3.sqrMagnitude; float effectorLength1to3 = SAFBIKSqrt( effectorLength1to3Sq ); float moveLenA = _thumbBranch.linkLength1to2; float moveLenASq = _thumbBranch.linkLength1to2Sq; float moveLenB = effectorLength1to3; float moveLenBSq = effectorLength1to3Sq; float moveLenC = _thumbBranch.linkLength2to3; float moveLenCSq = _thumbBranch.linkLength2to3Sq; // Compute angle moved A/B origin. float moveThetaAtoB = _ComputeTriangleTheta( moveLenA, moveLenB, moveLenC, moveLenASq, moveLenBSq, moveLenCSq ); if( moveThetaAtoB < _thumbBranch.thumb1_baseThetaAtoB ) { float newAngle = SAFBIKAcos( moveThetaAtoB ) - _thumbBranch.thumb1_Acos_baseThetaAtoB; if( newAngle > 0.01f * Mathf.Deg2Rad ) { float moveLenASq2 = moveLenASq * 2.0f; float moveLenAtoAD = SAFBIKSqrt( moveLenASq2 - moveLenASq2 * SAFBIKCos( newAngle ) ); { Vector3 solveDirection; SAFBIKMatMultVec( out solveDirection, ref fingerLink1.boneTransform.basis, ref _thumbBranch.thumbSolveZ ); Vector3 fingerLinkPosition2 = fingerLink1.boneTransform * (fingerLink2.bone._defaultPosition - fingerLink1.bone._defaultPosition); fingerLinkPosition2 += solveDirection * moveLenAtoAD; Vector3 newX = fingerLinkPosition2 - fingerLink1.boneTransform.origin; if( SAFBIKVecNormalize( ref newX ) ) { Vector3 dirY; SAFBIKMatMultVec( out dirY, ref fingerLink1.boneTransform.basis, ref fingerLink1.boneToSolvedBasis.column1 ); Matrix3x3 solveBasis1; if( SAFBIKComputeBasisFromXYLockX( out solveBasis1, isRight ? newX : -newX, dirY ) ) { SAFBIKMatMult( out fingerLink1.boneTransform.basis, ref solveBasis1, ref fingerLink1.solvedToBoneBasis ); } } } } } } { // thumb2 // Simply, compute direction thumb2 to effector. Vector3 fingerLinkPosition2 = fingerLink1.boneTransform * (fingerLink2.bone._defaultPosition - fingerLink1.bone._defaultPosition); Vector3 dirX = effectorPosition - fingerLinkPosition2; if( !SAFBIKVecNormalize( ref dirX ) ) { return false; } Vector3 dirY; SAFBIKMatMultVec( out dirY, ref fingerLink1.boneTransform.basis, ref thumbLink2.thumb_boneToSolvedBasis.column1 ); if( !SAFBIKComputeBasisFromXYLockX( out fingerLink2.boneTransform.basis, isRight ? dirX : -dirX, dirY ) ) { return false; } fingerLink2.boneTransform.origin = fingerLinkPosition2; SAFBIKMatMultRet0( ref fingerLink2.boneTransform.basis, ref thumbLink2.thumb_solvedToBoneBasis ); } } Quaternion worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref fingerLink0.boneTransform.basis, ref fingerLink0.bone._defaultBasis ); fingerLink0.bone.worldRotation = worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref fingerLink1.boneTransform.basis, ref fingerLink1.bone._defaultBasis ); fingerLink1.bone.worldRotation = worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref fingerLink2.boneTransform.basis, ref fingerLink2.bone._defaultBasis ); fingerLink2.bone.worldRotation = worldRotation; return true; }
public void HashCodeGenerationWorksCorrectly() { HashSet<int> hashCodes = new HashSet<int>(); Matrix3x4 value = new Matrix3x4(1); for (int i = 2; i <= 100; i++) { Assert.True(hashCodes.Add(value.GetHashCode()), "Unique hash code generation failure."); value *= i; } }
static Vector3 _GetEffectorPosition( InternalValues internalValues, Bone rootBone, Bone beginLinkBone, Effector effector, float link0ToEffectorLength, ref Matrix3x4 parentTransform ) { if( rootBone != null && beginLinkBone != null && effector != null ) { var effectorPosition = effector.worldPosition; if( effector.positionWeight < 1.0f - IKEpsilon ) { Vector3 endLinkPosition; if( internalValues.continuousSolverEnabled || internalValues.resetTransforms ) { endLinkPosition = parentTransform * (effector._defaultPosition - rootBone._defaultPosition); } else { endLinkPosition = effector.bone_worldPosition; } Vector3 beginLinkPosition = parentTransform * (beginLinkBone._defaultPosition - rootBone._defaultPosition); Vector3 moveFrom = endLinkPosition - beginLinkPosition; Vector3 moveTo = effectorPosition - beginLinkPosition; float lengthFrom = link0ToEffectorLength; // Optimized. float lengthTo = moveTo.magnitude; if( lengthFrom > IKEpsilon && lengthTo > IKEpsilon ) { Vector3 dirFrom = moveFrom * (1.0f / lengthFrom); Vector3 dirTo = moveTo * (1.0f / lengthTo); Vector3 dir = _LerpDir( ref dirFrom, ref dirTo, effector.positionWeight ); float len = Mathf.Lerp( lengthFrom, lengthTo, Mathf.Clamp01( 1.0f - (1.0f - effector.positionWeight) * _positionLerpRate ) ); return dir * len + beginLinkPosition; } } return effectorPosition; } return Vector3.zero; }
public void EqualityOperatorWorksCorrectly() { Matrix3x4 value1 = new Matrix3x4(100); Matrix3x4 value2 = new Matrix3x4(50) * 2; Assert.Equal(value1, value2); Assert.True(value1 == value2, "Equality operator failed."); }
Vector3 Solve3VarLinearEquation(Matrix3x4 coefficients) { if (closeEnough(coefficients.a1, 0)) { coefficients.a1 = 0; } if (closeEnough(coefficients.a2, 0)) { coefficients.a2 = 0; } if (closeEnough(coefficients.a3, 0)) { coefficients.a3 = 0; } if (closeEnough(coefficients.a4, 0)) { coefficients.a4 = 0; } if (closeEnough(coefficients.b1, 0)) { coefficients.b1 = 0; } if (closeEnough(coefficients.b2, 0)) { coefficients.b2 = 0; } if (closeEnough(coefficients.b3, 0)) { coefficients.b3 = 0; } if (closeEnough(coefficients.b4, 0)) { coefficients.b4 = 0; } if (closeEnough(coefficients.c1, 0)) { coefficients.c1 = 0; } if (closeEnough(coefficients.c2, 0)) { coefficients.c2 = 0; } if (closeEnough(coefficients.c3, 0)) { coefficients.c3 = 0; } if (closeEnough(coefficients.c4, 0)) { coefficients.c4 = 0; } if (closeEnough(coefficients.a1, 0)) { if (closeEnough(coefficients.b1, 0)) { coefficients.SwapRows(0, 2); } else if (closeEnough(coefficients.c1, 0)) { coefficients.SwapRows(0, 1); } } if (!closeEnough(coefficients.a1, 0)) { if (!closeEnough(coefficients.b1, 0)) { coefficients.AddRows(0, 1, -coefficients.b1 / coefficients.a1); } if (!closeEnough(coefficients.c1, 0)) { coefficients.AddRows(0, 2, -coefficients.c1 / coefficients.a1); } } if (closeEnough(coefficients.b2, 0)) { coefficients.SwapRows(1, 2); } if (!closeEnough(coefficients.b2, 0)) { if (!closeEnough(coefficients.c2, 0)) { coefficients.AddRows(1, 2, -coefficients.c2 / coefficients.b2); } } if (!closeEnough(coefficients.a1, 0, 0.00001f)) { coefficients.ScaleRow(0, 1 / coefficients.a1); } if (!closeEnough(coefficients.b2, 0, 0.00001f)) { coefficients.ScaleRow(1, 1 / coefficients.b2); } if (!closeEnough(coefficients.c3, 0, 0.00001f)) { coefficients.ScaleRow(2, 1 / coefficients.c3); } var res = new Vector3(); res.Z = coefficients.c4; res.Y = coefficients.b4 - (res.Z * coefficients.b3); res.X = coefficients.a4 - (res.Y * coefficients.a2) - (res.Z * coefficients.a3); return(res); }
public void MuliplyByMatrix4x3ProducesMatrix4x4() { Matrix3x4 matrix1 = new Matrix3x4(3); Matrix4x3 matrix2 = new Matrix4x3(2); Matrix4x4 result = matrix1 * matrix2; Matrix4x4 expected = new Matrix4x4(18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18); Assert.Equal(expected, result); }
/// <summary> /// Writes a <see cref="Matrix3x4"/> instance into the current stream. /// </summary> /// <param name="self">The extended <see cref="BinaryDataWriter"/>.</param> /// <param name="value">The <see cref="Matrix3x4"/> instance.</param> public static void Write(this BinaryDataWriter self, Matrix3x4 value) { self.Write(value.M11); self.Write(value.M12); self.Write(value.M13); self.Write(value.M14); self.Write(value.M21); self.Write(value.M22); self.Write(value.M23); self.Write(value.M24); self.Write(value.M31); self.Write(value.M32); self.Write(value.M33); self.Write(value.M34); }
bool _SolveNotThumb( int fingerType, ref Matrix3x4 parentTransform ) { _FingerBranch fingerBranch = _fingerBranches[fingerType]; if( fingerBranch == null || fingerBranch.fingerLinks.Length != 3 ) { return false; } bool isRight = (_fingerIKType == FingerIKType.RightWrist); _FingerLink beginLink = fingerBranch.fingerLinks[0]; _FingerLink bendingLink0 = fingerBranch.fingerLinks[1]; _FingerLink bendingLink1 = fingerBranch.fingerLinks[2]; Effector endEffector = fingerBranch.effector; float linkLength0 = beginLink.childToLength; float linkLength1 = bendingLink0.childToLength; float linkLength1Sq = bendingLink0.childToLengthSq; float linkLength2 = bendingLink1.childToLength; float linkLength2Sq = bendingLink1.childToLengthSq; float baseLength = fingerBranch.link0ToEffectorLength; Vector3 beginLinkPosition = parentTransform * (beginLink.bone._defaultPosition - _parentBone._defaultPosition); Vector3 effectorPosition = _GetEffectorPosition( _internalValues, _parentBone, beginLink.bone, endEffector, fingerBranch.link0ToEffectorLength, ref parentTransform ); Vector3 effectorTranslate = effectorPosition - beginLinkPosition; float effectorLength = effectorTranslate.magnitude; if( effectorLength <= IKEpsilon || baseLength <= IKEpsilon ) { return false; } Vector3 effectorDirection = effectorTranslate * (1.0f / effectorLength); bool isWarp = isRight ? (fingerBranch.notThumb1BaseAngle.angle <= IKEpsilon) : (fingerBranch.notThumb1BaseAngle.angle >= -IKEpsilon); { float maxLength = isWarp ? baseLength : (linkLength0 + linkLength1 + linkLength2); if( effectorLength > maxLength ) { effectorLength = maxLength; effectorTranslate = effectorDirection * effectorLength; effectorPosition = beginLinkPosition + effectorTranslate; } else if( effectorLength < linkLength1 ) { effectorLength = linkLength1; effectorTranslate = effectorDirection * effectorLength; effectorPosition = beginLinkPosition + effectorTranslate; } } bool isUpper = false; { Matrix3x3 beginToEndBasis; SAFBIKMatMult( out beginToEndBasis, ref parentTransform.basis, ref fingerBranch.boneToSolvedBasis ); Vector3 localEffectorDirection; SAFBIKMatMultVecInv( out localEffectorDirection, ref beginToEndBasis, ref effectorDirection ); isUpper = (localEffectorDirection.y >= 0.0f); if( _LimitFingerNotThumb( isRight, ref localEffectorDirection, ref _notThumbPitchUThetaLimit, ref _notThumbPitchLThetaLimit, ref _notThumbYawThetaLimit ) ) { SAFBIKMatMultVec( out effectorDirection, ref beginToEndBasis, ref localEffectorDirection ); effectorTranslate = effectorDirection * effectorLength; effectorPosition = beginLinkPosition + effectorTranslate; } } Vector3 solveDirY = Vector3.zero; Vector3 solveDirZ = Vector3.zero; if( !_SolveInDirect( isRight, ref solveDirY, ref solveDirZ, ref parentTransform.basis, ref beginLink.boneToSolvedBasis, ref effectorDirection ) ) { return false; } bool solveFingerIK = !isWarp; if( isWarp ) { bool imm_isUpper = false; float imm_traceRate = 0.0f; Vector3 bendingLink0Position = parentTransform * (bendingLink0.bone._defaultPosition - _parentBone._defaultPosition); Vector3 bendingLink1Position = parentTransform * (bendingLink1.bone._defaultPosition - _parentBone._defaultPosition); Vector3 endPosition = parentTransform * (endEffector._defaultPosition - _parentBone._defaultPosition); Vector3 beginLinkDirX = Vector3.zero; { Vector3 beginLinkToBendingLink0Direction = bendingLink0Position - beginLinkPosition; Vector3 beginLinkToEndDirection = endPosition - beginLinkPosition; if( SAFBIKVecNormalize2( ref beginLinkToBendingLink0Direction, ref beginLinkToEndDirection ) ) { Matrix3x3 effBasis; if( SAFBIKComputeBasisFromXZLockX( out effBasis, isRight ? effectorDirection : -effectorDirection, solveDirZ ) ) { Matrix3x3 bendBasis; Matrix3x3 endBasis; if( SAFBIKComputeBasisFromXZLockZ( out bendBasis, isRight ? beginLinkToBendingLink0Direction : -beginLinkToBendingLink0Direction, effBasis.column2 ) && SAFBIKComputeBasisFromXZLockZ( out endBasis, isRight ? beginLinkToEndDirection : -beginLinkToEndDirection, effBasis.column2 ) ) { // effBasis ... beginLink to current effector basis. // bendBasis ... beginLink to default bendLink0 basis. // endBasis ... beginLink to default effector basis. Vector3 effX = isRight ? effBasis.column0 : -effBasis.column0; Vector3 effY = effBasis.column1; Vector3 effZ = effBasis.column2; Vector3 bendX = isRight ? bendBasis.column0 : -bendBasis.column0; Vector3 bendY = bendBasis.column1; Vector3 endX = isRight ? endBasis.column0 : -endBasis.column0; Vector3 endY = endBasis.column1; // rotBendX ... begin to current bendLink0 basis. float endBendDotX = Vector3.Dot( bendX, endX ); // Cosine float endBendDotY = Vector3.Dot( bendX, endY ); // Sine Vector3 rotBendX = _Rotate( ref effX, ref effY, endBendDotX, endBendDotY ); imm_isUpper = (Vector3.Dot( endY, effX ) >= 0.0f); bool imm_isLimitL = false; float endEffDotX = Vector3.Dot( endX, effX ); if( imm_isUpper ) { if( isWarp ) { float traceLimitUAngle = _notThumb1PitchUTraceSmooth.angle; float cosTraceLimitUAngle = _notThumb1PitchUTraceSmooth.cos; if( traceLimitUAngle <= IKEpsilon || endEffDotX < cosTraceLimitUAngle ) { Vector3 rotBendY = Vector3.Cross( effZ, rotBendX ); if( SAFBIKVecNormalize( ref rotBendY ) ) { float cosTraceAngle = _notThumb1PitchUTrace.cos; float sinTraceAngle = _notThumb1PitchUTrace.sin; beginLinkDirX = _Rotate( ref rotBendX, ref rotBendY, cosTraceAngle, isRight ? -sinTraceAngle : sinTraceAngle ); } } else { float r = SAFBIKAcos( endEffDotX ); r = r / traceLimitUAngle; r = _notThumb1PitchUTrace.angle * r; beginLinkDirX = _Rotate( ref bendX, ref bendY, r ); } } else { solveFingerIK = true; } } else { if( isWarp ) { float baseAngle = Mathf.Abs( fingerBranch.notThumb1BaseAngle.angle ); float traceAngle = Mathf.Max( baseAngle, _notThumb1PitchLTrace.angle ); float cosTraceAngle = Mathf.Min( fingerBranch.notThumb1BaseAngle.cos, _notThumb1PitchLTrace.cos ); if( endEffDotX < cosTraceAngle ) { solveFingerIK = true; float smoothLen = linkLength2 * 0.25f; if( effectorLength >= baseLength - (smoothLen) ) { _LerpEffectorLength( ref effectorLength, ref effectorDirection, ref effectorTranslate, ref effectorPosition, ref beginLinkPosition, baseLength - smoothLen, linkLength0 + linkLength1 + linkLength2, smoothLen ); } else { // Nothing. } } else { if( traceAngle <= IKEpsilon || traceAngle == baseAngle ) { beginLinkDirX = bendX; if( traceAngle <= IKEpsilon ) { imm_traceRate = 1.0f; } else { float r = SAFBIKAcos( endEffDotX ); imm_traceRate = r / traceAngle; } } else { float r = SAFBIKAcos( endEffDotX ); r = r / traceAngle; imm_traceRate = r; r = (_notThumb1PitchLTrace.angle - baseAngle) * r; beginLinkDirX = _Rotate( ref bendX, ref bendY, -r ); } } } else { solveFingerIK = true; } } if( isWarp ) { if( !solveFingerIK ) { if( effectorLength < baseLength - IKEpsilon ) { float extendLen = 0.0f; if( !imm_isLimitL ) { extendLen = Vector3.Dot( beginLinkDirX, effX ); extendLen = SAFBIKSqrt( 1.0f - extendLen * extendLen ); // Cosine to Sine extendLen *= linkLength0; // Sine Length } float smoothLen = linkLength2 * 0.25f; if( extendLen > IKEpsilon && effectorLength >= baseLength - extendLen ) { float r = 1.0f - (effectorLength - (baseLength - extendLen)) / extendLen; beginLinkDirX = _FastLerpDir( ref beginLinkDirX, ref effX, r ); imm_traceRate += (1.0f - imm_traceRate) * r; } else { solveFingerIK = true; if( effectorLength >= baseLength - (extendLen + smoothLen) ) { _LerpEffectorLength( ref effectorLength, ref effectorDirection, ref effectorTranslate, ref effectorPosition, ref beginLinkPosition, baseLength - (extendLen + smoothLen), linkLength0 + linkLength1 + linkLength2, smoothLen ); } else { // Nothing. } } } } } } } } } if( !solveFingerIK ) { if( beginLinkDirX == Vector3.zero ) { return false; } if( !SAFBIKComputeBasisFromXZLockX( out beginLink.boneTransform.basis, isRight ? beginLinkDirX : -beginLinkDirX, solveDirZ ) ) { return false; } beginLink.boneTransform.origin = beginLinkPosition; SAFBIKMatMultRet0( ref beginLink.boneTransform.basis, ref beginLink.solvedToBoneBasis ); bendingLink0Position = beginLink.boneTransform * (bendingLink0.bone._defaultPosition - beginLink.bone._defaultPosition); bendingLink1Position = beginLink.boneTransform * (bendingLink1.bone._defaultPosition - beginLink.bone._defaultPosition); endPosition = beginLink.boneTransform * (endEffector._defaultPosition - beginLink.bone._defaultPosition); Vector3 basedEffectorPosition = beginLinkPosition + effectorDirection * baseLength; Vector3 bendingLink0ToEffectorDirection = basedEffectorPosition - bendingLink0Position; Vector3 bendingLink0ToBendingLink0Direction = bendingLink1Position - bendingLink0Position; Vector3 bendingLink0ToEndDirection = endPosition - bendingLink0Position; if( !SAFBIKVecNormalize3( ref bendingLink0ToEffectorDirection, ref bendingLink0ToBendingLink0Direction, ref bendingLink0ToEndDirection ) ) { return false; } Vector3 bendingLink0DirX = Vector3.zero; { Matrix3x3 effBasis; if( !SAFBIKComputeBasisFromXZLockX( out effBasis, isRight ? bendingLink0ToEffectorDirection : -bendingLink0ToEffectorDirection, solveDirZ ) ) { return false; } Matrix3x3 bendBasis; Matrix3x3 endBasis; // Effector direction stamp X/Y Plane.(Feedback Y Axis.) if( !SAFBIKComputeBasisFromXZLockZ( out bendBasis, isRight ? bendingLink0ToBendingLink0Direction : -bendingLink0ToBendingLink0Direction, effBasis.column2 ) || !SAFBIKComputeBasisFromXZLockZ( out endBasis, isRight ? bendingLink0ToEndDirection : -bendingLink0ToEndDirection, effBasis.column2 ) ) { return false; } Vector3 effX = isRight ? effBasis.column0 : -effBasis.column0; Vector3 effY = effBasis.column1; Vector3 bendX = isRight ? bendBasis.column0 : -bendBasis.column0; Vector3 endX = isRight ? endBasis.column0 : -endBasis.column0; Vector3 endY = endBasis.column1; float endBendDotX = Vector3.Dot( bendX, endX ); // Cosine float endBendDotY = Vector3.Dot( bendX, endY ); // Sine Vector3 rotBendX = _Rotate( ref effX, ref effY, endBendDotX, endBendDotY ); if( imm_isUpper ) { bendingLink0DirX = _FastLerpDir( ref rotBendX, ref effX, imm_traceRate ); } else { bendingLink0DirX = _FastLerpDir( ref bendX, ref effX, imm_traceRate ); } } if( !SAFBIKComputeBasisFromXZLockX( out bendingLink0.boneTransform.basis, isRight ? bendingLink0DirX : -bendingLink0DirX, solveDirZ ) ) { return false; } bendingLink0.boneTransform.origin = bendingLink0Position; SAFBIKMatMultRet0( ref bendingLink0.boneTransform.basis, ref bendingLink0.solvedToBoneBasis ); bendingLink1Position = bendingLink0.boneTransform * (bendingLink1.bone._defaultPosition - bendingLink0.bone._defaultPosition); { Vector3 dirX = basedEffectorPosition - bendingLink1Position; if( !SAFBIKVecNormalize( ref dirX ) ) { return false; } Vector3 dirZ; SAFBIKMatMultVec( out dirZ, ref bendingLink0.boneTransform.basis, ref bendingLink1.boneToSolvedBasis.column2 ); if( !SAFBIKComputeBasisFromXZLockX( out bendingLink1.boneTransform.basis, isRight ? dirX : -dirX, dirZ ) ) { return false; } bendingLink1.boneTransform.origin = bendingLink1Position; SAFBIKMatMultRet0( ref bendingLink1.boneTransform.basis, ref bendingLink1.solvedToBoneBasis ); } } } if( solveFingerIK ) { { Vector3 linkSolved = SolveFingerIK( ref beginLinkPosition, ref effectorPosition, ref solveDirY, linkLength0, linkLength1, linkLength2, ref fingerBranch.fingerIKParams ); if( linkSolved == Vector3.zero ) { return false; } // Limit angle for finger0. if( !isUpper ) { Matrix3x3 baseBasis; Vector3 dirX; SAFBIKMatMultVec( out dirX, ref parentTransform.basis, ref beginLink.boneToSolvedBasis.column0 ); if( SAFBIKComputeBasisFromXZLockZ( out baseBasis, dirX, solveDirZ ) ) { Vector3 localFingerSolve; SAFBIKMatMultVecInv( out localFingerSolve, ref baseBasis, ref linkSolved ); float finX = localFingerSolve.x; float finY = localFingerSolve.y; float finZ = localFingerSolve.z; float cosNotThumb1PitchLLimit = _notThumb1PitchLLimit.cos; if( (isRight && finX < cosNotThumb1PitchLLimit) || (!isRight && finX > -cosNotThumb1PitchLLimit) ) { float lenY = SAFBIKSqrt( 1.0f - (cosNotThumb1PitchLLimit * cosNotThumb1PitchLLimit + finZ * finZ) ); localFingerSolve.x = isRight ? cosNotThumb1PitchLLimit : -cosNotThumb1PitchLLimit; localFingerSolve.y = (finY >= 0.0f) ? lenY : -lenY; SAFBIKMatMultVec( out linkSolved, ref baseBasis, ref localFingerSolve ); } } } if( !SAFBIKComputeBasisFromXZLockX( out beginLink.boneTransform.basis, isRight ? linkSolved : -linkSolved, solveDirZ ) ) { return false; } beginLink.boneTransform.origin = beginLinkPosition; SAFBIKMatMultRet0( ref beginLink.boneTransform.basis, ref beginLink.solvedToBoneBasis ); } { Vector3 bendingLink0Position = beginLink.boneTransform * (bendingLink0.bone._defaultPosition - beginLink.bone._defaultPosition); // Forcefix: Vector3 bendingLink0ToEffector = effectorPosition - bendingLink0Position; Vector3 dirZ; SAFBIKMatMultVec( out dirZ, ref beginLink.boneTransform.basis, ref _internalValues.defaultRootBasis.column2 ); solveDirY = Vector3.Cross( dirZ, bendingLink0ToEffector ); if( !SAFBIKVecNormalize( ref solveDirY ) ) { return false; } solveDirY = isRight ? solveDirY : -solveDirY; Vector3 linkSolved = SolveLimbIK( ref bendingLink0Position, ref effectorPosition, linkLength1, linkLength1Sq, linkLength2, linkLength2Sq, ref solveDirY ); if( linkSolved == Vector3.zero ) { return false; } SAFBIKMatMultVec( out dirZ, ref beginLink.boneTransform.basis, ref bendingLink0.boneToSolvedBasis.column2 ); if( !SAFBIKComputeBasisFromXZLockX( out bendingLink0.boneTransform.basis, isRight ? linkSolved : -linkSolved, dirZ ) ) { return false; } bendingLink0.boneTransform.origin = bendingLink0Position; SAFBIKMatMultRet0( ref bendingLink0.boneTransform.basis, ref bendingLink0.solvedToBoneBasis ); } { Vector3 bendingLink1Position = bendingLink0.boneTransform * (bendingLink1.bone._defaultPosition - bendingLink0.bone._defaultPosition); Vector3 dirX = effectorPosition - bendingLink1Position; if( !SAFBIKVecNormalize( ref dirX ) ) { return false; } Vector3 dirZ; SAFBIKMatMultVec( out dirZ, ref bendingLink0.boneTransform.basis, ref bendingLink1.boneToSolvedBasis.column2 ); if( !SAFBIKComputeBasisFromXZLockX( out bendingLink1.boneTransform.basis, isRight ? dirX : -dirX, dirZ ) ) { return false; } bendingLink1.boneTransform.origin = bendingLink1Position; SAFBIKMatMultRet0( ref bendingLink1.boneTransform.basis, ref bendingLink1.solvedToBoneBasis ); } } Quaternion worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref beginLink.boneTransform.basis, ref beginLink.bone._defaultBasis ); beginLink.bone.worldRotation = worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref bendingLink0.boneTransform.basis, ref bendingLink0.bone._defaultBasis ); bendingLink0.bone.worldRotation = worldRotation; SAFBIKMatMultGetRot( out worldRotation, ref bendingLink1.boneTransform.basis, ref bendingLink1.bone._defaultBasis ); bendingLink1.bone.worldRotation = worldRotation; return true; }
public void SimpleAdditionGeneratesCorrectValues() { Matrix3x4 value1 = new Matrix3x4(1); Matrix3x4 value2 = new Matrix3x4(99); Matrix3x4 result = value1 + value2; for (int y = 0; y < Matrix3x4.RowCount; y++) { for (int x = 0; x < Matrix3x4.ColumnCount; x++) { Assert.AreEqual(1 + 99, result[x, y], Epsilon); } } }
public static void UniformMatrix3x4(int location, bool transpose, ref Matrix3x4 matrix) { unsafe { fixed (float* matrix_ptr = &matrix.Row0.X) { GL.UniformMatrix3x4(location, 1, transpose, matrix_ptr); } } }
// ReSharper disable once InconsistentNaming public bool Write(Chunked chunked, Stream output, List <byte> LODs, Dictionary <ulong, List <ImageLayer> > layers, object[] data) { IChunk chunk = chunked.FindNextChunk("MNRM").Value; if (chunk == null) { return(false); } MNRM model = (MNRM)chunk; chunk = chunked.FindNextChunk("CLDM").Value; CLDM materials = null; if (chunk != null) { materials = (CLDM)chunk; } chunk = chunked.FindNextChunk("lksm").Value; lksm skeleton = null; if (chunk != null) { skeleton = (lksm)chunk; } chunk = chunked.FindNextChunk("PRHM").Value; PRHM hardpoints = null; if (chunk != null) { hardpoints = (PRHM)chunk; } short[] hierarchy = (short[])skeleton?.Hierarchy.Clone(); Dictionary <int, HTLC.ClothNode> nodeMap = new Dictionary <int, HTLC.ClothNode>(); if (chunked.FindNextChunk("HTLC").Value is HTLC cloth) { uint clothIndex = 0; foreach (HTLC.ClothNode[] nodeCollection in cloth.Nodes) { if (nodeCollection == null) { continue; } int nodeIndex = 0; foreach (HTLC.ClothNode node in nodeCollection) { int parentRaw = node.VerticalParent; if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex) && cloth.NodeBones[clothIndex].ContainsKey(parentRaw)) { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = cloth.NodeBones[clothIndex][parentRaw]; if (cloth.NodeBones[clothIndex][parentRaw] == -1) { HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; } } } else { if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) // if on main skele { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = -1; HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; } } } if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) { // good code: if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { nodeMap[cloth.NodeBones[clothIndex][nodeIndex]] = node; } } nodeIndex++; } clothIndex++; } } using (BinaryWriter writer = new BinaryWriter(output)) { writer.Write((ushort)1); // version major writer.Write((ushort)4); // version minor if (data.Length > 1 && data[1] is string && ((string)data[1]).Length > 0) { writer.Write((string)data[1]); } else { writer.Write((byte)0); } if (data.Length > 2 && data[2] is string && ((string)data[2]).Length > 0) { writer.Write((string)data[2]); } else { writer.Write((byte)0); } if (skeleton == null) { writer.Write((ushort)0); // number of bones } else { writer.Write(skeleton.Data.bonesAbs); } // ReSharper disable once InconsistentNaming Dictionary <byte, List <int> > LODMap = new Dictionary <byte, List <int> >(); uint sz = 0; uint lookForLod = 0; bool lodOnly = data.Length > 3 && data[3] is bool && (bool)data[3]; for (int i = 0; i < model.Submeshes.Length; ++i) { SubmeshDescriptor submesh = model.Submeshes[i]; if (data.Length > 4 && data[4] is bool && (bool)data[4]) { if (submesh.flags == SubmeshFlags.COLLISION_MESH) { continue; } } if (LODs != null && !LODs.Contains(submesh.lod)) { continue; } if (lodOnly && lookForLod > 0 && submesh.lod != lookForLod) { continue; } if (!LODMap.ContainsKey(submesh.lod)) { LODMap.Add(submesh.lod, new List <int>()); } lookForLod = submesh.lod; sz++; LODMap[submesh.lod].Add(i); } //long meshCountPos = writer.BaseStream.Position; writer.Write(sz); writer.Write(hardpoints?.HardPoints.Length ?? 0); if (skeleton != null) { for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.Write(IdToString("bone", skeleton.IDs[i])); short parent = hierarchy[i]; if (parent == -1) { parent = (short)i; } writer.Write(parent); Matrix3x4 bone = skeleton.Matrices34[i]; Quaternion rot = new Quaternion(bone[0, 0], bone[0, 1], bone[0, 2], bone[0, 3]); Vector3 scl = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); if (nodeMap.ContainsKey(i)) { HTLC.ClothNode thisNode = nodeMap[i]; pos.X = thisNode.X; pos.Y = thisNode.Y; pos.Z = thisNode.Z; } writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(scl.X); writer.Write(scl.Y); writer.Write(scl.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } } foreach (KeyValuePair <byte, List <int> > kv in LODMap) { foreach (int i in kv.Value) { SubmeshDescriptor submesh = model.Submeshes[i]; ModelVertex[] vertex = model.Vertices[i]; ModelVertex[] normal = model.Normals[i]; ModelUV[][] uv = model.TextureCoordinates[i]; ModelIndice[] index = model.Indices[i]; ModelBoneData[] bones = model.Bones[i]; writer.Write($"Submesh_{i}.{kv.Key}.{materials.Materials[submesh.material]:X16}"); writer.Write(materials.Materials[submesh.material]); writer.Write((byte)uv.Length); writer.Write(vertex.Length); writer.Write(index.Length); for (int j = 0; j < vertex.Length; ++j) { writer.Write(vertex[j].x); writer.Write(vertex[j].y); writer.Write(vertex[j].z); writer.Write(-normal[j].x); writer.Write(-normal[j].y); writer.Write(-normal[j].z); foreach (ModelUV[] t in uv) { writer.Write(t[j].u); writer.Write(t[j].v); } if (skeleton != null && bones != null && bones[j].boneIndex != null && bones[j].boneWeight != null) { writer.Write((byte)4); writer.Write(skeleton.Lookup[bones[j].boneIndex[0]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[1]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[2]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[3]]); writer.Write(bones[j].boneWeight[0]); writer.Write(bones[j].boneWeight[1]); writer.Write(bones[j].boneWeight[2]); writer.Write(bones[j].boneWeight[3]); } else { // bone -> size + index + weight writer.Write((byte)0); } } List <ModelIndiceModifiable> indexNew = new List <ModelIndiceModifiable>(); foreach (ModelIndice indice in index) { indexNew.Add(new ModelIndiceModifiable { v1 = indice.v1, v2 = indice.v2, v3 = indice.v3 }); } foreach (ModelIndiceModifiable indice in indexNew) { writer.Write((byte)3); writer.Write(indice.v1); writer.Write(indice.v2); writer.Write(indice.v3); } } } if (hardpoints != null) { // attachments foreach (PRHM.HardPoint hp in hardpoints.HardPoints) { writer.Write(IdToString("attachment_", GUID.Index(hp.HardPointGUID))); Matrix4 mat = hp.Matrix.ToOpenTK(); Vector3 pos = mat.ExtractTranslation(); Quaternion rot = mat.ExtractRotation(); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } // extension 1.1 foreach (PRHM.HardPoint hp in hardpoints.HardPoints) { writer.Write(IdToString("bone", GUID.Index(hp.GUIDx012))); } } // ext 1.3: cloth writer.Write(0); // ext 1.4: embedded refpose if (skeleton != null) { for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.Write(IdToString("bone", skeleton.IDs[i])); short parent = hierarchy[i]; // if (parent == -1) { // parent = (short)i; // } writer.Write(parent); Matrix3x4 bone = skeleton.Matrices34Inverted[i]; // Quaternion3D quat = new Quaternion3D(bone[0, 0], bone[0, 1], bone[0, 2], bone[0, 3]); // why are they different Quaternion3D quat = new Quaternion3D(bone[0, 3], bone[0, 0], bone[0, 1], bone[0, 2]); Vector3D rot = C3D.ToEulerAngles(quat); if (rot.X == -3.14159274f && rot.Y == 0 && rot.Z == 0) { rot = new Vector3D(0, 3.14159274f, 3.14159274f); // effectively the same but you know, eulers. } Vector3 scl = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(scl.X); writer.Write(scl.Y); writer.Write(scl.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); } } } return(true); }
// ReSharper disable once InconsistentNaming // data is object[] { bool exportAttachments, string materialReference, string modelName, bool onlyOneLOD, bool skipCollision } public void Write(ICLIFlags flags, Chunked chunked, Stream output, List <byte> LODs, object[] data, FindLogic.Combo.ModelInfoNew modelInfo) { byte?flagLOD = null; if (flags is ExtractFlags extractFlags) { flagLOD = extractFlags.LOD; } IChunk chunk = chunked.FindNextChunk("MNRM").Value; if (chunk == null) { return; } MNRM model = (MNRM)chunk; chunk = chunked.FindNextChunk("CLDM").Value; CLDM materials = null; if (chunk != null) { materials = (CLDM)chunk; } chunk = chunked.FindNextChunk("lksm").Value; lksm skeleton = null; if (chunk != null) { skeleton = (lksm)chunk; } chunk = chunked.FindNextChunk("PRHM").Value; PRHM hardpoints = null; if (chunk != null) { hardpoints = (PRHM)chunk; } HTLC cloth = chunked.FindNextChunk("HTLC").Value as HTLC; short[] hierarchy = (short[])skeleton?.Hierarchy.Clone(); Dictionary <int, HTLC.ClothNode> nodeMap = new Dictionary <int, HTLC.ClothNode>(); if (cloth != null) { uint clothIndex = 0; foreach (HTLC.ClothNode[] nodeCollection in cloth.Nodes) { if (nodeCollection == null) { continue; } int nodeIndex = 0; foreach (HTLC.ClothNode node in nodeCollection) { int parentRaw = node.VerticalParent; if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex) && cloth.NodeBones[clothIndex].ContainsKey(parentRaw)) { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = cloth.NodeBones[clothIndex][parentRaw]; if (cloth.NodeBones[clothIndex][parentRaw] == -1) { HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; } } } else { if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = -1; HTLC.ClothNodeWeight weightedBone = node.Bones.Aggregate((i1, i2) => i1.Weight > i2.Weight ? i1 : i2); hierarchy[cloth.NodeBones[clothIndex][nodeIndex]] = weightedBone.Bone; } } } if (cloth.NodeBones[clothIndex].ContainsKey(nodeIndex)) { if (cloth.NodeBones[clothIndex][nodeIndex] != -1) { nodeMap[cloth.NodeBones[clothIndex][nodeIndex]] = node; } } nodeIndex++; } clothIndex++; } } using (BinaryWriter writer = new BinaryWriter(output)) { writer.Write((ushort)1); // version major writer.Write((ushort)5); // version minor if (data.Length > 1 && data[1] is string && ((string)data[1]).Length > 0) { writer.Write((string)data[1]); } else { writer.Write((byte)0); } if (data.Length > 2 && data[2] is string && ((string)data[2]).Length > 0) { writer.Write((string)data[2]); } else { writer.Write((byte)0); } if (skeleton == null) { writer.Write((ushort)0); // number of bones } else { writer.Write(skeleton.Data.bonesAbs); } // ReSharper disable once InconsistentNaming Dictionary <byte, List <int> > LODMap = new Dictionary <byte, List <int> >(); uint sz = 0; uint lookForLod = 0; if (model.Submeshes.Any(x => x.lod == flagLOD)) { lookForLod = (byte)flagLOD; } else if (flagLOD != null) { SubmeshDescriptor nextLowest = model.Submeshes.Where(p => p.lod < flagLOD).OrderBy(x => x.lod).LastOrDefault(); if (nextLowest.verticesToDraw == 0 && nextLowest.indexCount == 0) // not real mesh { SubmeshDescriptor nextHighest = model.Submeshes.Where(p => p.lod > flagLOD).OrderBy(x => x.lod).FirstOrDefault(); lookForLod = nextHighest.lod; } else { lookForLod = nextLowest.lod; } } for (int i = 0; i < model.Submeshes.Length; ++i) { SubmeshDescriptor submesh = model.Submeshes[i]; if (data.Length > 4 && data[4] is bool && (bool)data[4]) { if (submesh.flags == SubmeshFlags.COLLISION_MESH) { continue; } } if (lookForLod > 0 && submesh.lod != lookForLod && submesh.lod != 255) { continue; } if (!LODMap.ContainsKey(submesh.lod)) { LODMap.Add(submesh.lod, new List <int>()); } sz++; LODMap[submesh.lod].Add(i); } writer.Write(sz); writer.Write(hardpoints?.HardPoints.Length ?? 0); if (skeleton != null) { for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.Write(IdToString("bone", skeleton.IDs[i])); short parent = hierarchy[i]; if (parent == -1) { parent = (short)i; } writer.Write(parent); Matrix3x4 bone = skeleton.Matrices34[i]; Quaternion rot = new Quaternion(bone[0, 0], bone[0, 1], bone[0, 2], bone[0, 3]); Vector3 scl = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); if (nodeMap.ContainsKey(i)) { HTLC.ClothNode thisNode = nodeMap[i]; pos.X = thisNode.X; pos.Y = thisNode.Y; pos.Z = thisNode.Z; } writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(scl.X); writer.Write(scl.Y); writer.Write(scl.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } } foreach (KeyValuePair <byte, List <int> > kv in LODMap) { foreach (int i in kv.Value) { SubmeshDescriptor submesh = model.Submeshes[i]; ModelVertex[] vertex = model.Vertices[i]; ModelVertex[] normal = model.Normals[i]; ModelUV[][] uv = model.TextureCoordinates[i]; ModelIndice[] index = model.Indices[i]; ModelBoneData[] bones = model.Bones[i]; writer.Write($"Submesh_{i}.{kv.Key}.{materials.Materials[submesh.material]:X16}"); writer.Write(materials.Materials[submesh.material]); writer.Write((byte)uv.Length); writer.Write(vertex.Length); writer.Write(index.Length); for (int j = 0; j < vertex.Length; ++j) { writer.Write(vertex[j].x); writer.Write(vertex[j].y); writer.Write(vertex[j].z); writer.Write(-normal[j].x); writer.Write(-normal[j].y); writer.Write(-normal[j].z); foreach (ModelUV[] t in uv) { writer.Write(t[j].u); writer.Write(t[j].v); } if (skeleton != null && bones != null && bones[j].boneIndex != null && bones[j].boneWeight != null) { writer.Write((byte)4); writer.Write(skeleton.Lookup[bones[j].boneIndex[0]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[1]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[2]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[3]]); writer.Write(bones[j].boneWeight[0]); writer.Write(bones[j].boneWeight[1]); writer.Write(bones[j].boneWeight[2]); writer.Write(bones[j].boneWeight[3]); } else { // bone -> size + index + weight writer.Write((byte)0); } } List <ModelIndiceModifiable> indexNew = new List <ModelIndiceModifiable>(); foreach (ModelIndice indice in index) { indexNew.Add(new ModelIndiceModifiable { v1 = indice.v1, v2 = indice.v2, v3 = indice.v3 }); } foreach (ModelIndiceModifiable indice in indexNew) { writer.Write((byte)3); writer.Write(indice.v1); writer.Write(indice.v2); writer.Write(indice.v3); } } } if (hardpoints != null) { // attachments foreach (PRHM.HardPoint hp in hardpoints.HardPoints) { writer.Write(IdToString("hardpoint", GUID.Index(hp.HardPointGUID))); Matrix4 mat = hp.Matrix.ToOpenTK(); Vector3 pos = mat.ExtractTranslation(); Quaternion rot = mat.ExtractRotation(); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } // extension 1.1 foreach (PRHM.HardPoint hp in hardpoints.HardPoints) { writer.Write(IdToString("bone", GUID.Index(hp.GUIDx012))); } } // ext 1.3: cloth writer.Write(0); // ext 1.4: embedded refpose if (skeleton != null) { for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.Write(IdToString("bone", skeleton.IDs[i])); short parent = hierarchy[i]; writer.Write(parent); Matrix3x4 bone = skeleton.Matrices34Inverted[i]; Quaternion3D quat = new Quaternion3D(bone[0, 3], bone[0, 0], bone[0, 1], bone[0, 2]); Vector3D rot = C3D.ToEulerAngles(quat); // ReSharper disable CompareOfFloatsByEqualityOperator if (rot.X == -3.14159274f && rot.Y == 0 && rot.Z == 0) { rot = new Vector3D(0, 3.14159274f, 3.14159274f); // effectively the same but you know, eulers. } // ReSharper restore CompareOfFloatsByEqualityOperator Vector3 scl = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(scl.X); writer.Write(scl.Y); writer.Write(scl.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); } } // ext 1.5: guid writer.Write(GUID.Index(modelInfo.GUID)); // ext 1.6: cloth 2.0 if (cloth == null) { writer.Write(0); } else { writer.Write(cloth.Descriptors.Length); for (int i = 0; i < cloth.Descriptors.Length; i++) { var desc = cloth.Descriptors[i]; writer.Write(desc.Name); writer.Write(cloth.Nodes[i].Length); foreach (HTLC.ClothNode clothNode in cloth.Nodes[i]) { writer.Write(clothNode.Bones.Length); foreach (HTLC.ClothNodeWeight clothNodeWeight in clothNode.Bones) { writer.Write(clothNodeWeight.Bone); writer.Write(clothNodeWeight.Weight); } } } } } }
public void SetUniform(Matrix3x4 matrix, int id) { GL.UniformMatrix3x4(id, true, ref matrix); }
private NTROValue ReadField(ResourceIntrospectionManifest.ResourceDiskStruct.Field field, bool pointer) { switch (field.Type) { case DataType.Struct: var newStruct = Resource.IntrospectionManifest.ReferencedStructs.First(x => x.Id == field.TypeData); return(new NTROValue <NTROStruct>(field.Type, ReadStructure(newStruct, Reader.BaseStream.Position), pointer)); case DataType.Enum: // TODO: Lookup in ReferencedEnums return(new NTROValue <uint>(field.Type, Reader.ReadUInt32(), pointer)); case DataType.SByte: return(new NTROValue <sbyte>(field.Type, Reader.ReadSByte(), pointer)); case DataType.Byte: return(new NTROValue <byte>(field.Type, Reader.ReadByte(), pointer)); case DataType.Boolean: return(new NTROValue <bool>(field.Type, Reader.ReadByte() == 1 ? true : false, pointer)); case DataType.Int16: return(new NTROValue <short>(field.Type, Reader.ReadInt16(), pointer)); case DataType.UInt16: return(new NTROValue <ushort>(field.Type, Reader.ReadUInt16(), pointer)); case DataType.Int32: return(new NTROValue <int>(field.Type, Reader.ReadInt32(), pointer)); case DataType.UInt32: return(new NTROValue <uint>(field.Type, Reader.ReadUInt32(), pointer)); case DataType.Float: return(new NTROValue <float>(field.Type, Reader.ReadSingle(), pointer)); case DataType.Int64: return(new NTROValue <long>(field.Type, Reader.ReadInt64(), pointer)); case DataType.ExternalReference: var id = Reader.ReadUInt64(); var value = id > 0 ? Resource.ExternalReferences?.ResourceRefInfoList.FirstOrDefault(c => c.Id == id) : null; return(new NTROValue <ResourceExtRefList.ResourceReferenceInfo>(field.Type, value, pointer)); case DataType.UInt64: return(new NTROValue <ulong>(field.Type, Reader.ReadUInt64(), pointer)); case DataType.Vector: var vector3 = new Vector3( Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle()); return(new NTROValue <Vector3>(field.Type, vector3, pointer)); case DataType.Quaternion: case DataType.Color: case DataType.Fltx4: case DataType.Vector4D: case DataType.Vector4D_44: var vector4 = new Vector4( Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle()); return(new NTROValue <Vector4>(field.Type, vector4, pointer)); case DataType.String4: case DataType.String: return(new NTROValue <string>(field.Type, Reader.ReadOffsetString(Encoding.UTF8), pointer)); case DataType.Matrix3x4: case DataType.Matrix3x4a: var matrix3x4a = new Matrix3x4( Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle()); return(new NTROValue <Matrix3x4>(field.Type, matrix3x4a, pointer)); case DataType.CTransform: var transform = new CTransform( Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle(), Reader.ReadSingle()); return(new NTROValue <CTransform>(field.Type, transform, pointer)); default: throw new NotImplementedException($"Unknown data type: {field.Type} (name: {field.FieldName})"); } }
/// <summary> /// Outputs an EVP1 chunk to the specified stream. /// </summary> /// <param name="writer">Stream to write EVP1 to</param> public void WriteEVP1(EndianBinaryWriter writer) { // Header writer.Write("EVP1".ToCharArray()); // FourCC, "EVP1" writer.Write((int)0); // Placeholder for size writer.Write((short)partialWeightList.Count); // Number of weights writer.Write((short)-1); // Padding for header writer.Write((int)0x1C); // Offset to index count section, always 0x1C writer.Write((int)0); // Placeholder for index data offset writer.Write((int)0); // Placeholder for weight data offset writer.Write((int)0); // Placeholder for inverse bind matrix offset // Write index count table for (int i = 0; i < partialWeightList.Count; i++) { writer.Write((byte)(partialWeightList[i].BoneIndexes.Count)); } // Write offset to bone index table Util.WriteOffset(writer, 0x10); // Write bone index table for (int i = 0; i < partialWeightList.Count; i++) { for (int j = 0; j < partialWeightList[i].BoneIndexes.Count; j++) { writer.Write((ushort)(partialWeightList[i].BoneIndexes[j])); } } Util.PadStreamWithString(writer, 32); // Write offset to weight table Util.WriteOffset(writer, 0x14); // Write weight table foreach (Weight weight in partialWeightList) { for (int i = 0; i < weight.BoneWeights.Count; i++) { writer.Write(weight.BoneWeights[i]); } } Util.PadStreamWithString(writer, 32); // Write offset to inverse matrix table Util.WriteOffset(writer, 0x18); // Write inverse bind matrix table foreach (Matrix4 mat in InverseBindMatrices) { Vector3 trans = mat.ExtractTranslation(); Vector3 scale = mat.ExtractScale(); Quaternion rot = mat.ExtractRotation(); Matrix3x4 test = Matrix3x4.CreateScale(scale) * Matrix3x4.CreateFromQuaternion(rot) * Matrix3x4.CreateTranslation(trans); //Matrix3x4 mat3 = Matrix3x4.Mult(InverseBindMatrices, ident); // BMD stores the matrices as 3x4, so we discard the last row /* * writer.Write(test.M11); * writer.Write(test.M12); * writer.Write(test.M13); * writer.Write(test.M14); * * writer.Write(test.M21); * writer.Write(test.M22); * writer.Write(test.M23); * writer.Write(test.M24); * * writer.Write(test.M31); * writer.Write(test.M32); * writer.Write(test.M33); * writer.Write(test.M34); */ Vector4 Row1 = mat.Column0; Vector4 Row2 = mat.Column1; Vector4 Row3 = mat.Column2; writer.Write(Row1.X); writer.Write(Row1.Y); writer.Write(Row1.Z); writer.Write(Row1.W); writer.Write(Row2.X); writer.Write(Row2.Y); writer.Write(Row2.Z); writer.Write(Row2.W); writer.Write(Row3.X); writer.Write(Row3.Y); writer.Write(Row3.Z); writer.Write(Row3.W); } Util.PadStreamWithString(writer, 32); // Write chunk size Util.WriteOffset(writer, 4); }
protected static void MulPoint3x4_XYZ(ref Vector3 result, ref Matrix3x4 mat, Vector4 vec) { result.x = mat.m00 * vec.x + mat.m01 * vec.y + mat.m02 * vec.z + mat.m03; result.y = mat.m10 * vec.x + mat.m11 * vec.y + mat.m12 * vec.z + mat.m13; result.z = mat.m20 * vec.x + mat.m21 * vec.y + mat.m22 * vec.z + mat.m23; }
public void Parse(Stream input) { using (BinaryReader reader = new BinaryReader(input, Encoding.UTF8, true)) { Header = reader.Read <HeaderInfo>(); if (Header.descCount > 0) { Descriptors = new ClothDesc[Header.descCount]; input.Position = Header.descOffset; Nodes = new ClothNode[Header.descCount][]; BoneMap = new Dictionary <int, int> [Header.descCount]; NodeBones = new Dictionary <int, short> [Header.descCount]; for (ulong i = 0; i < Header.descCount; ++i) { Descriptors[i] = reader.Read <ClothDesc>(); long afterpos = reader.BaseStream.Position; if (Descriptors[i].section8Offset != 4574069944263674675) { reader.BaseStream.Position = Descriptors[i].section8Offset; NodeBones[i] = new Dictionary <int, short>(); for (int nodeIndex = 0; nodeIndex < Descriptors[i].driverNodeCount; nodeIndex++) { NodeBones[i][nodeIndex] = reader.ReadInt16(); } } Nodes[i] = new ClothNode[Descriptors[i].driverNodeCount]; reader.BaseStream.Position = Descriptors[i].section1Offset; for (int nodeIndex = 0; nodeIndex < Descriptors[i].driverNodeCount; nodeIndex++) { long nodeStart = reader.BaseStream.Position; float x = reader.ReadSingle(); float y = reader.ReadSingle(); float z = reader.ReadSingle(); uint zero = reader.ReadUInt32(); // zero float x2 = reader.ReadSingle(); float y2 = reader.ReadSingle(); float z2 = reader.ReadSingle(); if (zero != 0) { throw new InvalidDataException($"HTLC: zero != 0 ({zero})"); } if (Math.Abs(x - x2) > 0.01 || Math.Abs(y - y2) > 0.01 || Math.Abs(z - z2) > 0.01) { throw new InvalidDataException($"HTLC: location is different: {x}:{x2} {y}:{y2} {z}:{z2}"); } reader.BaseStream.Position = nodeStart + 0x15c; short ind1 = reader.ReadInt16(); short ind2 = reader.ReadInt16(); short ind3 = reader.ReadInt16(); short ind4 = reader.ReadInt16(); reader.BaseStream.Position = nodeStart + 0x170; float weight1 = reader.ReadSingle(); float weight2 = reader.ReadSingle(); float weight3 = reader.ReadSingle(); float weight4 = reader.ReadSingle(); reader.BaseStream.Position = nodeStart + 0x140; float verticalParentStrength1 = reader.ReadSingle(); reader.BaseStream.Position = nodeStart + 0x148; float verticalParentStrength2 = reader.ReadSingle(); reader.BaseStream.Position = nodeStart + 0x154; float diagonalParentStrength = reader.ReadSingle(); reader.BaseStream.Position = nodeStart + 0x14c; short verticalParent = reader.ReadInt16(); reader.BaseStream.Position = nodeStart + 0x158; short diagonalParent = reader.ReadInt16(); reader.BaseStream.Position = nodeStart + 0x150; short chainNumber = reader.ReadInt16(); reader.BaseStream.Position = nodeStart + 0x15a; byte isChild = reader.ReadByte(); if (isChild == 1 && chainNumber == -1) { throw new InvalidDataException("HTLC: node is child but not in a chain"); } reader.BaseStream.Position = nodeStart + 0xC0 + 16; Matrix3x4 ff = reader.Read <Matrix3x4B>().ToOpenTK(); // this is wrong... reader.BaseStream.Position = nodeStart + 0x180; Nodes[i][nodeIndex] = new ClothNode { ID = nodeIndex, X = x, Y = y, Z = z, ChainNumber = chainNumber, DiagonalParent = diagonalParent, VerticalParent = verticalParent, Bones = new [] { new ClothNodeWeight(ind1, weight1), new ClothNodeWeight(ind2, weight2), new ClothNodeWeight(ind3, weight3), new ClothNodeWeight(ind4, weight4) }, IsChild = isChild == 1, Matrix = ff }; } reader.BaseStream.Position = afterpos; } } } }
public void MuliplyByMatrix3x4ProducesMatrix3x2() { Matrix4x2 matrix1 = new Matrix4x2(3); Matrix3x4 matrix2 = new Matrix3x4(2); Matrix3x2 result = matrix1 * matrix2; Matrix3x2 expected = new Matrix3x2(24, 24, 24, 24, 24, 24); Assert.AreEqual(expected, result); }
Vector3 Solve3VarLinearEquation(Matrix3x4 coefficients) { if (closeEnough(coefficients.Determinant3x3(), 0)) { try { coefficients = new Matrix3x4(coefficients.a1 + 1, coefficients.a2 + 1, coefficients.a3 + 1, coefficients.a4, coefficients.b1 + 1, coefficients.b2 + 1, coefficients.b3 + 1, coefficients.b4, coefficients.c1 + 1, coefficients.c2 + 1, coefficients.c3 + 1, coefficients.c4); return(Solve3VarLinearEquation(coefficients)); } catch (StackOverflowException e) { Console.WriteLine("Unable to solve a texgen"); return(new Vector3(0, 0, 0)); } } if (closeEnough(coefficients.a1, 0)) { coefficients.a1 = 0; } if (closeEnough(coefficients.a2, 0)) { coefficients.a2 = 0; } if (closeEnough(coefficients.a3, 0)) { coefficients.a3 = 0; } if (closeEnough(coefficients.a4, 0)) { coefficients.a4 = 0; } if (closeEnough(coefficients.b1, 0)) { coefficients.b1 = 0; } if (closeEnough(coefficients.b2, 0)) { coefficients.b2 = 0; } if (closeEnough(coefficients.b3, 0)) { coefficients.b3 = 0; } if (closeEnough(coefficients.b4, 0)) { coefficients.b4 = 0; } if (closeEnough(coefficients.c1, 0)) { coefficients.c1 = 0; } if (closeEnough(coefficients.c2, 0)) { coefficients.c2 = 0; } if (closeEnough(coefficients.c3, 0)) { coefficients.c3 = 0; } if (closeEnough(coefficients.c4, 0)) { coefficients.c4 = 0; } if (closeEnough(coefficients.a1, 0)) { if (closeEnough(coefficients.b1, 0)) { coefficients.SwapRows(0, 2); } else if (closeEnough(coefficients.c1, 0)) { coefficients.SwapRows(0, 1); } } if (!closeEnough(coefficients.a1, 0)) { if (!closeEnough(coefficients.b1, 0)) { coefficients.AddRows(0, 1, -coefficients.b1 / coefficients.a1); } if (!closeEnough(coefficients.c1, 0)) { coefficients.AddRows(0, 2, -coefficients.c1 / coefficients.a1); } } if (closeEnough(coefficients.b2, 0)) { coefficients.SwapRows(1, 2); } if (!closeEnough(coefficients.b2, 0)) { if (!closeEnough(coefficients.c2, 0)) { coefficients.AddRows(1, 2, -coefficients.c2 / coefficients.b2); } } if (!closeEnough(coefficients.a1, 0, 0.00001f)) { coefficients.ScaleRow(0, 1 / coefficients.a1); } if (!closeEnough(coefficients.b2, 0, 0.00001f)) { coefficients.ScaleRow(1, 1 / coefficients.b2); } if (!closeEnough(coefficients.c3, 0, 0.00001f)) { coefficients.ScaleRow(2, 1 / coefficients.c3); } var res = new Vector3(); res.Z = coefficients.c4; res.Y = coefficients.b4 - (res.Z * coefficients.b3); res.X = coefficients.a4 - (res.Y * coefficients.a2) - (res.Z * coefficients.a3); return(res); }
public EVP1(EndianBinaryReader reader, int offset) { Weights = new List <Weight>(); InverseBindMatrices = new List <Matrix4>(); reader.BaseStream.Seek(offset, System.IO.SeekOrigin.Begin); reader.SkipInt32(); int evp1Size = reader.ReadInt32(); int entryCount = reader.ReadInt16(); reader.SkipInt16(); int weightCountsOffset = reader.ReadInt32(); int boneIndicesOffset = reader.ReadInt32(); int weightDataOffset = reader.ReadInt32(); int inverseBindMatricesOffset = reader.ReadInt32(); List <int> counts = new List <int>(); List <float> weights = new List <float>(); List <int> indices = new List <int>(); for (int i = 0; i < entryCount; i++) { counts.Add(reader.ReadByte()); } reader.BaseStream.Seek(boneIndicesOffset + offset, System.IO.SeekOrigin.Begin); for (int i = 0; i < entryCount; i++) { for (int j = 0; j < counts[i]; j++) { indices.Add(reader.ReadInt16()); } } reader.BaseStream.Seek(weightDataOffset + offset, System.IO.SeekOrigin.Begin); for (int i = 0; i < entryCount; i++) { for (int j = 0; j < counts[i]; j++) { weights.Add(reader.ReadSingle()); } } int totalRead = 0; for (int i = 0; i < entryCount; i++) { Weight weight = new Weight(); for (int j = 0; j < counts[i]; j++) { weight.AddWeight(weights[totalRead + j], indices[totalRead + j]); } Weights.Add(weight); totalRead += counts[i]; } reader.BaseStream.Seek(inverseBindMatricesOffset + offset, System.IO.SeekOrigin.Begin); int matrixCount = (evp1Size - inverseBindMatricesOffset) / 48; for (int i = 0; i < matrixCount; i++) { Matrix3x4 invBind = new Matrix3x4(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle()); InverseBindMatrices.Add(new Matrix4(invBind.Row0, invBind.Row1, invBind.Row2, Vector4.UnitW)); } reader.BaseStream.Seek(offset + evp1Size, System.IO.SeekOrigin.Begin); }
public bool Write(Chunked chunked, Stream output, List <byte> LODs, Dictionary <ulong, List <ImageLayer> > layers, object[] data) { IChunk chunk = chunked.FindNextChunk("MNRM").Value; if (chunk == null) { return(false); } MNRM model = (MNRM)chunk; chunk = chunked.FindNextChunk("CLDM").Value; CLDM materials = null; if (chunk != null) { materials = (CLDM)chunk; } chunk = chunked.FindNextChunk("lksm").Value; lksm skeleton = null; if (chunk != null) { skeleton = (lksm)chunk; } chunk = chunked.FindNextChunk("PRHM").Value; PRHM hardpoints = null; if (chunk != null) { hardpoints = (PRHM)chunk; } //Console.Out.WriteLine("Writing OWMDL"); using (BinaryWriter writer = new BinaryWriter(output)) { writer.Write((ushort)1); // version major writer.Write((ushort)1); // version minor if (data.Length > 1 && data[1] != null && data[1].GetType() == typeof(string) && ((string)data[1]).Length > 0) { writer.Write((string)data[1]); } else { writer.Write((byte)0); } if (data.Length > 2 && data[2] != null && data[2].GetType() == typeof(string) && ((string)data[2]).Length > 0) { writer.Write((string)data[2]); } else { writer.Write((byte)0); } if (skeleton == null) { writer.Write((ushort)0); // number of bones } else { writer.Write(skeleton.Data.bonesAbs); } Dictionary <byte, List <int> > LODMap = new Dictionary <byte, List <int> >(); uint sz = 0; uint lookForLod = 0; bool lodOnly = false; if (data.Length > 3 && data[3] != null && data[3].GetType() == typeof(bool) && (bool)data[3] == true) { lodOnly = true; } for (int i = 0; i < model.Submeshes.Length; ++i) { SubmeshDescriptor submesh = model.Submeshes[i]; if (data.Length > 4 && data[4] != null && data[4].GetType() == typeof(bool) && (bool)data[4] == true) { if ((SubmeshFlags)submesh.flags == SubmeshFlags.COLLISION_MESH) { continue; } } if (LODs != null && !LODs.Contains(submesh.lod)) { continue; } if (lodOnly && lookForLod > 0 && submesh.lod != lookForLod) { continue; } if (!LODMap.ContainsKey(submesh.lod)) { LODMap.Add(submesh.lod, new List <int>()); } lookForLod = submesh.lod; sz++; LODMap[submesh.lod].Add(i); } writer.Write(sz); if (hardpoints != null) { writer.Write(hardpoints.HardPoints.Length); } else { writer.Write((int)0); // number of attachments } if (skeleton != null) { for (int i = 0; i < skeleton.Data.bonesAbs; ++i) { writer.Write(IdToString("bone", skeleton.IDs[i])); short parent = skeleton.Hierarchy[i]; if (parent == -1) { parent = (short)i; } writer.Write(parent); Matrix3x4 bone = skeleton.Matrices34[i]; Quaternion rot = new Quaternion(bone[0, 0], bone[0, 1], bone[0, 2], bone[0, 3]); Vector3 scl = new Vector3(bone[1, 0], bone[1, 1], bone[1, 2]); Vector3 pos = new Vector3(bone[2, 0], bone[2, 1], bone[2, 2]); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(scl.X); writer.Write(scl.X); writer.Write(scl.X); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } } foreach (KeyValuePair <byte, List <int> > kv in LODMap) { //Console.Out.WriteLine("Writing LOD {0}", kv.Key); foreach (int i in kv.Value) { SubmeshDescriptor submesh = model.Submeshes[i]; ModelVertex[] vertex = model.Vertices[i]; ModelVertex[] normal = model.Normals[i]; ModelUV[][] uv = model.TextureCoordinates[i]; ModelIndice[] index = model.Indices[i]; ModelBoneData[] bones = model.Bones[i]; writer.Write($"Submesh_{i}.{kv.Key}.{materials.Materials[submesh.material]:X16}"); writer.Write(materials.Materials[submesh.material]); writer.Write((byte)uv.Length); writer.Write(vertex.Length); writer.Write(index.Length); for (int j = 0; j < vertex.Length; ++j) { writer.Write(vertex[j].x); writer.Write(vertex[j].y); writer.Write(vertex[j].z); writer.Write(-normal[j].x); writer.Write(-normal[j].y); writer.Write(-normal[j].z); for (int k = 0; k < uv.Length; ++k) { writer.Write((float)uv[k][j].u); writer.Write((float)uv[k][j].v); } if (skeleton != null && bones != null && bones[j].boneIndex != null && bones[j].boneWeight != null) { writer.Write((byte)4); writer.Write(skeleton.Lookup[bones[j].boneIndex[0]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[1]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[2]]); writer.Write(skeleton.Lookup[bones[j].boneIndex[3]]); writer.Write(bones[j].boneWeight[0]); writer.Write(bones[j].boneWeight[1]); writer.Write(bones[j].boneWeight[2]); writer.Write(bones[j].boneWeight[3]); } else { // bone -> size + index + weight writer.Write((byte)0); } } for (int j = 0; j < index.Length; ++j) { writer.Write((byte)3); writer.Write((int)index[j].v1); writer.Write((int)index[j].v2); writer.Write((int)index[j].v3); } } } if (hardpoints != null) { // attachments for (int i = 0; i < hardpoints.HardPoints.Length; ++i) { PRHM.HardPoint hp = hardpoints.HardPoints[i]; writer.Write(IdToString("attachment_", hp.id)); Matrix4 mat = hp.matrix.ToOpenTK(); Vector3 pos = mat.ExtractTranslation(); Quaternion rot = mat.ExtractRotation(); writer.Write(pos.X); writer.Write(pos.Y); writer.Write(pos.Z); writer.Write(rot.X); writer.Write(rot.Y); writer.Write(rot.Z); writer.Write(rot.W); } // extension 1.1 for (int i = 0; i < hardpoints.HardPoints.Length; ++i) { PRHM.HardPoint hp = hardpoints.HardPoints[i]; writer.Write(IdToString("bone", hp.id)); } } } return(true); }
private static extern void Urho3D_Object_Event_SetMatrix3x4(IntPtr map, uint key, ref Matrix3x4 value);
public Matrix3x4 Multiply( ref Matrix3x4 t ) { Matrix3x3 tmp_basis; Vector3 tmp_origin; SAFBIKMatMult( out tmp_basis, ref basis, ref t.basis ); SAFBIKMatMultVecAdd( out tmp_origin, ref basis, ref t.origin, ref origin ); return new Matrix3x4( ref tmp_basis, ref tmp_origin ); }