public void AddForceAtPosition(UnityEngine.Vector3 force, UnityEngine.Vector3 relativePostion)
{
if (isInWorld)
{
m_rigidBody.ApplyForce(force.ToBullet(), relativePostion.ToBullet());
}
}
public void AddTorque(UnityEngine.Vector3 torque)
{
if (isInWorld)
{
m_rigidBody.ApplyTorque(torque.ToBullet());
}
}
public void AddImpulseAtPosition(UnityEngine.Vector3 impulse, UnityEngine.Vector3 relativePostion)
{
if (isInWorld)
{
m_rigidBody.ApplyImpulse(impulse.ToBullet(), relativePostion.ToBullet());
}
}
public void AddForce(UnityEngine.Vector3 force)
{
if (isInWorld)
{
m_rigidBody.ApplyCentralForce(force.ToBullet());
}
}
public bool CreateFrame(UnityEngine.Vector3 forward, UnityEngine.Vector3 up, UnityEngine.Vector3 constraintPoint, ref BulletSharp.Math.Matrix m, ref string errorMsg)
{
BulletSharp.Math.Vector4 x;
BulletSharp.Math.Vector4 y;
BulletSharp.Math.Vector4 z;
if (forward == Vector3.zero)
{
errorMsg = "forward vector must not be zero";
return(false);
}
forward.Normalize();
if (up == Vector3.zero)
{
errorMsg = "up vector must not be zero";
return(false);
}
Vector3 right = Vector3.Cross(forward, up);
if (right == Vector3.zero)
{
errorMsg = "forward and up vector must not be colinear";
return(false);
}
up = Vector3.Cross(right, forward);
right.Normalize();
up.Normalize();
x.X = forward.x; x.Y = forward.y; x.Z = forward.z; x.W = 0f;
y.X = up.x; y.Y = up.y; y.Z = up.z; y.W = 0f;
z.X = right.x; z.Y = right.y; z.Z = right.z; z.W = 0f;
m.Row1 = x;
m.Row2 = y;
m.Row3 = z;
m.Origin = constraintPoint.ToBullet();
return(true);
}
public void AddImpulse(UnityEngine.Vector3 impulse)
{
if (isInWorld)
{
m_rigidBody.ApplyCentralImpulse(impulse.ToBullet());
}
}
/// <summary>
/// Applies a torque impulse to the RigidBody at a specified position
/// Should be called from BulletUpdate()
/// </summary>
/// <param name="torque">The torque impulse to be applied</param>
public void ApplyTorqueImpulse(Vector3 impulse)
{
if (!Initialized || Disposing || !Registered)
{
return;
}
BodyInstance.ApplyTorqueImpulse(impulse.ToBullet());
}
/// <summary>
/// Applies a continuous force to the RigidBody at a specified position
/// Should be called from BulletUpdate()
/// <param name="force">The continuous force to be applied</param>>
/// <param name="position">Local position at which to apply the force</param>>
/// </summary>
public void ApplyForce(Vector3 force, Vector3 position)
{
if (!Initialized || Disposing || !Registered)
{
return;
}
BodyInstance.ApplyForce(force.ToBullet(), position.ToBullet());
}
/**
* 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.
*/
public bool CreateOrConfigureRigidBody(ref RigidBody rb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs, MotionState motionState)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
localInertia = BulletSharp.Math.Vector3.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);
rbInfo.Friction = _friction;
rbInfo.RollingFriction = _rollingFriction;
rbInfo.LinearDamping = _linearDamping;
rbInfo.AngularDamping = _angularDamping;
rbInfo.Restitution = _restitution;
rbInfo.LinearSleepingThreshold = _linearSleepingThreshold;
rbInfo.AngularSleepingThreshold = _angularSleepingThreshold;
rbInfo.AdditionalDamping = _additionalDamping;
rbInfo.AdditionalAngularDampingFactor = _additionalAngularDampingFactor;
rbInfo.AdditionalAngularDampingThresholdSqr = _additionalAngularDampingThresholdSqr;
rbInfo.AdditionalDampingFactor = _additionalDampingFactor;
rbInfo.AdditionalLinearDampingThresholdSqr = _additionalLinearDampingThresholdSqr;
rb = new RigidBody(rbInfo);
rbInfo.Dispose();
}
else
{
float usedMass = 0f;
if (isDynamic())
{
usedMass = _mass;
}
rb.SetMassProps(usedMass, 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);
}
void CreateOrConfigureMultiBody(ref MultiBody mb, float baseMass, BCollisionShape[] shapes, BMultiBodyConstraint[] constraints)
{
BulletSharp.Math.Vector3 inertia = BulletSharp.Math.Vector3.Zero;
if (baseMass != 0)
{
CollisionShape cs = m_baseCollisionShape.GetCollisionShape();
cs.CalculateLocalInertia(baseMass, out inertia);
}
mb = new MultiBody(m_links.Count, baseMass, inertia, fixedBase, canSleep);
mb.BasePosition = transform.position.ToBullet();
UnityEngine.Quaternion r = UnityEngine.Quaternion.Inverse(transform.rotation);
mb.WorldToBaseRot = r.ToBullet();
for (int i = 0; i < m_links.Count; i++)
{
//Debug.Log("Found link " + i + " parent " + m_links[i].parentIndex + " index " + m_links[i].index);
BMultiBodyLink link = m_links[i];
CollisionShape cs = shapes[i].GetCollisionShape();
if (cs != null)
{
cs.CalculateLocalInertia(link.mass, out inertia);
}
else
{
inertia = BulletSharp.Math.Vector3.Zero;
}
FeatherstoneJointType jt = link.jointType;
int parentIdx = link.parentIndex;
// Vector from parent pivot (COM) to the joint pivot point in parent's frame
UnityEngine.Vector3 parentCOM2ThisPivotOffset;
link.FreezeJointAxis();
if (link.parentIndex >= 0)
{
parentCOM2ThisPivotOffset = link.parentCOM2JointPivotOffset;
}
else
{
parentCOM2ThisPivotOffset = transform.InverseTransformPoint(link.transform.TransformPoint(link.localPivotPosition));
}
// Vector from the joint pivot point to link's pivot point (COM) in link's frame.
UnityEngine.Vector3 thisPivotToThisCOMOffset = link.thisPivotToJointCOMOffset;
// Should rotate vectors in parent frame to vectors in local frame
UnityEngine.Quaternion parentToThisRotation = link.parentToJointRotation;
switch (jt)
{
case FeatherstoneJointType.Fixed:
mb.SetupFixed(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Planar:
mb.SetupPlanar(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Prismatic:
mb.SetupPrismatic(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Revolute:
mb.SetupRevolute(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Spherical:
mb.SetupSpherical(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
default:
Debug.LogError("Invalid joint type for link " + link.name);
break;
}
}
mb.CanSleep = true;
mb.HasSelfCollision = false;
mb.UseGyroTerm = true;
bool damping = true;
if (damping)
{
mb.LinearDamping = 0.1f;
mb.AngularDamping = 0.9f;
}
else
{
mb.LinearDamping = 0;
mb.AngularDamping = 0;
}
mb.FinalizeMultiDof();
m_multibody = mb;
}
//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
var ballPositions = new List <UnityEngine.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
BPhysicsWorld bw = BPhysicsWorld.Get();
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 = (DiscreteDynamicsWorld)bw.world;
BulletSharp.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;
float mass = rb.isDynamic() ? rb.mass : 0f;
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++)
{
int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
UnityEngine.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();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
//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 override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_rigidBody != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveRigidBody(m_rigidBody);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
BDebug.LogError(debugType, "The local scale on this rigid body 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)
{
BDebug.LogError(debugType, "There was no collision shape component attached to this BRigidBody. {0}", name);
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
_localInertia = BulletSharp.Math.Vector3.Zero;
if (isDynamic())
{
cs.CalculateLocalInertia(_mass, out _localInertia);
}
if (m_rigidBody == null)
{
m_motionState = new BGameObjectMotionState(transform);
float bulletMass = _mass;
if (!isDynamic())
{
bulletMass = 0f;
}
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(bulletMass, m_motionState, cs, _localInertia);
rbInfo.Friction = _friction;
rbInfo.RollingFriction = _rollingFriction;
rbInfo.LinearDamping = _linearDamping;
rbInfo.AngularDamping = _angularDamping;
rbInfo.Restitution = _restitution;
rbInfo.LinearSleepingThreshold = _linearSleepingThreshold;
rbInfo.AngularSleepingThreshold = _angularSleepingThreshold;
rbInfo.AdditionalDamping = _additionalDamping;
rbInfo.AdditionalAngularDampingFactor = _additionalAngularDampingFactor;
rbInfo.AdditionalAngularDampingThresholdSqr = _additionalAngularDampingThresholdSqr;
rbInfo.AdditionalDampingFactor = _additionalDampingFactor;
rbInfo.AdditionalLinearDampingThresholdSqr = _additionalLinearDampingThresholdSqr;
m_rigidBody = new RigidBody(rbInfo);
m_rigidBody.UserObject = this;
m_rigidBody.AngularVelocity = _angularVelocity.ToBullet();
m_rigidBody.LinearVelocity = _linearVelocity.ToBullet();
rbInfo.Dispose();
}
else
{
float usedMass = 0f;
if (isDynamic())
{
usedMass = _mass;
}
m_rigidBody.SetMassProps(usedMass, _localInertia);
m_rigidBody.Friction = _friction;
m_rigidBody.RollingFriction = _rollingFriction;
m_rigidBody.SetDamping(_linearDamping, _angularDamping);
m_rigidBody.Restitution = _restitution;
m_rigidBody.SetSleepingThresholds(_linearSleepingThreshold, _angularSleepingThreshold);
m_rigidBody.AngularVelocity = _angularVelocity.ToBullet();
m_rigidBody.LinearVelocity = _linearVelocity.ToBullet();
m_rigidBody.CollisionShape = cs;
}
m_rigidBody.CollisionFlags = m_collisionFlags;
m_rigidBody.LinearFactor = _linearFactor.ToBullet();
m_rigidBody.AngularFactor = _angularFactor.ToBullet();
//if kinematic then disable deactivation
if ((m_collisionFlags & BulletSharp.CollisionFlags.KinematicObject) != 0)
{
m_rigidBody.ActivationState = ActivationState.DisableDeactivation;
}
return(true);
}
/// <summary>
/// Initializes the Bullet RigidBody and attempts to register it with the physics world.
/// This should generally only be called internally or from a physics world instance.
/// </summary>
public override void InitializePhysicsBody()
{
//Check if already initialized
if (Initialized)
{
return;
}
//Make sure a BulletCollisionShape is attached and reference it if possible
PhysicsCollisionShape = GetComponent <BulletCollisionShape>();
if (PhysicsCollisionShape == null)
{
Debug.LogError("No Bullet collision shape is attached!\n" +
"Please attach a Bullet collision shape to this object");
return;
}
//Calculate the local intertial of the given shape
PhysicsCollisionShape.GetCollisionShape().CalculateLocalInertia(_mass, out _localInternia);
//Initialize the Bullet transform matrix and set the values based on Unity transform
InitialTransform = Matrix.AffineTransformation(1f, transform.rotation.ToBullet(), transform.position.ToBullet());
//Initialize the Bullet default motion state using the transform matrix
PhysicsMotionState = new BulletMotionState(transform);
//Initialize the Bullet rigidbody construction info and assign the relevant values
_constructionInfo = new RigidBodyConstructionInfo(_mass, PhysicsMotionState, PhysicsCollisionShape.GetCollisionShape())
{
LocalInertia = _localInternia,
LinearDamping = _linearDamping,
AngularDamping = _angularDamping,
Friction = _friction,
RollingFriction = _rollingFriction,
Restitution = _restitution,
LinearSleepingThreshold = _linearSleepThreshold,
AngularSleepingThreshold = _angularSleepThreshold
};
//Create the Bullet RigidBody
BodyInstance = new RigidBody(_constructionInfo)
{
LinearFactor = _linearFactor.ToBullet(),
AngularFactor = _angularFactor.ToBullet()
};
BodyInstance.CollisionFlags = CollisionFlags.CustomMaterialCallback;
//Set sleeping flag
if (!_allowSleeping)
{
BodyInstance.ActivationState = ActivationState.DisableDeactivation;
}
//_rigidBody.CcdMotionThreshold = 0.5f;
//_rigidBody.CcdSweptSphereRadius = 0.25f;
//Register with the physics world
BulletPhysicsWorldManager.Register(BodyInstance);
Registered = true;
//Initialization complete
Initialized = true;
}
public void Move(Vector3 displacement)
{
m_characterController.SetWalkDirection(displacement.ToBullet());
}
//Initialize and register the constraint
protected override void InitializeConstraint()
{
_constraint = new Point2PointConstraint(_rigidbody.BodyInstance, _connectedBody.BodyInstance, _localPivot.ToBullet(), _connectedPivot.ToBullet());
}
