private void SetVector4Value(BulletWriter writer, XmlNode valueNode, int offset)
{
int i = 0;
foreach (string value in valueNode.InnerText.Split(' '))
{
if (!string.IsNullOrWhiteSpace(value))
{
writer.Write(Scalar.Parse(value, CultureInfo.InvariantCulture), offset + i * sizeof(Scalar));
i++;
if (i == 4)
{
break;
}
}
}
}
private void DeSerializeGeneric6DofConstraintData(XmlElement element)
{
int ptr = int.Parse(element.GetAttribute("pointer"));
byte[] dof6Data = new byte[Marshal.SizeOf(typeof(Generic6DofConstraintFloatData))];
using (var stream = new MemoryStream(dof6Data))
{
using (var writer = new BulletWriter(stream))
{
XmlNode node = element["m_typeConstraintData"];
if (node == null)
{
return;
}
SetPointerValue(writer, node["m_rbA"], TypedConstraintFloatData.Offset("RigidBodyA"));
SetPointerValue(writer, node["m_rbB"], TypedConstraintFloatData.Offset("RigidBodyB"));
writer.Write(0, TypedConstraintFloatData.Offset("Name"));
SetIntValue(writer, node["m_objectType"], TypedConstraintFloatData.Offset("ObjectType"));
SetIntValue(writer, node["m_userConstraintType"], TypedConstraintFloatData.Offset("UserConstraintType"));
SetIntValue(writer, node["m_userConstraintId"], TypedConstraintFloatData.Offset("UserConstraintId"));
SetIntValue(writer, node["m_needsFeedback"], TypedConstraintFloatData.Offset("NeedsFeedback"));
SetFloatValue(writer, node["m_appliedImpulse"], TypedConstraintFloatData.Offset("AppliedImpulse"));
SetFloatValue(writer, node["m_dbgDrawSize"], TypedConstraintFloatData.Offset("DebugDrawSize"));
SetIntValue(writer, node["m_disableCollisionsBetweenLinkedBodies"], TypedConstraintFloatData.Offset("DisableCollisionsBetweenLinkedBodies"));
SetIntValue(writer, node["m_overrideNumSolverIterations"], TypedConstraintFloatData.Offset("OverrideNumSolverIterations"));
SetFloatValue(writer, node["m_breakingImpulseThreshold"], TypedConstraintFloatData.Offset("BreakingImpulseThreshold"));
SetIntValue(writer, node["m_isEnabled"], TypedConstraintFloatData.Offset("IsEnabled"));
SetTransformValue(writer, element["m_rbAFrame"], Generic6DofConstraintFloatData.Offset("RigidBodyAFrame"));
SetTransformValue(writer, element["m_rbBFrame"], Generic6DofConstraintFloatData.Offset("RigidBodyBFrame"));
SetVector4Value(writer, element["m_linearUpperLimit"], Generic6DofConstraintFloatData.Offset("LinearUpperLimit"));
SetVector4Value(writer, element["m_linearLowerLimit"], Generic6DofConstraintFloatData.Offset("LinearLowerLimit"));
SetVector4Value(writer, element["m_angularUpperLimit"], Generic6DofConstraintFloatData.Offset("AngularUpperLimit"));
SetVector4Value(writer, element["m_angularLowerLimit"], Generic6DofConstraintFloatData.Offset("AngularLowerLimit"));
SetIntValue(writer, element["m_useLinearReferenceFrameA"], Generic6DofConstraintFloatData.Offset("UseLinearReferenceFrameA"));
SetIntValue(writer, element["m_useOffsetForConstraintFrame"], Generic6DofConstraintFloatData.Offset("UseOffsetForConstraintFrame"));
}
}
_constraintData.Add(dof6Data);
}
private void SetPointerValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(new IntPtr(long.Parse(valueNode.InnerText)), offset);
}
private void SetIntValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(int.Parse(valueNode.InnerText), offset);
}
private void SetFloatValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(Scalar.Parse(valueNode.InnerText, CultureInfo.InvariantCulture), offset);
}
public bool ConvertAllObjects(BulletFile file)
{
_shapeMap.Clear();
_bodyMap.Clear();
foreach (byte[] bvhData in file.Bvhs)
{
OptimizedBvh bvh = CreateOptimizedBvh();
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
// QuantizedBvhData is parsed in C++, so we need to actually fix pointers
GCHandle bvhDataHandle = GCHandle.Alloc(bvhData, GCHandleType.Pinned);
IntPtr bvhDataPinnedPtr = bvhDataHandle.AddrOfPinnedObject();
IntPtr contiguousNodesHandlePtr = IntPtr.Zero;
IntPtr quantizedContiguousNodesHandlePtr = IntPtr.Zero;
IntPtr subTreeInfoHandlePtr = IntPtr.Zero;
using (var stream = new MemoryStream(bvhData))
{
using (var reader = new BulletReader(stream))
{
long contiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("ContiguousNodesPtr"));
long quantizedContiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr"));
long subTreeInfoPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("SubTreeInfoPtr"));
using (var writer = new BulletWriter(stream))
{
if (contiguousNodesPtr != 0)
{
GCHandle contiguousNodesHandle = GCHandle.Alloc(file.LibPointers[contiguousNodesPtr], GCHandleType.Pinned);
contiguousNodesHandlePtr = GCHandle.ToIntPtr(contiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("ContiguousNodesPtr");
writer.Write(contiguousNodesHandle.AddrOfPinnedObject());
}
if (quantizedContiguousNodesPtr != 0)
{
GCHandle quantizedContiguousNodesHandle = GCHandle.Alloc(file.LibPointers[quantizedContiguousNodesPtr], GCHandleType.Pinned);
quantizedContiguousNodesHandlePtr = GCHandle.ToIntPtr(quantizedContiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr");
writer.Write(quantizedContiguousNodesHandle.AddrOfPinnedObject());
}
if (subTreeInfoPtr != 0)
{
GCHandle subTreeInfoHandle = GCHandle.Alloc(file.LibPointers[subTreeInfoPtr], GCHandleType.Pinned);
subTreeInfoHandlePtr = GCHandle.ToIntPtr(subTreeInfoHandle);
stream.Position = QuantizedBvhFloatData.Offset("SubTreeInfoPtr");
writer.Write(subTreeInfoHandle.AddrOfPinnedObject());
}
}
}
}
bvh.DeSerializeFloat(bvhDataPinnedPtr);
bvhDataHandle.Free();
if (contiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(contiguousNodesHandlePtr).Free();
}
if (quantizedContiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(quantizedContiguousNodesHandlePtr).Free();
}
if (subTreeInfoHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(subTreeInfoHandlePtr).Free();
}
}
foreach (KeyValuePair <long, byte[]> lib in file.LibPointers)
{
if (lib.Value == bvhData)
{
_bvhMap.Add(lib.Key, bvh);
break;
}
}
}
foreach (byte[] shapeData in file.CollisionShapes)
{
CollisionShape shape = ConvertCollisionShape(shapeData, file.LibPointers);
if (shape != null)
{
foreach (KeyValuePair <long, byte[]> lib in file.LibPointers)
{
if (lib.Value == shapeData)
{
_shapeMap.Add(lib.Key, shape);
break;
}
}
using (var stream = new MemoryStream(shapeData, false))
{
using (var reader = new BulletReader(stream))
{
long namePtr = reader.ReadPtr(CollisionShapeFloatData.Offset("Name"));
if (namePtr != 0)
{
byte[] nameData = file.LibPointers[namePtr];
int length = Array.IndexOf(nameData, (byte)0);
string name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
_objectNameMap.Add(shape, name);
_nameShapeMap.Add(name, shape);
}
}
}
}
}
foreach (byte[] solverInfoData in file.DynamicsWorldInfo)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
//throw new NotImplementedException();
}
else
{
//throw new NotImplementedException();
}
}
foreach (byte[] bodyData in file.RigidBodies)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertRigidBodyFloat(bodyData, file.LibPointers);
}
}
foreach (byte[] colObjData in file.CollisionObjects)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
using (var colObjStream = new MemoryStream(colObjData, false))
{
using (var colObjReader = new BulletReader(colObjStream))
{
long shapePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("CollisionShape"));
CollisionShape shape = _shapeMap[shapePtr];
Math.Matrix startTransform = colObjReader.ReadMatrix(CollisionObjectFloatData.Offset("WorldTransform"));
long namePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("Name"));
string name = null;
if (namePtr != 0)
{
byte[] nameData = file.FindLibPointer(namePtr);
int length = Array.IndexOf(nameData, (byte)0);
name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
}
CollisionObject colObj = CreateCollisionObject(ref startTransform, shape, name);
_bodyMap.Add(colObjData, colObj);
}
}
}
}
foreach (byte[] constraintData in file.Constraints)
{
var stream = new MemoryStream(constraintData, false);
using (var reader = new BulletReader(stream))
{
long collisionObjectAPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyA"));
long collisionObjectBPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyB"));
RigidBody a = null, b = null;
if (collisionObjectAPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectAPtr))
{
a = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectAPtr];
a = RigidBody.Upcast(_bodyMap[coData]);
if (a == null)
{
a = TypedConstraint.GetFixedBody();
}
}
}
if (collisionObjectBPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectBPtr))
{
b = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectBPtr];
b = RigidBody.Upcast(_bodyMap[coData]);
if (b == null)
{
b = TypedConstraint.GetFixedBody();
}
}
}
if (a == null && b == null)
{
stream.Dispose();
continue;
}
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertConstraintFloat(a, b, constraintData, file.Version, file.LibPointers);
}
}
stream.Dispose();
}
return(true);
}
private void DeSerializeCollisionShapeData(XmlElement parent, BulletWriter writer)
{
SetIntValue(writer, parent["m_shapeType"], CollisionShapeData.Offset("ShapeType"));
writer.Write(0, CollisionShapeData.Offset("Name"));
}
private void SetVector4Value(BulletWriter writer, XmlNode valueNode, int offset)
{
int i = 0;
foreach (string value in valueNode.InnerText.Split(' '))
{
if (!string.IsNullOrWhiteSpace(value))
{
writer.Write(float.Parse(value, CultureInfo.InvariantCulture), offset + i * sizeof(float));
i++;
if (i == 4)
{
break;
}
}
}
}
public bool ConvertAllObjects(BulletFile file)
{
_shapeMap.Clear();
_bodyMap.Clear();
foreach (byte[] bvhData in file._bvhs)
{
OptimizedBvh bvh = CreateOptimizedBvh();
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
// QuantizedBvhData is parsed in C++, so we need to actually fix pointers
GCHandle bvhDataHandle = GCHandle.Alloc(bvhData, GCHandleType.Pinned);
IntPtr bvhDataPinnedPtr = bvhDataHandle.AddrOfPinnedObject();
IntPtr contiguousNodesHandlePtr = IntPtr.Zero;
IntPtr quantizedContiguousNodesHandlePtr = IntPtr.Zero;
IntPtr subTreeInfoHandlePtr = IntPtr.Zero;
using (MemoryStream stream = new MemoryStream(bvhData))
{
using (BulletReader reader = new BulletReader(stream))
{
long contiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("ContiguousNodesPtr"));
long quantizedContiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr"));
long subTreeInfoPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("SubTreeInfoPtr"));
using (BulletWriter writer = new BulletWriter(stream))
{
if (contiguousNodesPtr != 0)
{
GCHandle contiguousNodesHandle = GCHandle.Alloc(file.LibPointers[contiguousNodesPtr], GCHandleType.Pinned);
contiguousNodesHandlePtr = GCHandle.ToIntPtr(contiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("ContiguousNodesPtr");
writer.Write(contiguousNodesHandle.AddrOfPinnedObject());
}
if (quantizedContiguousNodesPtr != 0)
{
GCHandle quantizedContiguousNodesHandle = GCHandle.Alloc(file.LibPointers[quantizedContiguousNodesPtr], GCHandleType.Pinned);
quantizedContiguousNodesHandlePtr = GCHandle.ToIntPtr(quantizedContiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr");
writer.Write(quantizedContiguousNodesHandle.AddrOfPinnedObject());
}
if (subTreeInfoPtr != 0)
{
GCHandle subTreeInfoHandle = GCHandle.Alloc(file.LibPointers[subTreeInfoPtr], GCHandleType.Pinned);
subTreeInfoHandlePtr = GCHandle.ToIntPtr(subTreeInfoHandle);
stream.Position = QuantizedBvhFloatData.Offset("SubTreeInfoPtr");
writer.Write(subTreeInfoHandle.AddrOfPinnedObject());
}
}
}
}
bvh.DeSerializeFloat(bvhDataPinnedPtr);
bvhDataHandle.Free();
if (contiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(contiguousNodesHandlePtr).Free();
}
if (quantizedContiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(quantizedContiguousNodesHandlePtr).Free();
}
if (subTreeInfoHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(subTreeInfoHandlePtr).Free();
}
}
foreach (KeyValuePair<long, byte[]> lib in file.LibPointers)
{
if (lib.Value == bvhData)
{
_bvhMap.Add(lib.Key, bvh);
break;
}
}
}
foreach (byte[] shapeData in file._collisionShapes)
{
CollisionShape shape = ConvertCollisionShape(shapeData, file.LibPointers);
if (shape != null)
{
foreach (KeyValuePair<long, byte[]> lib in file.LibPointers)
{
if (lib.Value == shapeData)
{
_shapeMap.Add(lib.Key, shape);
break;
}
}
using (MemoryStream stream = new MemoryStream(shapeData, false))
{
using (BulletReader reader = new BulletReader(stream))
{
long namePtr = reader.ReadPtr(CollisionShapeFloatData.Offset("Name"));
if (namePtr != 0)
{
byte[] nameData = file.LibPointers[namePtr];
int length = Array.IndexOf(nameData, (byte)0);
string name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
_objectNameMap.Add(shape, name);
_nameShapeMap.Add(name, shape);
}
}
}
}
}
foreach (byte[] solverInfoData in file._dynamicsWorldInfo)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
//throw new NotImplementedException();
}
else
{
//throw new NotImplementedException();
}
}
foreach (byte[] bodyData in file._rigidBodies)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertRigidBodyFloat(bodyData, file.LibPointers);
}
}
foreach (byte[] colObjData in file._collisionObjects)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
using (MemoryStream colObjStream = new MemoryStream(colObjData, false))
{
using (BulletReader colObjReader = new BulletReader(colObjStream))
{
long shapePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("CollisionShape"));
CollisionShape shape = _shapeMap[shapePtr];
Math.Matrix startTransform = colObjReader.ReadMatrix(CollisionObjectFloatData.Offset("WorldTransform"));
long namePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("Name"));
string name = null;
if (namePtr != 0)
{
byte[] nameData = file.FindLibPointer(namePtr);
int length = Array.IndexOf(nameData, (byte)0);
name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
}
CollisionObject colObj = CreateCollisionObject(ref startTransform, shape, name);
_bodyMap.Add(colObjData, colObj);
}
}
}
}
foreach (byte[] constraintData in file._constraints)
{
MemoryStream stream = new MemoryStream(constraintData, false);
using (BulletReader reader = new BulletReader(stream))
{
long collisionObjectAPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyA"));
long collisionObjectBPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyB"));
RigidBody a = null, b = null;
if (collisionObjectAPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectAPtr))
{
a = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectAPtr];
a = RigidBody.Upcast(_bodyMap[coData]);
if (a == null)
{
a = TypedConstraint.GetFixedBody();
}
}
}
if (collisionObjectBPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectBPtr))
{
b = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectBPtr];
b = RigidBody.Upcast(_bodyMap[coData]);
if (b == null)
{
b = TypedConstraint.GetFixedBody();
}
}
}
if (a == null && b == null)
{
stream.Dispose();
continue;
}
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertConstraintFloat(a, b, constraintData, file.Version, file.LibPointers);
}
}
stream.Dispose();
}
return true;
}
private void SetPointerValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(new IntPtr(long.Parse(valueNode.InnerText)), offset);
}
private void SetIntValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(int.Parse(valueNode.InnerText), offset);
}
private void SetFloatValue(BulletWriter writer, XmlNode valueNode, int offset)
{
writer.Write(float.Parse(valueNode.InnerText, CultureInfo.InvariantCulture), offset);
}
private void DeSerializeGeneric6DofConstraintData(XmlElement element)
{
int ptr = int.Parse(element.GetAttribute("pointer"));
byte[] dof6Data = new byte[Marshal.SizeOf(typeof(Generic6DofConstraintFloatData))];
using (var stream = new MemoryStream(dof6Data))
{
using (var writer = new BulletWriter(stream))
{
XmlNode node = element["m_typeConstraintData"];
if (node == null)
{
return;
}
SetPointerValue(writer, node["m_rbA"], TypedConstraintFloatData.Offset("RigidBodyA"));
SetPointerValue(writer, node["m_rbB"], TypedConstraintFloatData.Offset("RigidBodyB"));
writer.Write(0, TypedConstraintFloatData.Offset("Name"));
SetIntValue(writer, node["m_objectType"], TypedConstraintFloatData.Offset("ObjectType"));
SetIntValue(writer, node["m_userConstraintType"], TypedConstraintFloatData.Offset("UserConstraintType"));
SetIntValue(writer, node["m_userConstraintId"], TypedConstraintFloatData.Offset("UserConstraintId"));
SetIntValue(writer, node["m_needsFeedback"], TypedConstraintFloatData.Offset("NeedsFeedback"));
SetFloatValue(writer, node["m_appliedImpulse"], TypedConstraintFloatData.Offset("AppliedImpulse"));
SetFloatValue(writer, node["m_dbgDrawSize"], TypedConstraintFloatData.Offset("DebugDrawSize"));
SetIntValue(writer, node["m_disableCollisionsBetweenLinkedBodies"], TypedConstraintFloatData.Offset("DisableCollisionsBetweenLinkedBodies"));
SetIntValue(writer, node["m_overrideNumSolverIterations"], TypedConstraintFloatData.Offset("OverrideNumSolverIterations"));
SetFloatValue(writer, node["m_breakingImpulseThreshold"], TypedConstraintFloatData.Offset("BreakingImpulseThreshold"));
SetIntValue(writer, node["m_isEnabled"], TypedConstraintFloatData.Offset("IsEnabled"));
SetTransformValue(writer, element["m_rbAFrame"], Generic6DofConstraintFloatData.Offset("RigidBodyAFrame"));
SetTransformValue(writer, element["m_rbBFrame"], Generic6DofConstraintFloatData.Offset("RigidBodyBFrame"));
SetVector4Value(writer, element["m_linearUpperLimit"], Generic6DofConstraintFloatData.Offset("LinearUpperLimit"));
SetVector4Value(writer, element["m_linearLowerLimit"], Generic6DofConstraintFloatData.Offset("LinearLowerLimit"));
SetVector4Value(writer, element["m_angularUpperLimit"], Generic6DofConstraintFloatData.Offset("AngularUpperLimit"));
SetVector4Value(writer, element["m_angularLowerLimit"], Generic6DofConstraintFloatData.Offset("AngularLowerLimit"));
SetIntValue(writer, element["m_useLinearReferenceFrameA"], Generic6DofConstraintFloatData.Offset("UseLinearReferenceFrameA"));
SetIntValue(writer, element["m_useOffsetForConstraintFrame"], Generic6DofConstraintFloatData.Offset("UseOffsetForConstraintFrame"));
}
}
_constraintData.Add(dof6Data);
}
private void DeSerializeCollisionShapeData(XmlElement parent, BulletWriter writer)
{
SetIntValue(writer, parent["m_shapeType"], CollisionShapeFloatData.Offset("ShapeType"));
writer.Write(0, CollisionShapeFloatData.Offset("Name"));
}