public virtual void Update(float elapsedTime)
{
if (pauseSimulation)
{
return;
}
elapsedTime *= simulationSpeed;
if (numSubSteps > 1)
{
int updateTime = Math.Max((int)(Math.Round(elapsedTime / simulationTimeStep)), 1);
updateTime = Math.Min(numSubSteps, updateTime);
for (int i = 0; i < updateTime; i++)
{
HavokDllBridge.update(simulationTimeStep);
}
}
else
{
HavokDllBridge.update(elapsedTime);
}
IntPtr bodyPtr = Marshal.AllocHGlobal(objectIDs.Count * sizeof(int));
IntPtr transformPtr = Marshal.AllocHGlobal(objectIDs.Count * sizeof(float) * 16);
int totalSize = 0;
HavokDllBridge.get_updated_transforms(bodyPtr, transformPtr, ref totalSize);
unsafe
{
int * bodyAddr = (int *)bodyPtr;
IntPtr tmpPtr = transformPtr;
float[] mat = new float[16];
IntPtr body = IntPtr.Zero;
for (int i = 0; i < totalSize; i++)
{
body = new IntPtr(*bodyAddr);
if (reverseIDs.ContainsKey(body))
{
tmpVec1 = scaleTable[body];
Matrix.CreateScale(ref tmpVec1, out tmpMat1);
Marshal.Copy(tmpPtr, mat, 0, mat.Length);
MatrixHelper.FloatsToMatrix(mat, out tmpMat2);
Matrix.Multiply(ref tmpMat1, ref tmpMat2, out tmpMat1);
reverseIDs[body].PhysicsWorldTransform = tmpMat1;
}
tmpPtr = new IntPtr(tmpPtr.ToInt32() + sizeof(float) * 16);
bodyAddr++;
}
}
Marshal.FreeHGlobal(bodyPtr);
Marshal.FreeHGlobal(transformPtr);
}
public void Dispose()
{
HavokDllBridge.dispose();
objectIDs.Clear();
reverseIDs.Clear();
scaleTable.Clear();
}
public void SetAngularVelocity(IPhysicsObject physObj, Vector3 velocity)
{
if (!objectIDs.ContainsKey(physObj))
{
return;
}
HavokDllBridge.set_angular_velocity(objectIDs[physObj], Vector3Helper.ToFloats(velocity));
}
public void AddTorque(IPhysicsObject physObj, float timeStep, Vector3 torque)
{
if (!objectIDs.ContainsKey(physObj))
{
return;
}
HavokDllBridge.add_torque(objectIDs[physObj], timeStep, Vector3Helper.ToFloats(torque));
}
public Vector3 GetAngularVelocity(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return(Vector3.Zero);
}
float[] velocity = new float[3];
HavokDllBridge.get_angular_velocity(objectIDs[physObj], velocity);
return(new Vector3(velocity[0], velocity[1], velocity[2]));
}
public Vector3 GetPosition(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return(Vector3.Zero);
}
float[] position = new float[3];
HavokDllBridge.get_body_position(objectIDs[physObj], position);
return(Vector3Helper.FromFloats(position));
}
public void RemovePhysicsObject(IPhysicsObject physObj)
{
if (objectIDs.ContainsKey(physObj))
{
HavokDllBridge.remove_rigid_body(objectIDs[physObj]);
reverseIDs.Remove(objectIDs[physObj]);
scaleTable.Remove(objectIDs[physObj]);
objectIDs.Remove(physObj);
}
}
public Quaternion GetRotation(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return(Quaternion.Identity);
}
float[] rotation = new float[4];
HavokDllBridge.get_body_rotation(objectIDs[physObj], rotation);
return(new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]));
}
public Matrix GetTransform(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return(Matrix.Identity);
}
float[] transform = new float[16];
HavokDllBridge.get_body_transform(objectIDs[physObj], transform);
return(MatrixHelper.FloatsToMatrix(transform));
}
public void InitializePhysics()
{
Vector3 g = info.Gravity * info.GravityDirection;
if (!HavokDllBridge.init_world(Vector3Helper.ToFloats(g), info.WorldSize,
info.CollisionTolerance, info.HavokSimulationType, info.HavokSolverType,
info.FireCollisionCallbacks, info.EnableDeactivation))
{
throw new GoblinException("Failed to initialize Havok physics");
}
}
public void ApplyHardKeyFrame(IPhysicsObject physObj, Vector3 newPos, Quaternion newRot, float timeStep)
{
if (!objectIDs.ContainsKey(physObj))
{
return;
}
float[] pos = Vector3Helper.ToFloats(ref newPos);
float[] rot = { newRot.X, newRot.Y, newRot.Z, newRot.W };
HavokDllBridge.apply_hard_keyframe(objectIDs[physObj], pos, rot, timeStep);
}
public BoundingBox GetAxisAlignedBoundingBox(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return(new BoundingBox());
}
float[] min = new float[3];
float[] max = new float[3];
HavokDllBridge.get_AABB(objectIDs[physObj], min, max);
Vector3Helper.FromFloats(min, out tmpVec1);
Vector3Helper.FromFloats(max, out tmpVec2);
return(new BoundingBox(tmpVec1, tmpVec2));
}
public void RestartsSimulation()
{
HavokDllBridge.dispose();
InitializePhysics();
List <IPhysicsObject> physObjs = new List <IPhysicsObject>(objectIDs.Keys);
objectIDs.Clear();
reverseIDs.Clear();
scaleTable.Clear();
foreach (IPhysicsObject physObj in physObjs)
{
AddPhysicsObject(physObj);
}
}
public void StopKeyframe(IPhysicsObject physObj)
{
if (!objectIDs.ContainsKey(physObj))
{
return;
}
Vector3 scale, trans;
Quaternion quat;
physObj.PhysicsWorldTransform.Decompose(out scale, out quat, out trans);
float[] pos = Vector3Helper.ToFloats(ref trans);
float[] rot = { quat.X, quat.Y, quat.Z, quat.W };
HavokDllBridge.apply_hard_keyframe(objectIDs[physObj], pos, rot, 0.016f);
}
public void ApplySoftKeyFrame(IPhysicsObject physObj, Vector3 newPos, Quaternion newRot,
Vector3 angularPositionFactor, Vector3 angularVelocityFactor, Vector3 linearPositionFactor,
Vector3 linearVelocityFactor, float maxAngularAcceleration, float maxLinearAcceleration,
float maxAllowedDistance, float timeStep)
{
if (!objectIDs.ContainsKey(physObj))
{
return;
}
float[] pos = Vector3Helper.ToFloats(ref newPos);
float[] rot = { newRot.X, newRot.Y, newRot.Z, newRot.W };
float[] angularPosFac = Vector3Helper.ToFloats(ref angularPositionFactor);
float[] angularVelFac = Vector3Helper.ToFloats(ref angularVelocityFactor);
float[] linearPosFac = Vector3Helper.ToFloats(ref linearPositionFactor);
float[] linearVelFac = Vector3Helper.ToFloats(ref linearVelocityFactor);
HavokDllBridge.apply_soft_keyframe(objectIDs[physObj], pos, rot, angularPosFac,
angularVelFac, linearPosFac, linearVelFac, maxAngularAcceleration,
maxLinearAcceleration, maxAllowedDistance, timeStep);
}
private IntPtr GetCollisionShape(IPhysicsObject physObj, Vector3 scale)
{
IntPtr collisionShape = IntPtr.Zero;
Vector3 boundingBox = Vector3Helper.GetDimensions(physObj.Model.MinimumBoundingBox);
float[] dim = new float[3];
float[] top = new float[3];
float[] bottom = new float[3];
float[] vertexCloud = null;
float radius = 0;
float height = 0;
float convexRadius = 0.05f;
if (physObj is HavokObject)
{
convexRadius = ((HavokObject)physObj).ConvexRadius;
}
switch (physObj.Shape)
{
case ShapeType.Box:
if (physObj.ShapeData.Count == 3)
{
dim[0] = physObj.ShapeData[0];
dim[1] = physObj.ShapeData[1];
dim[2] = physObj.ShapeData[2];
}
else
{
dim[0] = boundingBox.X * scale.X;
dim[1] = boundingBox.Y * scale.Y;
dim[2] = boundingBox.Z * scale.Z;
}
collisionShape = HavokDllBridge.create_box_shape(dim, convexRadius);
break;
case ShapeType.Sphere:
if (physObj.ShapeData.Count == 1)
{
radius = physObj.ShapeData[0];
}
else
{
radius = boundingBox.X * scale.X / 2;
}
collisionShape = HavokDllBridge.create_sphere_shape(radius);
break;
case ShapeType.Capsule:
case ShapeType.Cylinder:
if (physObj.ShapeData.Count == 2)
{
radius = physObj.ShapeData[0];
height = physObj.ShapeData[1];
}
else
{
radius = boundingBox.X * scale.X / 2;
height = boundingBox.Y * scale.Y;
}
if (physObj.Shape == ShapeType.Capsule)
{
top[1] = height / 2 - radius;
bottom[1] = -height / 2 + radius;
collisionShape = HavokDllBridge.create_capsule_shape(top, bottom, radius);
}
else
{
top[1] = height / 2;
bottom[1] = -height / 2;
collisionShape = HavokDllBridge.create_cylinder_shape(top, bottom, radius, convexRadius);
}
break;
case ShapeType.Cone:
throw new GoblinException("Cone shape is not supported by Havok physics");
case ShapeType.ChamferCylinder:
throw new GoblinException("ChamferCylinder is not supported by Havok physics");
case ShapeType.ConvexHull:
List <Vector3> vertices = physObj.MeshProvider.Vertices;
vertexCloud = new float[vertices.Count * 3];
for (int i = 0; i < vertices.Count; i++)
{
vertexCloud[i * 3] = vertices[i].X * scale.X;
vertexCloud[i * 3 + 1] = vertices[i].Y * scale.Y;
vertexCloud[i * 3 + 2] = vertices[i].Z * scale.Z;
}
collisionShape = HavokDllBridge.create_convex_shape(vertices.Count, vertexCloud,
sizeof(float) * 3, convexRadius);
break;
case ShapeType.TriangleMesh:
if (physObj.MeshProvider == null)
{
throw new GoblinException("MeshProvider cannot be null to construct TriangleMesh shape");
}
List <int> indices = physObj.MeshProvider.Indices;
vertices = physObj.MeshProvider.Vertices;
vertexCloud = new float[vertices.Count * 3];
for (int i = 0; i < vertices.Count; i++)
{
vertexCloud[i * 3] = vertices[i].X * scale.X;
vertexCloud[i * 3 + 1] = vertices[i].Y * scale.Y;
vertexCloud[i * 3 + 2] = vertices[i].Z * scale.Z;
}
collisionShape = HavokDllBridge.create_mesh_shape(vertices.Count, vertexCloud,
sizeof(float) * 3, indices.Count / 3, indices.ToArray(), convexRadius);
break;
case ShapeType.Compound:
break;
}
if (physObj is HavokObject)
{
if (((HavokObject)physObj).IsPhantom)
{
collisionShape = HavokDllBridge.create_phantom_shape(collisionShape,
((HavokObject)physObj).PhantomEnterCallback,
((HavokObject)physObj).PhantomLeaveCallback);
}
}
return(collisionShape);
}
public void SetBodyWorldLeaveCallback(HavokDllBridge.BodyLeaveWorldCallback callback)
{
HavokDllBridge.add_world_leave_callback(callback);
}
public void AddPhysicsObject(IPhysicsObject physObj)
{
if (objectIDs.ContainsKey(physObj))
{
return;
}
physObj.PhysicsWorldTransform = physObj.CompoundInitialWorldTransform;
HavokPhysics.MotionType motionType = MotionType.MOTION_INVALID;
HavokPhysics.CollidableQualityType qualityType =
CollidableQualityType.COLLIDABLE_QUALITY_INVALID;
float friction = -1;
float restitution = -1;
float maxLinearVelocity = -1;
float maxAngularVelocity = -1;
float allowedPenetrationDepth = -1;
float gravityFactor = 1;
if ((physObj is HavokObject))
{
HavokObject havokObj = (HavokObject)physObj;
motionType = havokObj.MotionType;
qualityType = havokObj.QualityType;
friction = havokObj.Friction;
restitution = havokObj.Restitution;
maxLinearVelocity = havokObj.MaxLinearVelocity;
maxAngularVelocity = havokObj.MaxAngularVelocity;
allowedPenetrationDepth = havokObj.AllowedPenetrationDepth;
gravityFactor = havokObj.GravityFactor;
}
else
{
bool isDynamic = (physObj.Mass != 0.0f && physObj.Interactable);
if (isDynamic)
{
motionType = MotionType.MOTION_DYNAMIC;
}
else
{
motionType = MotionType.MOTION_FIXED;
}
}
Quaternion rotation;
Vector3 trans;
Vector3 scale;
physObj.CompoundInitialWorldTransform.Decompose(out scale, out rotation, out trans);
IntPtr shape = GetCollisionShape(physObj, scale);
float[] pos = Vector3Helper.ToFloats(ref trans);
float[] rot = { rotation.X, rotation.Y, rotation.Z, rotation.W };
IntPtr body = HavokDllBridge.add_rigid_body(shape, physObj.Mass, motionType, qualityType,
pos, rot, Vector3Helper.ToFloats(physObj.InitialLinearVelocity), physObj.LinearDamping,
maxLinearVelocity, Vector3Helper.ToFloats(physObj.InitialAngularVelocity),
physObj.AngularDamping.X, maxAngularVelocity, friction, restitution,
allowedPenetrationDepth, physObj.NeverDeactivate, gravityFactor);
objectIDs.Add(physObj, body);
reverseIDs.Add(body, physObj);
scaleTable.Add(body, scale);
if ((physObj is HavokObject))
{
if (((HavokObject)physObj).ContactCallback != null ||
((HavokObject)physObj).CollisionStartCallback != null ||
((HavokObject)physObj).CollisionEndCallback != null)
{
HavokDllBridge.add_contact_listener(body, ((HavokObject)physObj).ContactCallback,
((HavokObject)physObj).CollisionStartCallback,
((HavokObject)physObj).CollisionEndCallback);
}
}
}
