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 );
}
