public static void SetOrientation(this BulletSharp.Math.Matrix bm, BulletSharp.Math.Quaternion q)
{
/*
* float d = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
* Debug.Assert(d != 0.0f);
* float s = 2.0f / d;
* float xs = value.X * s, ys = value.Y * s, zs = value.Z * s;
* float wx = value.W * xs, wy = value.W * ys, wz = value.W * zs;
* float xx = value.X * xs, xy = value.X * ys, xz = value.X * zs;
* float yy = value.Y * ys, yz = value.Y * zs, zz = value.Z * zs;
* M11 = 1.0f - (yy + zz); M12 = xy - wz; M13 = xz + wy;
* M21 = xy + wz; M22 = 1.0f - (xx + zz); M23 = yz - wx;
* M31 = xz - wy; M32 = yz + wx; M33 = 1.0f - (xx + yy);
*/
float xx = q.X * q.X;
float yy = q.Y * q.Y;
float zz = q.Z * q.Z;
float xy = q.X * q.Y;
float zw = q.Z * q.W;
float zx = q.Z * q.X;
float yw = q.Y * q.W;
float yz = q.Y * q.Z;
float xw = q.X * q.W;
bm.M11 = 1.0f - (2.0f * (yy + zz));
bm.M12 = 2.0f * (xy + zw);
bm.M13 = 2.0f * (zx - yw);
bm.M21 = 2.0f * (xy - zw);
bm.M22 = 1.0f - (2.0f * (zz + xx));
bm.M23 = 2.0f * (yz + xw);
bm.M31 = 2.0f * (zx + yw);
bm.M32 = 2.0f * (yz - xw);
bm.M33 = 1.0f - (2.0f * (yy + xx));
}
public static QuaternionB ToBullet(this Quaternion v, ref QuaternionB vb)
{
vb.X = v.x;
vb.Y = v.y;
vb.Z = v.z;
vb.W = v.w;
return(vb);
}
internal override bool _BuildCollisionObject()
{
WorldController world = GetWorld();
if (collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(this);
}
}
if (transform.localScale != Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (collisionObject == null)
{
collisionObject = new PairCachingGhostObject();
collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
collisionObject.WorldTransform = worldTrans;
collisionObject.UserObject = this;
collisionObject.CollisionFlags = collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
else
{
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
collisionObject.WorldTransform = worldTrans;
collisionObject.CollisionShape = cs;
collisionObject.CollisionFlags = collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
return(true);
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get;
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(this);
}
}
// if (transform.localScale != UnityEngine.Vector3.one)
// {
// Log.Error("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
// }
m_collisionShape = this.GetParent <Unit>().GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Log.Error("There was no collision shape component attached to this BRigidBody. ");
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new BulletSharp.PairCachingGhostObject();
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = this.GetParent <Unit>().Quaternion.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = this.GetParent <Unit>().Position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
else
{
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = this.GetParent <Unit>().Quaternion.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = this.GetParent <Unit>().Position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionShape = cs;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.KinematicObject;
m_collisionObject.CollisionFlags &= ~BulletSharp.CollisionFlags.StaticObject;
}
return(true);
}
//called by Physics World just before rigid body is added to world.
//the current rigid body properties are used to rebuild the rigid body.
internal virtual bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(m_collisionObject);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new CollisionObject();
m_collisionObject.CollisionShape = cs;
m_collisionObject.UserObject = this;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionFlags = m_collisionFlags;
}
else
{
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionFlags = m_collisionFlags;
}
return(true);
}
public ObjetoJuego(TgcMesh mesh, TGCVector3 translation = new TGCVector3(), TGCVector3 rotation = new TGCVector3(), float angle = 0)
{
this.mesh = mesh;
this.translation = new BulletSharp.Math.Vector3(translation.X, translation.Y, translation.Z);
scale = new BulletSharp.Math.Vector3();
this.rotation = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(rotation.X, rotation.Y, rotation.Z), angle);
AABB = new TgcBoundingAxisAlignBox();
}
public virtual void SetRotation(Quaternion rotation)
{
if (isInWorld)
{
BulletSharp.Math.Matrix newTrans = m_collisionObject.WorldTransform;
BulletSharp.Math.Quaternion q = rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out newTrans);
newTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = newTrans;
transform.rotation = rotation;
}
else
{
transform.rotation = rotation;
}
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get;
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
world.RemoveCollisionObject(this);
}
}
m_collisionShape = this.GetParent <Unit>().GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Log.Warning("There was no collision shape component attached to this BRigidBody. ");
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new BulletSharp.GhostObject();
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = this.GetParent <Unit>().Quaternion.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = this.GetParent <Unit>().Position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.NoContactResponse;
}
else
{
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = this.GetParent <Unit>().Quaternion.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = this.GetParent <Unit>().Position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionShape = cs;
m_collisionObject.CollisionFlags = m_collisionObject.CollisionFlags | BulletSharp.CollisionFlags.NoContactResponse;
}
return(true);
}
public virtual void SetRotation(Quaternion rotation)
{
if (isInWorld)
{
MatrixB newTrans = collisionObject.WorldTransform;
QuaternionB q = rotation.ToBullet();
MatrixB.RotationQuaternion(ref q, out newTrans);
newTrans.Origin = transform.position.ToBullet();
collisionObject.WorldTransform = newTrans;
transform.rotation = rotation;
}
else
{
transform.rotation = rotation;
}
}
public virtual void SetPositionAndRotation(Vector3 position, Quaternion rotation)
{
if (isInWorld)
{
BulletSharp.Math.Matrix newTrans = m_collisionObject.WorldTransform;
BulletSharp.Math.Quaternion q = rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out newTrans);
newTrans.Origin = this.GetParent <Unit>().Position.ToBullet();
m_collisionObject.WorldTransform = newTrans;
this.GetParent <Unit>().Position = position;
this.GetParent <Unit>().Quaternion = rotation;
}
else
{
this.GetParent <Unit>().Position = position;
this.GetParent <Unit>().Quaternion = rotation;
}
}
public static BulletSharp.Math.Vector3 ToEuler(this BulletSharp.Math.Quaternion quat)
{
var eulers = new BulletSharp.Math.Vector3
{
X = (float)Math.Atan2(2 * (quat.W * quat.X + quat.Y * quat.Z), 1 - 2 * (quat.X * quat.X + quat.Y * quat.Y)),
Z = (float)Math.Atan2(2 * (quat.W * quat.Z + quat.X * quat.Y), 1 - 2 * (quat.Y * quat.Y + quat.Z * quat.Z))
};
var expr = 2 * (quat.W * quat.Y - quat.Z * quat.X);
if (Math.Abs(expr) >= 1)
{
eulers.Y = (float)(Math.Sign(expr) * Math.PI / 2);
}
else
{
eulers.Y = (float)Math.Asin(expr);
}
return(eulers);
}
public static void SetOrientation(this MatrixB bm, QuaternionB q)
{
float xx = q.X * q.X;
float yy = q.Y * q.Y;
float zz = q.Z * q.Z;
float xy = q.X * q.Y;
float zw = q.Z * q.W;
float zx = q.Z * q.X;
float yw = q.Y * q.W;
float yz = q.Y * q.Z;
float xw = q.X * q.W;
bm.M11 = 1.0f - (2.0f * (yy + zz));
bm.M12 = 2.0f * (xy + zw);
bm.M13 = 2.0f * (zx - yw);
bm.M21 = 2.0f * (xy - zw);
bm.M22 = 1.0f - (2.0f * (zz + xx));
bm.M23 = 2.0f * (yz + xw);
bm.M31 = 2.0f * (zx + yw);
bm.M32 = 2.0f * (yz - xw);
bm.M33 = 1.0f - (2.0f * (yy + xx));
}
public static Quaternion ToUnity(this BulletSharp.Math.Quaternion v)
{
return(new Quaternion(v.X, v.Y, v.Z, v.W));
}
public static UnityEngine.Quaternion ToUnity(this BulletSharp.Math.Quaternion v)
{
//return new UnityEngine.Quaternion(v.X, v.Y, v.Z, v.W);
return(new UnityEngine.Quaternion(v.X, v.Z, v.Y, -v.W));
}
public static Quaternion ToUnity(this QuaternionB v)
{
return(new Quaternion(v.X, v.Y, v.Z, v.W));
}
public static QuaternionB GetOrientation(this MatrixB bm)
{
//Scaling is the length of the rows.
Vector3B scale;
scale.X = (float)System.Math.Sqrt((bm.M11 * bm.M11) + (bm.M12 * bm.M12) + (bm.M13 * bm.M13));
scale.Y = (float)System.Math.Sqrt((bm.M21 * bm.M21) + (bm.M22 * bm.M22) + (bm.M23 * bm.M23));
scale.Z = (float)System.Math.Sqrt((bm.M31 * bm.M31) + (bm.M32 * bm.M32) + (bm.M33 * bm.M33));
//The rotation is the left over matrix after dividing out the scaling.
float mm11 = bm.M11 / scale.X;
float mm12 = bm.M12 / scale.X;
float mm13 = bm.M13 / scale.X;
float mm21 = bm.M21 / scale.Y;
float mm22 = bm.M22 / scale.Y;
float mm23 = bm.M23 / scale.Y;
float mm31 = bm.M31 / scale.Z;
float mm32 = bm.M32 / scale.Z;
float mm33 = bm.M33 / scale.Z;
//------------------------
float sqrt;
float half;
float trace = mm11 + mm22 + mm33;
QuaternionB result = new QuaternionB();
if (trace > 0.0f)
{
sqrt = Mathf.Sqrt(trace + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (mm23 - mm32) * sqrt;
result.Y = (mm31 - mm13) * sqrt;
result.Z = (mm12 - mm21) * sqrt;
}
else if ((mm11 >= mm22) && (mm11 >= mm33))
{
sqrt = Mathf.Sqrt(1.0f + mm11 - mm22 - mm33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (mm12 + mm21) * half;
result.Z = (mm13 + mm31) * half;
result.W = (mm23 - mm32) * half;
}
else if (mm22 > mm33)
{
sqrt = Mathf.Sqrt(1.0f + mm22 - mm11 - mm33);
half = 0.5f / sqrt;
result.X = (mm21 + mm12) * half;
result.Y = 0.5f * sqrt;
result.Z = (mm32 + mm23) * half;
result.W = (mm31 - mm13) * half;
}
else
{
sqrt = Mathf.Sqrt(1.0f + mm33 - mm11 - mm22);
half = 0.5f / sqrt;
result.X = (mm31 + mm13) * half;
result.Y = (mm32 + mm23) * half;
result.Z = 0.5f * sqrt;
result.W = (mm12 - mm21) * half;
}
//------------------------
return(result);
}
/// <summary>
/// Creates or configures a RigidBody based on the current settings. Does not alter the internal state of this component in any way.
/// Can be used to create copies of this BRigidBody for use in other physics simulations.
/// </summary>
public bool CreateOrConfigureRigidBody(ref RigidBody rb, ref Vector3B localInertia, CollisionShape cs, MotionState motionState)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
localInertia = Vector3B.Zero;
if (isDynamic())
{
cs.CalculateLocalInertia(_mass, out localInertia);
}
if (rb == null)
{
float bulletMass = _mass;
if (!isDynamic())
{
bulletMass = 0f;
}
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(bulletMass, motionState, cs, localInertia)
{
Friction = _friction,
RollingFriction = _rollingFriction,
LinearDamping = _linearDamping,
AngularDamping = _angularDamping,
Restitution = _restitution,
LinearSleepingThreshold = _linearSleepingThreshold,
AngularSleepingThreshold = _angularSleepingThreshold,
AdditionalDamping = _additionalDamping,
AdditionalAngularDampingFactor = _additionalAngularDampingFactor,
AdditionalAngularDampingThresholdSqr = _additionalAngularDampingThresholdSqr,
AdditionalDampingFactor = _additionalDampingFactor,
AdditionalLinearDampingThresholdSqr = _additionalLinearDampingThresholdSqr
};
rb = new RigidBody(rbInfo);
if (motionState == null)
{
QuaternionB rotation = transform.rotation.ToBullet();
Vector3B position = transform.position.ToBullet();
//Vector3B scale = transform.lossyScale.ToBullet();
rb.WorldTransform = Matrix.AffineTransformationCustom(ref position, ref rotation);
}
rbInfo.Dispose();
}
else
{
rb.SetMassProps(isDynamic() ? _mass : 0, localInertia);
rb.Friction = _friction;
rb.RollingFriction = _rollingFriction;
rb.SetDamping(_linearDamping, _angularDamping);
rb.Restitution = _restitution;
rb.SetSleepingThresholds(_linearSleepingThreshold, _angularSleepingThreshold);
rb.CollisionShape = cs;
}
rb.AngularVelocity = angularVelocity.ToBullet();
rb.LinearVelocity = velocity.ToBullet();
rb.CollisionFlags = m_collisionFlags;
rb.LinearFactor = _linearFactor.ToBullet();
rb.AngularFactor = _angularFactor.ToBullet();
if (m_rigidBody != null)
{
rb.DeactivationTime = m_rigidBody.DeactivationTime;
rb.InterpolationLinearVelocity = m_rigidBody.InterpolationLinearVelocity;
rb.InterpolationAngularVelocity = m_rigidBody.InterpolationAngularVelocity;
rb.InterpolationWorldTransform = m_rigidBody.InterpolationWorldTransform;
}
//if kinematic then disable deactivation
if ((m_collisionFlags & BulletSharp.CollisionFlags.KinematicObject) != 0)
{
rb.ActivationState = ActivationState.DisableDeactivation;
}
return(true);
}
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveCollisionObject(this);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return(false);
}
if (!(m_collisionShape.GetCollisionShape() is ConvexShape))
{
Debug.LogError("The CollisionShape on this BCharacterController was not a convex shape. " + name);
return(false);
}
m_collisionShape.GetCollisionShape();
if (m_collisionObject == null)
{
m_collisionObject = new PairCachingGhostObject();
m_collisionObject.CollisionShape = m_collisionShape.GetCollisionShape();
m_collisionObject.CollisionFlags = m_collisionFlags;
m_characterController = new KinematicCharacterController((PairCachingGhostObject)m_collisionObject, (ConvexShape)m_collisionShape.GetCollisionShape(), stepHeight, upAxis);
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
//world.world.AddCollisionObject(m_collisionObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
//((DynamicsWorld)world.world).AddAction(m_characterController);
}
else
{
m_collisionObject.CollisionShape = m_collisionShape.GetCollisionShape();
m_collisionObject.CollisionFlags = m_collisionFlags;
if (m_characterController != null)
{
world.RemoveAction(m_characterController);
}
m_characterController = new KinematicCharacterController((PairCachingGhostObject)m_collisionObject, (ConvexShape)m_collisionShape.GetCollisionShape(), stepHeight, upAxis);
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
}
return(true);
}
public static OpenTK.Quaternion ToOpenTK(this BulletSharp.Math.Quaternion q)
{
return(new OpenTK.Quaternion(q.X, q.Y, q.Z, q.W));
}
//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <Vector3> SimulateBall(BRigidBody ballRb, Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
List <Vector3> ballPositions = new List <Vector3>(numberOfSimulationSteps);
//Create a World
Debug.Log("Initialize physics");
CollisionConfiguration CollisionConf;
CollisionDispatcher Dispatcher;
BroadphaseInterface Broadphase;
CollisionWorld cw;
ConstraintSolver Solver;
BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;
//This should create a copy of the BPhysicsWorld with the same settings
WorldController bw = WorldsManager.WorldController;
bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
World = (DiscreteDynamicsWorld)cw;
//Copy all existing rigidbodies in scene
// IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
// this is because collisions must be resolved in the same order for the sim to be deterministic
DiscreteDynamicsWorld sourceWorld = bw.DDWorld;
RigidBody bulletBallRb = null;
BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity;
for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
{
CollisionObject co = sourceWorld.CollisionObjectArray[i];
if (co != null && co.UserObject is BRigidBody)
{
BRigidBody rb = (BRigidBody)co.UserObject;
BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
CollisionShape shape = null;
if (existingShape is BSphereShape)
{
shape = ((BSphereShape)existingShape).CopyCollisionShape();
}
else if (existingShape is BBoxShape)
{
shape = ((BBoxShape)existingShape).CopyCollisionShape();
}
RigidBody bulletRB = null;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin;
BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
bulletRB.WorldTransform = mm;
World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
if (rb == ballRb)
{
bulletBallRb = bulletRB;
bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
}
}
}
//Step the simulation numberOfSimulationSteps times
for (int i = 0; i < numberOfSimulationSteps; i++)
{
World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
World.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
public static BulletSharp.Math.Quaternion GetOrientation(this BulletSharp.Math.Matrix bm)
{
/*
* float trace = M11 + M22 + M33;
*
* float[] temp = new float[4];
*
* if (trace > 0.0f)
* {
* float s = Mathf.Sqrt(trace + (1.0f));
* temp[3] = (s * (0.5f));
* s = (0.5f) / s;
*
* temp[0] = ((M32 - M23) * s);
* temp[1] = ((M13 - M31) * s);
* temp[2] = ((M21 - M12) * s);
*
* temp[0] = ((M23 - M32) * s);
* temp[1] = ((M31 - M13) * s);
* temp[2] = ((M12 - M21) * s);
* }
* else
* {
* int i = M11 < M22 ?
* (M22 < M33 ? 2 : 1) :
* (M11 < M33 ? 2 : 0);
* int j = (i + 1) % 3;
* int k = (i + 2) % 3;
*
* float s = Mathf.Sqrt(this[i,i] - this[j,j] - this[k,k] + 1.0f);
* temp[i] = s * 0.5f;
* s = 0.5f / s;
*
* temp[3] = (this[j,k] - this[k,j]) * s;
* temp[j] = (this[i,j] + this[j,i]) * s;
* temp[k] = (this[i,k] + this[k,i]) * s;
* }
* return new BulletSharp.Math.Quaternion(temp[0], temp[1], temp[2], temp[3]);
*/
//Scaling is the length of the rows.
BulletSharp.Math.Vector3 scale;
scale.X = (float)System.Math.Sqrt((bm.M11 * bm.M11) + (bm.M12 * bm.M12) + (bm.M13 * bm.M13));
scale.Y = (float)System.Math.Sqrt((bm.M21 * bm.M21) + (bm.M22 * bm.M22) + (bm.M23 * bm.M23));
scale.Z = (float)System.Math.Sqrt((bm.M31 * bm.M31) + (bm.M32 * bm.M32) + (bm.M33 * bm.M33));
//The rotation is the left over matrix after dividing out the scaling.
float mm11 = bm.M11 / scale.X;
float mm12 = bm.M12 / scale.X;
float mm13 = bm.M13 / scale.X;
float mm21 = bm.M21 / scale.Y;
float mm22 = bm.M22 / scale.Y;
float mm23 = bm.M23 / scale.Y;
float mm31 = bm.M31 / scale.Z;
float mm32 = bm.M32 / scale.Z;
float mm33 = bm.M33 / scale.Z;
//------------------------
float sqrt;
float half;
float trace = mm11 + mm22 + mm33;
BulletSharp.Math.Quaternion result = new BulletSharp.Math.Quaternion();
if (trace > 0.0f)
{
sqrt = (float)Mathf.Sqrt(trace + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (mm23 - mm32) * sqrt;
result.Y = (mm31 - mm13) * sqrt;
result.Z = (mm12 - mm21) * sqrt;
}
else if ((mm11 >= mm22) && (mm11 >= mm33))
{
sqrt = (float)Mathf.Sqrt(1.0f + mm11 - mm22 - mm33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (mm12 + mm21) * half;
result.Z = (mm13 + mm31) * half;
result.W = (mm23 - mm32) * half;
}
else if (mm22 > mm33)
{
sqrt = (float)Mathf.Sqrt(1.0f + mm22 - mm11 - mm33);
half = 0.5f / sqrt;
result.X = (mm21 + mm12) * half;
result.Y = 0.5f * sqrt;
result.Z = (mm32 + mm23) * half;
result.W = (mm31 - mm13) * half;
}
else
{
sqrt = (float)Mathf.Sqrt(1.0f + mm33 - mm11 - mm22);
half = 0.5f / sqrt;
result.X = (mm31 + mm13) * half;
result.Y = (mm32 + mm23) * half;
result.Z = 0.5f * sqrt;
result.W = (mm12 - mm21) * half;
}
//------------------------
return(result);
}
public static BulletSharp.Math.Quaternion GetOrientation(this BulletSharp.Math.Matrix bm)
{
/*
float trace = M11 + M22 + M33;
float[] temp = new float[4];
if (trace > 0.0f)
{
float s = UnityEngine.Mathf.Sqrt(trace + (1.0f));
temp[3] = (s * (0.5f));
s = (0.5f) / s;
temp[0] = ((M32 - M23) * s);
temp[1] = ((M13 - M31) * s);
temp[2] = ((M21 - M12) * s);
temp[0] = ((M23 - M32) * s);
temp[1] = ((M31 - M13) * s);
temp[2] = ((M12 - M21) * s);
}
else
{
int i = M11 < M22 ?
(M22 < M33 ? 2 : 1) :
(M11 < M33 ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
float s = UnityEngine.Mathf.Sqrt(this[i,i] - this[j,j] - this[k,k] + 1.0f);
temp[i] = s * 0.5f;
s = 0.5f / s;
temp[3] = (this[j,k] - this[k,j]) * s;
temp[j] = (this[i,j] + this[j,i]) * s;
temp[k] = (this[i,k] + this[k,i]) * s;
}
return new BulletSharp.Math.Quaternion(temp[0], temp[1], temp[2], temp[3]);
*/
//Scaling is the length of the rows.
BulletSharp.Math.Vector3 scale;
scale.X = (float)System.Math.Sqrt((bm.M11 * bm.M11) + (bm.M12 * bm.M12) + (bm.M13 * bm.M13));
scale.Y = (float)System.Math.Sqrt((bm.M21 * bm.M21) + (bm.M22 * bm.M22) + (bm.M23 * bm.M23));
scale.Z = (float)System.Math.Sqrt((bm.M31 * bm.M31) + (bm.M32 * bm.M32) + (bm.M33 * bm.M33));
//The rotation is the left over matrix after dividing out the scaling.
float mm11 = bm.M11 / scale.X;
float mm12 = bm.M12 / scale.X;
float mm13 = bm.M13 / scale.X;
float mm21 = bm.M21 / scale.Y;
float mm22 = bm.M22 / scale.Y;
float mm23 = bm.M23 / scale.Y;
float mm31 = bm.M31 / scale.Z;
float mm32 = bm.M32 / scale.Z;
float mm33 = bm.M33 / scale.Z;
//------------------------
float sqrt;
float half;
float trace = mm11 + mm22 + mm33;
BulletSharp.Math.Quaternion result = new BulletSharp.Math.Quaternion();
if (trace > 0.0f)
{
sqrt = (float)UnityEngine.Mathf.Sqrt(trace + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (mm23 - mm32) * sqrt;
result.Y = (mm31 - mm13) * sqrt;
result.Z = (mm12 - mm21) * sqrt;
}
else if ((mm11 >= mm22) && (mm11 >= mm33))
{
sqrt = (float)UnityEngine.Mathf.Sqrt(1.0f + mm11 - mm22 - mm33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (mm12 + mm21) * half;
result.Z = (mm13 + mm31) * half;
result.W = (mm23 - mm32) * half;
}
else if (mm22 > mm33)
{
sqrt = (float)UnityEngine.Mathf.Sqrt(1.0f + mm22 - mm11 - mm33);
half = 0.5f / sqrt;
result.X = (mm21 + mm12) * half;
result.Y = 0.5f * sqrt;
result.Z = (mm32 + mm23) * half;
result.W = (mm31 - mm13) * half;
}
else
{
sqrt = (float)UnityEngine.Mathf.Sqrt(1.0f + mm33 - mm11 - mm22);
half = 0.5f / sqrt;
result.X = (mm31 + mm13) * half;
result.Y = (mm32 + mm23) * half;
result.Z = 0.5f * sqrt;
result.W = (mm12 - mm21) * half;
}
//------------------------
return result;
}
