public bool RemoveConstraint(TypedConstraint c)
{
if (!isDisposed)
{
if (physicWorldParameters.worldType < WorldType.RigidBodyDynamics)
{
Debug.LogError("World type must not be collision only");
return(false);
}
if (physicWorldParameters.debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing constraint {0} from world", c.Userobject);
}
DWorld.RemoveConstraint(c);
if (c.Userobject is BTypedConstraint)
{
((BTypedConstraint)c.Userobject).m_isInWorld = false;
}
return(true);
}
return(false);
}
private void PickRigidBody(RigidBody body, ref Vector3 pickPosition)
{
if (body.IsStaticObject || body.IsKinematicObject)
{
return;
}
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
DiscreteDynamicsWorld world = _demo.Simulation.World;
Vector3 localPivot = Vector3.TransformCoordinate(pickPosition, Matrix.Invert(body.CenterOfMassTransform));
if (_demo.Input.KeysDown.Contains(Keys.ShiftKey))
{
var dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
world.AddConstraint(dof6);
_rigidBodyPickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
var p2p = new Point2PointConstraint(body, localPivot);
world.AddConstraint(p2p);
_rigidBodyPickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
}
public void Evaluate(int SpreadMax)
{
if (this.FInput.IsConnected)
{
this.FType.SliceCount = SpreadMax;
this.FLifeTime.SliceCount = SpreadMax;
this.FId.SliceCount = SpreadMax;
this.FCustom.SliceCount = SpreadMax;
for (int i = 0; i < SpreadMax; i++)
{
TypedConstraint cst = FInput[i];
ConstraintCustomData sc = (ConstraintCustomData)cst.UserObject;
FType[i] = cst.ConstraintType;
FId[i] = sc.Id;
FLifeTime[i] = sc.LifeTime;
FCustom[i] = sc.Custom;
}
}
else
{
this.FType.SliceCount = 0;
this.FLifeTime.SliceCount = 0;
this.FId.SliceCount = 0;
this.FCustom.SliceCount = 0;
}
}
public static void AttachConstraint(this IConstraintContainer world, TypedConstraint typedConstraint)
{
ConstraintCustomData cd = new ConstraintCustomData(world.GetNewConstraintId());
typedConstraint.UserObject = cd;
world.Register(typedConstraint);
}
public virtual void Dispose()
{
GC.SuppressFinalize(this);
_constraint?.Dispose();
_constraint = null;
}
//Unregister a constraint with the physics world
public static void Unregister(TypedConstraint constraint)
{
if (_decommisioning)
{
return;
}
Instance.Unregister_Internal(constraint);
}
//override this one
protected virtual void Dispose(bool isdisposing)
{
if (m_constraintPtr != null)
{
m_constraintPtr.Dispose();
m_constraintPtr = null;
}
}
public static void DeleteAndDisposeAllConstraints(this DynamicsWorld world)
{
for (int i = 0; i < world.NumConstraints; i++)
{
TypedConstraint constraint = world.GetConstraint(i);
world.DeleteAndDisposeConstraint(constraint);
}
}
private void OnConstraintDeleted(TypedConstraint constraint, int id)
{
if (constraint is TConstraint)
{
TConstraint tc = (TConstraint)constraint;
this.constraintList.Remove(tc);
}
}
void OnDestroy()
{
for (int i = 0; i < createdObjs.Count; i++)
{
Destroy(createdObjs[i]);
}
createdObjs.Clear();
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();
}
//delete collision shapes
foreach (CollisionShape shape in CollisionShapes)
{
shape.Dispose();
}
CollisionShapes.Clear();
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
}
private static void CleanupConstraints(DynamicsWorld world)
{
for (int i = world.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = world.GetConstraint(i);
world.RemoveConstraint(constraint);
constraint.Dispose();
}
}
public void RemoveConctraint(TypedConstraint cst, ConstraintCustomData cdata)
{
this.dynamicsWorld.DeleteAndDisposeConstraint(cst);
if (this.ConstraintDeleted != null)
{
this.ConstraintDeleted(cst, cdata.Id);
}
}
/// <summary>
/// Creates the joint data
/// </summary>
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
RotationalJoint_Base nodeR = (RotationalJoint_Base)GetSkeletalJoint();
CollisionObject parentObject = ((BulletRigidNode)GetParent()).BulletObject;
WheelDriverMeta wheel = GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>();
Vector3 pivot = nodeR.basePoint.Convert();
Matrix4 locJ, locP; //Local Joint Pivot, Local Parent Pivot
BulletObject.WorldTransform = parentObject.WorldTransform * Matrix4.CreateTranslation(pivot);
if (debug)
{
Console.WriteLine("Pivot at " + pivot);
}
GetFrames(nodeR.basePoint.Convert(), parentObject.WorldTransform, BulletObject.WorldTransform, out locP, out locJ);
//HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, /*(RigidBody)*/BulletObject, locP, locJ);
HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, BulletObject, pivot, Vector3.Zero, nodeR.axis.Convert(), nodeR.axis.Convert());
joint = temp;
temp.SetAxis(nodeR.axis.Convert());
if (nodeR.hasAngularLimit)
{
temp.SetLimit(nodeR.angularLimitLow, nodeR.angularLimitHigh);
}
//also need to find a less screwy way to do this
Update = (f) => { (/*(RigidBody)*/ BulletObject).ApplyTorque(nodeR.axis.Convert() * f * 25); };
if (debug)
{
Console.WriteLine("{0} joint made", wheel == null ? "Rotational" : "Wheel");
}
break;
default:
if (debug)
{
Console.WriteLine("Received joint of type {0}", GetSkeletalJoint().GetJointType());
}
break;
}
}
public virtual void ExitPhysics()
{
BulletExampleRunner.Get().ExitPhysics();
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();
}
//delete collision shapes
foreach (CollisionShape shape in CollisionShapes)
{
shape.Dispose();
}
CollisionShapes.Clear();
_world.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
}
void RemovePickingConstraint()
{
if (pickConstraint != null && PhysicsContext.World != null)
{
PhysicsContext.World.RemoveConstraint(pickConstraint);
pickConstraint.Dispose();
pickConstraint = null;
pickedBody.ForceActivationState(ActivationState.ActiveTag);
pickedBody.DeactivationTime = 0;
pickedBody = null;
}
}
public void releaseObject()
{
if (_pick_constraint != null && _physics_world.world != null)
{
_physics_world.world.RemoveConstraint(_pick_constraint);
_pick_constraint.Dispose();
_pick_constraint = null;
_picked_body.ForceActivationState(ActivationState.ActiveTag);
_picked_body.DeactivationTime = 0;
_picked_body = null;
_zoom_picked_distance = false;
}
}
internal void ProcessDelete()
{
int cnt = this.RigidBodies.Count;
for (int i = 0; i < cnt; i++)
{
RigidBody body = this.RigidBodies[i];
BodyCustomData bd = (BodyCustomData)body.UserObject;
bd.Created = false;
if (bd.MarkedForDeletion)
{
if (this.RigidBodyDeleted != null)
{
this.RigidBodyDeleted(body, bd.Id);
}
this.Unregister(body);
}
}
cnt = this.SoftBodies.Count;
for (int i = 0; i < cnt; i++)
{
SoftBody body = this.SoftBodies[i];
BodyCustomData bd = (BodyCustomData)body.UserObject;
bd.Created = false;
if (bd.MarkedForDeletion)
{
if (this.SoftBodyDeleted != null)
{
this.SoftBodyDeleted(body, bd.Id);
}
this.Unregister(body);
}
}
cnt = this.Constraints.Count;
for (int i = 0; i < cnt; i++)
{
TypedConstraint cst = this.constraints[i];
ConstraintCustomData cd = (ConstraintCustomData)cst.UserObject;
cd.Created = false;
if (cd.MarkedForDeletion)
{
if (this.ConstraintDeleted != null)
{
this.ConstraintDeleted(cst, cd.Id);
}
this.Unregister(cst);
}
}
}
public void Evaluate(int SpreadMax)
{
if (FInput.PluginIO.IsConnected)
{
for (int i = 0; i < SpreadMax; i++)
{
if (FApply[i])
{
TypedConstraint cst = this.FInput[i];
ConstraintCustomData cust = (ConstraintCustomData)cst.UserObject;
cust.MarkedForDeletion = true;
}
}
}
}
public void Dispose()
{
for (int i = _World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = _World.GetConstraint(i);
_World.RemoveConstraint(constraint);
constraint.Dispose();
}
_World.Dispose();
broadphase.Dispose();
if (dispatcher != null)
{
dispatcher.Dispose();
}
collisionConf.Dispose();
}
public static void Dispose() // TODO: can crash; examine!
{
// remove/dispose of constraints
for (int i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
lock (_worldSyncRoot)
World.RemoveConstraint(constraint);
constraint.Dispose();
}
// remove the rigidbodies from the dynamics world and delete them
for (int 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();
}
lock (_worldSyncRoot)
World.RemoveCollisionObject(obj);
obj.Dispose();
}
// delete collision shapes
foreach (CollisionShape shape in _collisionShapes)
{
shape.Dispose();
}
_collisionShapes.Clear();
lock (_worldSyncRoot)
World.Dispose();
_broadphase.Dispose();
if (_dispatcher != null)
{
_dispatcher.Dispose();
}
_collisionConf.Dispose();
}
private void LoadJoint(UrdfJoint joint)
{
RigidBody childRigidBody;
if (!_linkToRigidBody.TryGetValue(joint.Child.Name, out childRigidBody))
{
return;
}
RigidBody parentRigidBody;
if (joint.Parent.Collisions.Any())
{
if (!_linkToRigidBody.TryGetValue(joint.Parent.Name, out parentRigidBody))
{
return;
}
}
else
{
parentRigidBody = TypedConstraint.GetFixedBody();
}
TypedConstraint constraint;
if (joint is UrdfContinuousJoint)
{
constraint = CreateRevoluteJoint(childRigidBody, parentRigidBody);
}
else if (joint is UrdfFixedJoint)
{
Matrix childFrame = ParseInertia(joint.Child.Inertial.Inertia);
childFrame = ParsePose(joint.Origin);
constraint = CreateFixedJoint(childRigidBody, parentRigidBody, childFrame);
}
else
{
//throw new NotImplementedException();
return;
}
World.AddConstraint(constraint, true);
}
public void unload()
{
int i;
// Remove constraints
for (i = _physics_world.world.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = _physics_world.world.GetConstraint(i);
_physics_world.world.RemoveConstraint(constraint);
constraint.Dispose();
}
// Remove rigidbodies from the dynamics world and delete them
for (i = _physics_world.world.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = _physics_world.world.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
_physics_world.world.RemoveCollisionObject(obj);
obj.Dispose();
}
// Delete collision shapes
foreach (CollisionShape shape in _physics_world.collision_shapes)
{
shape.Dispose();
}
_physics_world.collision_shapes.Clear();
// Delete the world
_physics_world.world.Dispose();
_broadphase.Dispose();
if (_dispatcher != null)
{
_dispatcher.Dispose();
}
_collision_config.Dispose();
}
public void RemovePickingConstraint()
{
if (_rigidBodyPickConstraint != null)
{
_demo.Simulation.World.RemoveConstraint(_rigidBodyPickConstraint);
_rigidBodyPickConstraint.Dispose();
_rigidBodyPickConstraint = null;
_pickedBody.ForceActivationState(ActivationState.ActiveTag);
_pickedBody.DeactivationTime = 0;
_pickedBody = null;
}
if (_multiBodyPickConstraint != null)
{
_multiBodyPickConstraint.MultiBodyA.CanSleep = _prevCanSleep;
(_demo.Simulation.World as MultiBodyDynamicsWorld).RemoveMultiBodyConstraint(_multiBodyPickConstraint);
_multiBodyPickConstraint.Dispose();
_multiBodyPickConstraint = null;
}
}
void RemovePickingConstraint()
{
if (pickConstraint != null && World != null)
{
World.RemoveConstraint(pickConstraint);
pickConstraint.Dispose();
pickConstraint = null;
pickedBody.ForceActivationState(ActivationState.ActiveTag);
pickedBody.DeactivationTime = 0;
pickedBody = null;
}
if (pickingMultiBodyPoint2Point != null)
{
pickingMultiBodyPoint2Point.MultiBodyA.CanSleep = prevCanSleep;
(World as MultiBodyDynamicsWorld).RemoveMultiBodyConstraint(pickingMultiBodyPoint2Point);
pickingMultiBodyPoint2Point.Dispose();
pickingMultiBodyPoint2Point = null;
}
}
//Register a constraint with the simulation
public override void Register(TypedConstraint constraint)
{
if (!_initlialized)
{
InitializeWorld(BulletUpdate);
Debug.LogWarning("A Constraint attempted to register with the simulation before it was initialized!\n" +
"Please check your script execution order");
}
//Check if already registered and warn the user
if (_bulletConstraints.Contains(constraint))
{
Debug.LogError("Specified Constraint has already been registered with the simulation!\n");
return;
}
//Register the Bullet RigidBody with the simulation and list for tracking
_bulletConstraints.Add(constraint);
_dynamicsWorld.AddConstraint(constraint);
}
public void ExitPhysics()
{
//remove/dispose constraints
int i;
for (i = physics_world.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = physics_world.GetConstraint(i);
physics_world.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = physics_world.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = physics_world.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
physics_world.RemoveCollisionObject(obj);
obj.Dispose();
}
//delete collision shapes
foreach (CollisionShape shape in collisionShapes)
{
shape.Dispose();
}
collisionShapes.Clear();
physics_world.Dispose();
broadphase.Dispose();
if (dispatcher != null)
{
dispatcher.Dispose();
}
collisionConf.Dispose();
}
public void Drop()
{
if (simThread != null)
{
simThread.Join();
}
for (int i = bulletWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint c = bulletWorld.GetConstraint(i);
bulletWorld.RemoveConstraint(c);
c.Dispose();
}
for (int i = bulletWorld.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject o = bulletWorld.CollisionObjectArray[i];
RigidBody b = o as RigidBody;
if (b != null && b.MotionState != null)
{
b.MotionState.Dispose();
}
bulletWorld.RemoveCollisionObject(o);
o.Dispose();
}
foreach (CollisionShape s in bulletShapes)
{
s.Dispose();
}
bulletShapes.Clear();
bulletWorld.Dispose();
bulletBroadphase.Dispose();
bulletCollisionDispatcher.Dispose();
bulletCollisionConfiguration.Dispose();
}
//Unregister a constraint with the simulation
public override void Unregister(TypedConstraint constraint)
{
//Warn the user if the physics world has not been initialized
if (!_initlialized)
{
Debug.LogError("A Constraint attempted to unregister from the simulation before it was initialized!\n" +
"Please check your scene setup!");
return;
}
//Check if the specified Object has been registered
if (!_bulletConstraints.Contains(constraint))
{
Debug.LogError("Specified Constraint has not been registered with this simulation!\n" +
"Please check your scene setup!");
return;
}
//Unregister the Bullet object from the simulation and callback
_bulletConstraints.Remove(constraint);
_dynamicsWorld.RemoveConstraint(constraint);
}
public void RemoveConstraint(TypedConstraint c)
{
if (!_isDisposed)
{
if (m_worldType < WorldType.RigidBodyDynamics)
{
Debug.LogError("World type must not be collision only");
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing constraint {0} from world", c.Userobject);
}
((DiscreteDynamicsWorld)m_world).RemoveConstraint(c);
if (c.Userobject is BTypedConstraint)
{
((BTypedConstraint)c.Userobject).m_isInWorld = false;
}
}
}
void ResetScene()
{
while (World.NumConstraints > 0)
{
TypedConstraint pc = World.GetConstraint(0);
World.RemoveConstraint(pc);
pc.Dispose();
}
// Don't foreach here, it'll fail.
for (int i = World.CollisionObjectArray.Count - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = RigidBody.Upcast(obj);
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
if (obj is SoftBody)
{
SoftWorld.RemoveSoftBody(obj as SoftBody);
}
else
{
if (body != null)
{
World.RemoveRigidBody(body);
}
else
{
World.RemoveCollisionObject(obj);
}
}
obj.Dispose();
}
}
public virtual void addConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies) { }
int btGetConstraintIslandId(TypedConstraint lhs)
{
int islandId;
CollisionObject rcolObj0 = lhs.RigidBodyA;
CollisionObject rcolObj1 = lhs.RigidBodyB;
islandId = rcolObj0.IslandTag >= 0 ? rcolObj0.IslandTag : rcolObj1.IslandTag;
return islandId;
}
//public abstract void debugDrawWorld();
public virtual void addConstraint(TypedConstraint constraint) { }
public void addConstraintRef(TypedConstraint c)
{
int index = m_constraintRefs.IndexOf(c);
if (index == -1)
m_constraintRefs.Add(c);
m_checkCollideWith = true;
}
public void removeConstraintRef(TypedConstraint c)
{
m_constraintRefs.Remove(c);
m_checkCollideWith = m_constraintRefs.Count > 0;
}
public virtual void removeConstraint(TypedConstraint constraint) { }
public override void getInfo1(TypedConstraint.ConstraintInfo1 info)
{
if (m_useSolveConstraintObsolete)
{
info.m_numConstraintRows = 0;
info.nub = 0;
}
else
{
//prepare constraint
calculateTransforms(m_rbA.CenterOfMassTransform, m_rbB.CenterOfMassTransform);
info.m_numConstraintRows = 0;
info.nub = 6;
int i;
//test linear limits
for (i = 0; i < 3; i++)
{
if (m_linearLimits.needApplyForce(i))
{
info.m_numConstraintRows++;
info.nub--;
}
}
//test angular limits
for (i = 0; i < 3; i++)
{
if (testAngularLimitMotor(i))
{
info.m_numConstraintRows++;
info.nub--;
}
}
}
}
public override void getInfo2(TypedConstraint.ConstraintInfo2 info)
{
getInfo2NonVirtual(info, m_rbA.CenterOfMassTransform, m_rbB.CenterOfMassTransform, m_rbA.LinearVelocity, m_rbB.LinearVelocity, m_rbA.AngularVelocity, m_rbB.AngularVelocity);
}
