private void DeSerializeRigidBodyFloatData(XmlElement element)
{
int ptr;
if (!int.TryParse(element.GetAttribute("pointer"), out ptr))
{
_fileOK = false;
return;
}
byte[] rbData = new byte[Marshal.SizeOf(typeof(RigidBodyFloatData))];
using (var stream = new MemoryStream(rbData))
{
using (var writer = new BulletWriter(stream))
{
XmlNode node = element["m_collisionObjectData"];
if (node == null)
{
return;
}
SetPointerValue(writer, node["m_collisionShape"], CollisionObjectFloatData.Offset("CollisionShape"));
SetTransformValue(writer, node["m_worldTransform"], CollisionObjectFloatData.Offset("WorldTransform"));
SetTransformValue(writer, node["m_interpolationWorldTransform"], CollisionObjectFloatData.Offset("InterpolationWorldTransform"));
SetVector4Value(writer, node["m_interpolationLinearVelocity"], CollisionObjectFloatData.Offset("InterpolationLinearVelocity"));
SetVector4Value(writer, node["m_interpolationAngularVelocity"], CollisionObjectFloatData.Offset("InterpolationAngularVelocity"));
SetVector4Value(writer, node["m_anisotropicFriction"], CollisionObjectFloatData.Offset("AnisotropicFriction"));
SetFloatValue(writer, node["m_contactProcessingThreshold"], CollisionObjectFloatData.Offset("ContactProcessingThreshold"));
SetFloatValue(writer, node["m_deactivationTime"], CollisionObjectFloatData.Offset("DeactivationTime"));
SetFloatValue(writer, node["m_friction"], CollisionObjectFloatData.Offset("Friction"));
SetFloatValue(writer, node["m_restitution"], CollisionObjectFloatData.Offset("Restitution"));
SetFloatValue(writer, node["m_hitFraction"], CollisionObjectFloatData.Offset("HitFraction"));
SetFloatValue(writer, node["m_ccdSweptSphereRadius"], CollisionObjectFloatData.Offset("CcdSweptSphereRadius"));
SetFloatValue(writer, node["m_ccdMotionThreshold"], CollisionObjectFloatData.Offset("CcdMotionThreshold"));
SetIntValue(writer, node["m_hasAnisotropicFriction"], CollisionObjectFloatData.Offset("HasAnisotropicFriction"));
SetIntValue(writer, node["m_collisionFlags"], CollisionObjectFloatData.Offset("CollisionFlags"));
SetIntValue(writer, node["m_islandTag1"], CollisionObjectFloatData.Offset("IslandTag1"));
SetIntValue(writer, node["m_companionId"], CollisionObjectFloatData.Offset("CompanionId"));
SetIntValue(writer, node["m_activationState1"], CollisionObjectFloatData.Offset("ActivationState1"));
SetIntValue(writer, node["m_internalType"], CollisionObjectFloatData.Offset("InternalType"));
SetIntValue(writer, node["m_checkCollideWith"], CollisionObjectFloatData.Offset("CheckCollideWith"));
SetMatrix3x3Value(writer, element["m_invInertiaTensorWorld"], RigidBodyFloatData.Offset("InvInertiaTensorWorld"));
SetVector4Value(writer, element["m_linearVelocity"], RigidBodyFloatData.Offset("LinearVelocity"));
SetVector4Value(writer, element["m_angularVelocity"], RigidBodyFloatData.Offset("AngularVelocity"));
SetVector4Value(writer, element["m_angularFactor"], RigidBodyFloatData.Offset("AngularFactor"));
SetVector4Value(writer, element["m_linearFactor"], RigidBodyFloatData.Offset("LinearFactor"));
SetVector4Value(writer, element["m_gravity"], RigidBodyFloatData.Offset("Gravity"));
SetVector4Value(writer, element["m_gravity_acceleration"], RigidBodyFloatData.Offset("GravityAcceleration"));
SetVector4Value(writer, element["m_invInertiaLocal"], RigidBodyFloatData.Offset("InvInertiaLocal"));
SetVector4Value(writer, element["m_totalTorque"], RigidBodyFloatData.Offset("TotalTorque"));
SetVector4Value(writer, element["m_totalForce"], RigidBodyFloatData.Offset("TotalForce"));
SetFloatValue(writer, element["m_inverseMass"], RigidBodyFloatData.Offset("InverseMass"));
SetFloatValue(writer, element["m_linearDamping"], RigidBodyFloatData.Offset("LinearDamping"));
SetFloatValue(writer, element["m_angularDamping"], RigidBodyFloatData.Offset("AngularDamping"));
SetFloatValue(writer, element["m_additionalDampingFactor"], RigidBodyFloatData.Offset("AdditionalDampingFactor"));
SetFloatValue(writer, element["m_additionalLinearDampingThresholdSqr"], RigidBodyFloatData.Offset("AdditionalLinearDampingThresholdSqr"));
SetFloatValue(writer, element["m_additionalAngularDampingThresholdSqr"], RigidBodyFloatData.Offset("AdditionalAngularDampingThresholdSqr"));
SetFloatValue(writer, element["m_additionalAngularDampingFactor"], RigidBodyFloatData.Offset("AdditionalAngularDampingFactor"));
SetFloatValue(writer, element["m_angularSleepingThreshold"], RigidBodyFloatData.Offset("AngularSleepingThreshold"));
SetFloatValue(writer, element["m_linearSleepingThreshold"], RigidBodyFloatData.Offset("LinearSleepingThreshold"));
SetIntValue(writer, element["m_additionalDamping"], RigidBodyFloatData.Offset("AdditionalDamping"));
}
}
_rigidBodyData.Add(rbData);
}
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);
}
