public XbimPackedNormal Transform(XbimQuaternion q)
{
XbimVector3D v1 = Normal;
XbimVector3D v2;
XbimQuaternion.Transform(ref v1, ref q, out v2);
return(new XbimPackedNormal(v2));
}
public XbimFaceTriangulation Transform(XbimQuaternion q)
{
var result = new XbimFaceTriangulation(_indices.Count, _normals.Count);
foreach (var normal in _normals)
result.AddNormal(normal.Transform(q));
foreach (var index in _indices)
result.AddIndex(index);
return result;
}
public void QuaternionTests()
{
var q = new XbimQuaternion();
Assert.AreEqual(true, q.IsIdentity(), "Uninitialised quaternion should be identity.");
q = new XbimQuaternion(0.0f, 0.0f, 0.0f, 1.0f);
Assert.AreEqual(true, q.IsIdentity(), "Should be identity when initialised with floats.");
var mat = new XbimMatrix3D();
q = mat.GetRotationQuaternion();
Assert.AreEqual(true, q.IsIdentity(), "Quaternion from identity matrix shold be identity.");
}
/// <summary>
/// Decomposes a matrix into a scale, rotation, and translation.
/// </summary>
/// <param name="scale">When the method completes, contains the scaling component of the decomposed matrix.</param>
/// <param name="rotation">When the method completes, contains the rtoation component of the decomposed matrix.</param>
/// <param name="translation">When the method completes, contains the translation component of the decomposed matrix.</param>
/// <remarks>
/// This method is designed to decompose an SRT transformation matrix only.
/// </remarks>
public bool Decompose(out XbimVector3D scale, out XbimQuaternion rotation, out XbimVector3D translation)
{
//Source: Unknown
// References: http://www.gamedev.net/community/forums/topic.asp?topic_id=441695
// via https://code.google.com/p/sharpdx/source/browse/Source/SharpDX/Matrix.cs?r=9f9e209b1be04f06f294bc6d72b06055ad6abdcc
//Get the translation.
translation = new XbimVector3D();
translation.X = this._offsetX;
translation.Y = this._offsetY;
translation.Z = this._offsetZ;
//Scaling is the length of the rows.
scale = new XbimVector3D();
scale.X = Math.Sqrt((M11 * M11) + (M12 * M12) + (M13 * M13));
scale.Y = Math.Sqrt((M21 * M21) + (M22 * M22) + (M23 * M23));
scale.Z = Math.Sqrt((M31 * M31) + (M32 * M32) + (M33 * M33));
//If any of the scaling factors are zero, than the rotation matrix can not exist.
//
double ZeroTolerance = 0.000003;
if (Math.Abs(scale.X) < ZeroTolerance ||
Math.Abs(scale.Y) < ZeroTolerance ||
Math.Abs(scale.Z) < ZeroTolerance)
{
rotation = new XbimQuaternion(); // defaults to identity
return(false);
}
//The rotation is the left over matrix after dividing out the scaling.
XbimMatrix3D rotationmatrix = new XbimMatrix3D();
rotationmatrix.M11 = M11 / scale.X;
rotationmatrix.M12 = M12 / scale.X;
rotationmatrix.M13 = M13 / scale.X;
rotationmatrix.M21 = M21 / scale.Y;
rotationmatrix.M22 = M22 / scale.Y;
rotationmatrix.M23 = M23 / scale.Y;
rotationmatrix.M31 = M31 / scale.Z;
rotationmatrix.M32 = M32 / scale.Z;
rotationmatrix.M33 = M33 / scale.Z;
rotationmatrix.M44 = 1;
XbimQuaternion.RotationMatrix(ref rotationmatrix, out rotation);
return(true);
}
public XbimFaceTriangulation Transform(XbimQuaternion q)
{
var result = new XbimFaceTriangulation(_indices.Count, _normals.Count);
foreach (var normal in _normals)
{
result.AddNormal(normal.Transform(q));
}
foreach (var index in _indices)
{
result.AddIndex(index);
}
return(result);
}
/// <summary>
/// Creates a quaternion given a rotation matrix.
/// </summary>
/// <param name="matrix">The rotation matrix.</param>
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
public static void RotationMatrix(ref XbimMatrix3D matrix, out XbimQuaternion result)
{
double sqrt;
double half;
double scale = matrix.M11 + matrix.M22 + matrix.M33;
result = new XbimQuaternion();
if (scale > 0.0f)
{
sqrt = Math.Sqrt(scale + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (matrix.M23 - matrix.M32) * sqrt;
result.Y = (matrix.M31 - matrix.M13) * sqrt;
result.Z = (matrix.M12 - matrix.M21) * sqrt;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
sqrt = Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (matrix.M12 + matrix.M21) * half;
result.Z = (matrix.M13 + matrix.M31) * half;
result.W = (matrix.M23 - matrix.M32) * half;
}
else if (matrix.M22 > matrix.M33)
{
sqrt = Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
half = 0.5f / sqrt;
result.X = (matrix.M21 + matrix.M12) * half;
result.Y = 0.5f * sqrt;
result.Z = (matrix.M32 + matrix.M23) * half;
result.W = (matrix.M31 - matrix.M13) * half;
}
else
{
sqrt = Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
half = 0.5f / sqrt;
result.X = (matrix.M31 + matrix.M13) * half;
result.Y = (matrix.M32 + matrix.M23) * half;
result.Z = 0.5f * sqrt;
result.W = (matrix.M12 - matrix.M21) * half;
}
}
/// <summary>
/// Transforms a 3D vector by the given <see cref="SharpDX.Quaternion"/> rotation.
/// </summary>
/// <param name="vector">The vector to rotate.</param>
/// <param name="rotation">The <see cref="SharpDX.Quaternion"/> rotation to apply.</param>
/// <param name="result">When the method completes, contains the transformed <see cref="SharpDX.Vector4"/>.</param>
public static void Transform(ref XbimVector3D vector, ref XbimQuaternion rotation, out XbimVector3D result)
{
double x = rotation.X + rotation.X;
double y = rotation.Y + rotation.Y;
double z = rotation.Z + rotation.Z;
double wx = rotation.W * x;
double wy = rotation.W * y;
double wz = rotation.W * z;
double xx = rotation.X * x;
double xy = rotation.X * y;
double xz = rotation.X * z;
double yy = rotation.Y * y;
double yz = rotation.Y * z;
double zz = rotation.Z * z;
result = new XbimVector3D(
((vector.X * ((1.0f - yy) - zz)) + (vector.Y * (xy - wz))) + (vector.Z * (xz + wy)),
((vector.X * (xy + wz)) + (vector.Y * ((1.0f - xx) - zz))) + (vector.Z * (yz - wx)),
((vector.X * (xz - wy)) + (vector.Y * (yz + wx))) + (vector.Z * ((1.0f - xx) - yy))
);
}
public static void Read(this XbimMeshGeometry3D m3D, byte[] mesh, XbimMatrix3D? transform = null)
{
var indexBase = m3D.Positions.Count;
var qrd = new XbimQuaternion();
XbimMatrix3D? matrix3D = null;
if (transform.HasValue)
{
qrd = transform.Value.GetRotationQuaternion();
matrix3D = transform.Value;
}
using (var ms = new MemoryStream(mesh))
{
using (var br = new BinaryReader(ms))
{
// ReSharper disable once UnusedVariable
var version = br.ReadByte(); //stream format version
var numVertices = br.ReadInt32();
var numTriangles = br.ReadInt32();
var uniqueVertices = new List<XbimPoint3D>(numVertices);
var vertices = new List<XbimPoint3D>(numVertices * 4); //approx the size
var triangleIndices = new List<int>(numTriangles * 3);
var normals = new List<XbimVector3D>(numVertices * 4);
for (var i = 0; i < numVertices; i++)
{
double x = br.ReadSingle();
double y = br.ReadSingle();
double z = br.ReadSingle();
var p = new XbimPoint3D(x, y, z);
if (matrix3D.HasValue)
p = matrix3D.Value.Transform(p);
uniqueVertices.Add(p);
}
var numFaces = br.ReadInt32();
for (var i = 0; i < numFaces; i++)
{
var numTrianglesInFace = br.ReadInt32();
if (numTrianglesInFace == 0) continue;
var isPlanar = numTrianglesInFace > 0;
numTrianglesInFace = Math.Abs(numTrianglesInFace);
if (isPlanar)
{
var normal = br.ReadPackedNormal().Normal;
if (!qrd.IsIdentity())
{
var baseVal = new XbimVector3D(normal.X, normal.Y, normal.Z);
XbimQuaternion.Transform(ref baseVal, ref qrd, out normal);
}
var uniqueIndices = new Dictionary<int, int>();
for (var j = 0; j < numTrianglesInFace; j++)
{
for (var k = 0; k < 3; k++)
{
var idx = ReadIndex(br, numVertices);
int writtenIdx;
if (!uniqueIndices.TryGetValue(idx, out writtenIdx)) //we haven't got it, so add it
{
writtenIdx = vertices.Count;
vertices.Add(uniqueVertices[idx]);
uniqueIndices.Add(idx, writtenIdx);
//add a matching normal
normals.Add(normal);
}
triangleIndices.Add(indexBase + writtenIdx);
}
}
}
else
{
var uniqueIndices = new Dictionary<int, int>();
for (var j = 0; j < numTrianglesInFace; j++)
{
for (var k = 0; k < 3; k++)
{
var idx = ReadIndex(br, numVertices);
var normal = br.ReadPackedNormal().Normal;
int writtenIdx;
if (!uniqueIndices.TryGetValue(idx, out writtenIdx)) //we haven't got it, so add it
{
writtenIdx = vertices.Count;
vertices.Add(uniqueVertices[idx]);
uniqueIndices.Add(idx, writtenIdx);
if (!qrd.IsIdentity())
{
var baseVal = new XbimVector3D(normal.X, normal.Y, normal.Z);
XbimQuaternion.Transform(ref baseVal, ref qrd, out normal);
}
normals.Add(normal);
}
triangleIndices.Add(indexBase + writtenIdx);
}
}
}
}
m3D.Positions = m3D.Positions.Concat(vertices).ToList();
m3D.TriangleIndices = m3D.TriangleIndices.Concat(triangleIndices).ToList();
m3D.Normals = m3D.Normals.Concat(normals).ToList();
}
}
}
/// <summary>
/// Reads an ascii string of Xbim mesh geometry data
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
public bool Read(String data, XbimMatrix3D? trans = null)
{
var version = 2; //we are at at least verson 2 now
var q = new XbimQuaternion();
if (trans.HasValue)
q = trans.Value.GetRotationQuaternion();
using (var sr = new StringReader(data))
{
var vertexList = new List<XbimPoint3D>(); //holds the actual positions of the vertices in this data set in the mesh
var normalList = new List<XbimVector3D>(); //holds the actual normals of the vertices in this data set in the mesh
String line;
// Read and display lines from the data until the end of
// the data is reached.
while ((line = sr.ReadLine()) != null)
{
var tokens = line.Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
if (tokens.Length > 1) //we need a command and some data
{
var command = tokens[0].Trim().ToUpper();
switch (command)
{
case "P":
version = Int32.Parse(tokens[1]);
var pointCount = 512;
//var faceCount = 128;
//var triangleCount = 256;
var normalCount = 512;
if (tokens.Length > 1) pointCount = Int32.Parse(tokens[2]);
// if (tokens.Length > 2) faceCount = Int32.Parse(tokens[3]);
// if (tokens.Length > 3) triangleCount = Int32.Parse(tokens[4]);
//version 2 of the string format uses packed normals
if (version < 2 && tokens.Length > 4) normalCount = Int32.Parse(tokens[5]);
vertexList = new List<XbimPoint3D>(pointCount);
normalList = new List<XbimVector3D>(normalCount);
break;
case "V": //process vertices
for (var i = 1; i < tokens.Length; i++)
{
var xyz = tokens[i].Split(',');
var p = new XbimPoint3D(Convert.ToDouble(xyz[0], CultureInfo.InvariantCulture),
Convert.ToDouble(xyz[1], CultureInfo.InvariantCulture),
Convert.ToDouble(xyz[2], CultureInfo.InvariantCulture));
if (trans.HasValue)
p = trans.Value.Transform(p);
vertexList.Add(p);
}
break;
case "N": //processes normals
for (var i = 1; i < tokens.Length; i++)
{
var xyz = tokens[i].Split(',');
var v = new XbimVector3D(Convert.ToDouble(xyz[0], CultureInfo.InvariantCulture),
Convert.ToDouble(xyz[1], CultureInfo.InvariantCulture),
Convert.ToDouble(xyz[2], CultureInfo.InvariantCulture));
normalList.Add(v);
}
break;
case "T": //process triangulated meshes
var currentNormal = XbimVector3D.Zero;
//each time we start a new mesh face we have to duplicate the vertices to ensure that we get correct shading of planar and non planar faces
var writtenVertices = new Dictionary<int, int>();
for (var i = 1; i < tokens.Length; i++)
{
var triangleIndices = tokens[i].Split(new[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
if (triangleIndices.Length != 3) throw new Exception("Invalid triangle definition");
for (var t = 0; t < 3; t++)
{
var indexNormalPair = triangleIndices[t].Split(new[] { '/' }, StringSplitOptions.RemoveEmptyEntries);
if (indexNormalPair.Length > 1) //we have a normal defined
{
var normalStr = indexNormalPair[1].Trim();
if (version < 2)
{
switch (normalStr)
{
case "F": //Front
currentNormal = new XbimVector3D(0, -1, 0);
break;
case "B": //Back
currentNormal = new XbimVector3D(0, 1, 0);
break;
case "L": //Left
currentNormal = new XbimVector3D(-1, 0, 0);
break;
case "R": //Right
currentNormal = new XbimVector3D(1, 0, 0);
break;
case "U": //Up
currentNormal = new XbimVector3D(0, 0, 1);
break;
case "D": //Down
currentNormal = new XbimVector3D(0, 0, -1);
break;
default: //it is an index number
var normalIndex = int.Parse(indexNormalPair[1]);
currentNormal = normalList[normalIndex];
break;
}
}
else
{
var normalIndex = ushort.Parse(indexNormalPair[1]);
var packedNormal = new XbimPackedNormal(normalIndex);
currentNormal = packedNormal.Normal;
}
if (trans.HasValue)
{
XbimVector3D v;
XbimQuaternion.Transform(ref currentNormal, ref q, out v);
currentNormal = v;
}
}
//now add the index
var index = int.Parse(indexNormalPair[0]);
int alreadyWrittenAt;
if (!writtenVertices.TryGetValue(index, out alreadyWrittenAt)) //if we haven't written it in this mesh pass, add it again unless it is the first one which we know has been written
{
//all vertices will be unique and have only one normal
writtenVertices.Add(index, PositionCount);
TriangleIndices.Add(PositionCount);
Positions.Add(vertexList[index]);
Normals.Add(currentNormal);
}
else //just add the index reference
{
TriangleIndices.Add(alreadyWrittenAt);
}
}
}
break;
case "F":
break;
default:
throw new Exception("Invalid Geometry Command");
}
}
}
}
return true;
}
/// <summary>
/// Decomposes a matrix into a scale, rotation, and translation.
/// </summary>
/// <param name="scale">When the method completes, contains the scaling component of the decomposed matrix.</param>
/// <param name="rotation">When the method completes, contains the rtoation component of the decomposed matrix.</param>
/// <param name="translation">When the method completes, contains the translation component of the decomposed matrix.</param>
/// <remarks>
/// This method is designed to decompose an SRT transformation matrix only.
/// </remarks>
public bool Decompose(out XbimVector3D scale, out XbimQuaternion rotation, out XbimVector3D translation)
{
//Source: Unknown
// References: http://www.gamedev.net/community/forums/topic.asp?topic_id=441695
// via https://code.google.com/p/sharpdx/source/browse/Source/SharpDX/Matrix.cs?r=9f9e209b1be04f06f294bc6d72b06055ad6abdcc
//Get the translation.
translation = new XbimVector3D();
translation.X = this._offsetX;
translation.Y = this._offsetY;
translation.Z = this._offsetZ;
//Scaling is the length of the rows.
scale = new XbimVector3D();
scale.X = Math.Sqrt((M11 * M11) + (M12 * M12) + (M13 * M13));
scale.Y = Math.Sqrt((M21 * M21) + (M22 * M22) + (M23 * M23));
scale.Z = Math.Sqrt((M31 * M31) + (M32 * M32) + (M33 * M33));
//If any of the scaling factors are zero, than the rotation matrix can not exist.
//
double ZeroTolerance = 0.000003;
if (Math.Abs(scale.X) < ZeroTolerance ||
Math.Abs(scale.Y) < ZeroTolerance ||
Math.Abs(scale.Z) < ZeroTolerance)
{
rotation = new XbimQuaternion(); // defaults to identity
return false;
}
//The rotation is the left over matrix after dividing out the scaling.
XbimMatrix3D rotationmatrix = new XbimMatrix3D();
rotationmatrix.M11 = M11 / scale.X;
rotationmatrix.M12 = M12 / scale.X;
rotationmatrix.M13 = M13 / scale.X;
rotationmatrix.M21 = M21 / scale.Y;
rotationmatrix.M22 = M22 / scale.Y;
rotationmatrix.M23 = M23 / scale.Y;
rotationmatrix.M31 = M31 / scale.Z;
rotationmatrix.M32 = M32 / scale.Z;
rotationmatrix.M33 = M33 / scale.Z;
rotationmatrix.M44 = 1;
XbimQuaternion.RotationMatrix(ref rotationmatrix, out rotation);
return true;
}
static XbimQuaternion()
{
_identity = new XbimQuaternion(0.0, 0.0, 0.0, 1.0);
_identity._isNotDefaultInitialised = true;
}
/// <summary>
/// Creates a quaternion given a rotation matrix.
/// </summary>
/// <param name="matrix">The rotation matrix.</param>
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
public static void RotationMatrix(ref XbimMatrix3D matrix, out XbimQuaternion result)
{
double sqrt;
double half;
double scale = matrix.M11 + matrix.M22 + matrix.M33;
result = new XbimQuaternion();
if (scale > 0.0f)
{
sqrt = Math.Sqrt(scale + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (matrix.M23 - matrix.M32) * sqrt;
result.Y = (matrix.M31 - matrix.M13) * sqrt;
result.Z = (matrix.M12 - matrix.M21) * sqrt;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
sqrt = Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (matrix.M12 + matrix.M21) * half;
result.Z = (matrix.M13 + matrix.M31) * half;
result.W = (matrix.M23 - matrix.M32) * half;
}
else if (matrix.M22 > matrix.M33)
{
sqrt = Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33);
half = 0.5f / sqrt;
result.X = (matrix.M21 + matrix.M12) * half;
result.Y = 0.5f * sqrt;
result.Z = (matrix.M32 + matrix.M23) * half;
result.W = (matrix.M31 - matrix.M13) * half;
}
else
{
sqrt = Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22);
half = 0.5f / sqrt;
result.X = (matrix.M31 + matrix.M13) * half;
result.Y = (matrix.M32 + matrix.M23) * half;
result.Z = 0.5f * sqrt;
result.W = (matrix.M12 - matrix.M21) * half;
}
}
static XbimQuaternion()
{
_identity = new XbimQuaternion(0.0, 0.0, 0.0, 1.0);
_identity._isNotDefaultInitialised = true;
}
/// <summary>
/// Transforms a 3D vector by the given <see cref="SharpDX.Quaternion"/> rotation.
/// </summary>
/// <param name="vector">The vector to rotate.</param>
/// <param name="rotation">The <see cref="SharpDX.Quaternion"/> rotation to apply.</param>
/// <param name="result">When the method completes, contains the transformed <see cref="SharpDX.Vector4"/>.</param>
public static void Transform(ref XbimVector3D vector, ref XbimQuaternion rotation, out XbimVector3D result)
{
double x = rotation.X + rotation.X;
double y = rotation.Y + rotation.Y;
double z = rotation.Z + rotation.Z;
double wx = rotation.W * x;
double wy = rotation.W * y;
double wz = rotation.W * z;
double xx = rotation.X * x;
double xy = rotation.X * y;
double xz = rotation.X * z;
double yy = rotation.Y * y;
double yz = rotation.Y * z;
double zz = rotation.Z * z;
result = new XbimVector3D(
((vector.X * ((1.0f - yy) - zz)) + (vector.Y * (xy - wz))) + (vector.Z * (xz + wy)),
((vector.X * (xy + wz)) + (vector.Y * ((1.0f - xx) - zz))) + (vector.Z * (yz - wx)),
((vector.X * (xz - wy)) + (vector.Y * (yz + wx))) + (vector.Z * ((1.0f - xx) - yy))
);
}
public XbimPackedNormal Transform(XbimQuaternion q)
{
XbimVector3D v1 = Normal;
XbimVector3D v2;
XbimQuaternion.Transform(ref v1, ref q, out v2);
return new XbimPackedNormal(v2);
}
