protected override void Dispose(bool isdisposing)
{
if (isInWorld && isdisposing && m_linkCollider != null)
{
BPhysicsWorld pw = BPhysicsWorld.Get();
if (pw != null && pw.world != null)
{
//constraints must be removed before rigid body is removed
/*
* for (int i = m_linkCollider.NumConstraintRefs; i > 0; i--)
* {
* BTypedConstraint tc = (BTypedConstraint)m_rigidBody.GetConstraintRef(i - 1).Userobject;
* ((DiscreteDynamicsWorld)pw.world).RemoveConstraint(tc.GetConstraint());
* }
*/
((DiscreteDynamicsWorld)pw.world).RemoveCollisionObject(m_linkCollider);
}
}
if (m_linkCollider != null)
{
m_linkCollider.Dispose();
m_linkCollider = null;
}
}
protected override void Dispose(bool isdisposing)
{
if (isInWorld && isdisposing && m_multibody != null)
{
BPhysicsWorld pw = BPhysicsWorld.Get();
if (pw != null && pw.world != null)
{
if (m_baseCollider != null)
{
pw.world.RemoveCollisionObject(m_baseCollider);
}
if (m_multibody != null)
{
((MultiBodyDynamicsWorld)pw.world).RemoveMultiBody(m_multibody);
}
}
}
if (m_multibody != null)
{
m_multibody.Dispose();
m_multibody = null;
}
if (m_baseCollider != null)
{
m_baseCollider.Dispose();
m_baseCollider = null;
}
if (m_links != null)
{
m_links.Clear();
m_links = null;
}
}
/**
* 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 virtual bool CreateMultiBody(ref MultiBody mb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs)
{
localInertia = BulletSharp.Math.Vector3.Zero;
if (_mass > 0)
{
cs.CalculateLocalInertia(_mass, out localInertia);
}
if (mb == null)
{
int nbLinks = Links.Count;
foreach (BMultiBodyLink link in Links)
{
nbLinks += link.NbLinks;
}
Debug.Log("Adding multibody with " + nbLinks + " links");
mb = new MultiBody(nbLinks, _mass, localInertia, false, false);
mb.BaseWorldTransform = transform.localToWorldMatrix.ToBullet();
mb.HasSelfCollision = SelfCollision;
var collider = new MultiBodyLinkCollider(mb, -1);
collider.CollisionShape = cs;
collider.WorldTransform = transform.localToWorldMatrix.ToBullet();
collider.CollisionFlags = collisionFlags;
collider.UserObject = UserObject ?? this;
BPhysicsWorld.Get().world.AddCollisionObject(collider, groupsIBelongTo, collisionMask);
mb.BaseCollider = collider;
m_collisionObject = collider;
BulletMultiBodyLinkColliderProxy baseProxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>();
if (baseProxy == null)
{
baseProxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>();
}
baseProxy.target = collider;
try
{
int currentLinkIndex = 0;
for (int i = 0; i < Links.Count; ++i)
{
currentLinkIndex += Links[i].AddLinkToMultiBody(this, currentLinkIndex, -1, transform);
}
mb.FinalizeMultiDof();
}
catch (Exception e) // if an error occurs, don't add the object, otherwise unity will crash
{
Debug.LogErrorFormat("Error occured while setting MultiBody : {0}\n{1}", e.Message, e.StackTrace);
BPhysicsWorld.Get().world.RemoveCollisionObject(collider);
return(false);
}
mb.CanSleep = false;
mb.UserObject = UserObject ?? this;
}
return(true);
}
private void Update()
{
if (m_baseCollider != null && isInWorld)
{
Matrix4x4 m = m_baseCollider.WorldTransform.ToUnity();
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
for (int i = 0; i < m_links.Count; i++)
{
MultiBodyLinkCollider linkCollider = m_links[i].GetLinkCollider();
m = linkCollider.WorldTransform.ToUnity();
m_links[i].transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
m_links[i].transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
}
}
}
void DoEverything()
{
OnInitializePhysics();
for (int i = 0; i < 100; i++)
{
OnUpdate();
for (int j = 0; j < numLinks; j++)
{
MultiBodyLinkCollider linkCollider = links[j];
UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity();
UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
linkPositions[j] = p;
UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
}
}
ExitPhysics();
}
void AddColliders(MultiBody multiBody, Vector3 baseHalfExtents, Vector3 linkHalfExtents)
{
// Add a collider for the base
Quaternion[] worldToLocal = new Quaternion[multiBody.NumLinks + 1];
Vector3[] localOrigin = new Vector3[multiBody.NumLinks + 1];
worldToLocal[0] = multiBody.WorldToBaseRot;
localOrigin[0] = multiBody.BasePosition;
//if (true)
{
var collider = new MultiBodyLinkCollider(multiBody, -1);
collider.CollisionShape = new BoxShape(baseHalfExtents);
Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse);
tr.Origin = localOrigin[0];
collider.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
collider.Friction = Friction;
multiBody.BaseCollider = collider;
}
for (int i = 0; i < multiBody.NumLinks; i++)
{
int parent = multiBody.GetParent(i);
worldToLocal[i + 1] = multiBody.GetParentToLocalRot(i) * worldToLocal[parent + 1];
localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse.Rotate(multiBody.GetRVector(i)));
}
for (int i = 0; i < multiBody.NumLinks; i++)
{
var collider = new MultiBodyLinkCollider(multiBody, i);
collider.CollisionShape = new BoxShape(linkHalfExtents);
Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse) * Matrix.Translation(localOrigin[i + 1]);
collider.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
collider.Friction = Friction;
BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
multiBody.GetLink(i).Collider = collider;
}
}
private void PickMultiBody(MultiBodyLinkCollider collider, ref Vector3 pickPosition)
{
MultiBody multiBody = collider.MultiBody;
if (multiBody == null)
{
return;
}
_prevCanSleep = multiBody.CanSleep;
multiBody.CanSleep = false;
Vector3 pivotInA = multiBody.WorldPosToLocal(collider.Link, pickPosition);
var p2p = new MultiBodyPoint2Point(multiBody, collider.Link, null, pivotInA, pickPosition)
{
MaxAppliedImpulse = 2
};
var world = _demo.Simulation.World as MultiBodyDynamicsWorld;
world.AddMultiBodyConstraint(p2p);
_multiBodyPickConstraint = p2p;
}
/// <summary>
/// A MultiBody object needs to be created and added a controled sequence
/// 1) The multibody needs to be added to the PhysicsWorld.
/// 2) Then the base colliders and other colliders can be created and added. They
/// depend on the multibody reference.
/// 3) Then collision objects can be added.
/// </summary>
internal void CreateColliders()
{
if (m_multibody == null)
{
Debug.LogError("Multibody must exist before creating colliders");
return;
}
m_baseCollider = new MultiBodyLinkCollider(m_multibody, -1);
m_baseCollider.UserObject = this;
//todo validate that shape exists on base and all
BCollisionShape shape = GetComponent <BCollisionShape>();
m_baseCollider.CollisionShape = shape.GetCollisionShape();
Matrix worldTrans = Matrix.RotationQuaternion(transform.rotation.ToBullet());
worldTrans.Origin = transform.position.ToBullet();
m_baseCollider.WorldTransform = worldTrans;
bool isDynamic = (baseMass > 0 && !fixedBase);
m_groupsIBelongTo = isDynamic ? (m_groupsIBelongTo) : (m_groupsIBelongTo | BulletSharp.CollisionFilterGroups.StaticFilter);
m_collisionMask = isDynamic ? (m_collisionMask) : (m_collisionMask | BulletSharp.CollisionFilterGroups.StaticFilter);
m_multibody.BaseCollider = m_baseCollider;
for (int i = 0; i < m_links.Count; i++)
{
//create colliders
MultiBodyLinkCollider col = new MultiBodyLinkCollider(m_multibody, m_links[i].index);
m_links[i].AssignMultiBodyLinkColliderOnCreation(this, col);
col.UserObject = m_links[i];
shape = m_links[i].GetComponent <BCollisionShape>();
col.CollisionShape = shape.GetCollisionShape();
worldTrans = Matrix.RotationQuaternion(m_links[i].transform.rotation.ToBullet());
worldTrans.Origin = m_links[i].transform.position.ToBullet();
col.WorldTransform = worldTrans;
m_multibody.GetLink(i).Collider = col;
}
}
IEnumerator myCoroutine()
{
Debug.Log("Waiting to start");
while (!Input.GetKeyDown(KeyCode.Space))
{
yield return(null);
}
OnInitializePhysics();
Debug.Log("Initialized");
int frame = 0;
while (frame < 10)
{
if (Input.GetKeyDown(KeyCode.Space))
{
frame++;
Debug.Log("Do Update " + frame);
for (int j = 0; j < numLinks; j++)
{
MultiBodyLinkCollider linkCollider = links[j];
for (int k = 0; k < 5; k++)
{
OnUpdate();
}
UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity();
UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
linkPositions[j] = p;
UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
}
}
yield return(null);
}
Debug.Log("Finalize");
ExitPhysics();
}
public void CreateUnityMultiBodyLinkColliderProxy(MultiBodyLinkCollider body)
{
GameObject cube = Instantiate <GameObject>(cubePrefab);
CollisionShape cs = body.CollisionShape;
if (cs is BoxShape)
{
BoxShape bxcs = cs as BoxShape;
BulletSharp.Math.Vector3 s = bxcs.HalfExtentsWithMargin;
MeshRenderer mr = cube.GetComponentInChildren <MeshRenderer>();
mr.transform.localScale = s.ToUnity() * 2f;
Matrix4x4 m = body.WorldTransform.ToUnity();
cube.transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
cube.transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
cube.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
Destroy(cube.GetComponent <BulletRigidBodyProxy>());
BulletMultiBodyLinkColliderProxy cp = cube.AddComponent <BulletMultiBodyLinkColliderProxy>();
cp.target = body;
}
else
{
Debug.LogError("Not implemented");
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -9.81f, 0);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
//Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//multiBody.UseRK4Integration = true;
//multiBody.BaseWorldTransform = Matrix.Identity;
//init the links
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for (int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
multiBody.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
multiBody.CanSleep = canSleep;
multiBody.HasSelfCollision = selfCollide;
multiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
#else
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
CollisionShapes.Add(shape);
var col = new MultiBodyLinkCollider(multiBody, i);
col.CollisionShape = shape;
//const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
public int AddLinkToMultiBody(BMultiBody mb, int currentLinkIndex, int parentIndex, Transform parent)
{
if (isLinked)
{
Debug.LogErrorFormat("Cannot add link {0} to multibody {1} bacause it is already linked", name, mb.name);
return(0);
}
BCollisionShape collisionShape = GetComponent <BCollisionShape>();
if (collisionShape == null)
{
throw new MissingComponentException("Could not find " + typeof(BCollisionShape).Name + " component on BodyLink " + name);
}
multiBody = mb;
linkId = currentLinkIndex;
parentLinkId = parentIndex;
parentTransform = parent;
CollisionShape shape = collisionShape.GetCollisionShape();
if (shape == null)
{
throw new MissingComponentException("Could not get collision shape from " + collisionShape.GetType().Name + " shape component on BodyLink " + name);
}
BulletSharp.Math.Vector3 linkInertia;
shape.CalculateLocalInertia(Mass, out linkInertia);
if (BPhysicsWorld.Get().debugType >= BulletUnity.Debugging.BDebug.DebugType.Debug)
{
Debug.LogFormat(this, "Adding link {0} : {1} to parent {2} of multibody {3}", currentLinkIndex, name, parentIndex, mb.name);
}
SetupLink(linkInertia);
linkCollider = new MultiBodyLinkCollider(mb.MultiBody, currentLinkIndex);
linkCollider.CollisionShape = shape;
linkCollider.WorldTransform = transform.localToWorldMatrix.ToBullet();
linkCollider.CollisionFlags = collisionFlags;
linkCollider.Friction = Friction;
linkCollider.RollingFriction = RollingFriction;
linkCollider.Restitution = Restitution;
linkCollider.UserObject = this;
BPhysicsWorld.Get().world.AddCollisionObject(linkCollider, groupsIBelongTo, collisionMask);
m_collisionObject = linkCollider;
BulletMultiBodyLinkColliderProxy proxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>();
if (proxy == null)
{
proxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>();
}
mb.MultiBody.GetLink(currentLinkIndex).Collider = linkCollider;
proxy.target = linkCollider;
isLinked = true;
foreach (BMultiBodyConstraint mbc in Constraints)
{
mbc.AddConstraintToMultiBody(MultiBody, LinkId);
}
int addedLinks = 1;
for (int i = 0; i < Links.Count; ++i)
{
addedLinks += Links[i].AddLinkToMultiBody(mb, i + currentLinkIndex + 1, currentLinkIndex, transform);
}
return(addedLinks);
}
public virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
switch (Input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F1:
MessageBox.Show(
"Move using WASD + shift\n" +
"Left click - point camera\n" +
"Right click - pick up an object using a Point2PointConstraint\n" +
"Right click + shift - pick up an object using a fixed Generic6DofConstraint\n" +
"Space - shoot box\n" +
"Q - quit\n" +
Graphics.InfoText,
"Help");
// Key release won't be captured
Input.KeysDown.Remove(Keys.F1);
break;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
GraphicsLibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case (Keys.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
using (var serializer = new DefaultSerializer(maxSerializeBufferSize))
{
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0,
dataBytes.Length);
using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create))
{
file.Write(dataBytes, 0, dataBytes.Length);
}
}
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (World != null)
{
Vector3 rayFrom = Freelook.Eye;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
World.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (Input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
World.AddConstraint(dof6);
pickConstraint = 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
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
World.AddConstraint(p2p);
pickConstraint = 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);
*/
}
}
}
else
{
MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
if (multiCol != null && multiCol.MultiBody != null)
{
MultiBody mb = multiCol.MultiBody;
prevCanSleep = mb.CanSleep;
mb.CanSleep = false;
Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);
MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
pickingMultiBodyPoint2Point = p2p;
}
}
oldPickingDist = (pickPos - rayFrom).Length;
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
MovePickedBody();
}
}
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings)
{
int nLinks = settings.NumLinks;
float mass = 13.5f * Scaling;
Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling;
var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep);
//body.HasSelfCollision = false;
//Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI);
Quaternion orn = new Quaternion(0, 0, 0, 1);
body.BasePosition = settings.BasePosition;
body.WorldToBaseRot = orn;
body.BaseVelocity = Vector3.Zero;
Vector3 joint_axis_hinge = new Vector3(1, 0, 0);
Vector3 joint_axis_prismatic = new Vector3(0, 0, 1);
Quaternion parent_to_child = orn.Inverse();
Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic);
Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge);
int this_link_num = -1;
int link_num_counter = 0;
Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling;
Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling;
for (int i = 0; i < nLinks; i++)
{
float initial_joint_angle = 0.3f;
if (i > 0)
{
initial_joint_angle = -0.06f;
}
int child_link_num = link_num_counter++;
if (settings.UsePrismatic) // i == (nLinks - 1))
{
body.SetupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision);
}
else
{
body.SetupRevolute(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision);
}
body.SetJointPos(child_link_num, initial_joint_angle);
this_link_num = i;
/*if (false) //!useGroundShape && i == 4)
* {
* Vector3 pivotInAworld = new Vector3(0, 20, 46);
* Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld);
* Vector3 pivotInBworld = pivotInAworld;
* MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld);
* (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
* }*/
if (settings.UsePrismatic)
{
//MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3);
if (settings.CreateConstraints)
{
MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
}
}
else
{
//if (true)
{
var con = new MultiBodyJointMotor(body, i, 0, 50000);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
}
var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2);
}
}
// Add a collider for the base
Quaternion[] worldToLocal = new Quaternion[nLinks + 1];
Vector3[] localOrigin = new Vector3[nLinks + 1];
worldToLocal[0] = body.WorldToBaseRot;
localOrigin[0] = body.BasePosition;
//Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f);
Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f);
float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 };
//float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W };
//if (true)
{
CollisionShape box = new BoxShape(halfExtents * Scaling);
var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia);
RigidBody bodyB = new RigidBody(bodyInfo);
var collider = new MultiBodyLinkCollider(body, -1);
collider.CollisionShape = box;
Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse()) * Matrix.Translation(localOrigin[0]);
collider.WorldTransform = tr;
bodyB.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
collider.Friction = Friction;
body.BaseCollider = collider;
}
for (int i = 0; i < body.NumLinks; i++)
{
int parent = body.GetParent(i);
worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1];
localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse().Rotate(body.GetRVector(i)));
}
for (int i = 0; i < body.NumLinks; i++)
{
CollisionShape box = new BoxShape(halfExtents * Scaling);
var collider = new MultiBodyLinkCollider(body, i);
collider.CollisionShape = box;
Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse()) * Matrix.Translation(localOrigin[i + 1]);
collider.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
collider.Friction = Friction;
body.GetLink(i).Collider = collider;
//World.DebugDrawer.DrawBox(halfExtents, pos, quat);
}
(World as MultiBodyDynamicsWorld).AddMultiBody(body);
return(body);
}
public PendulumDemoSimulation()
{
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
Broadphase = new DbvtBroadphase();
_solver = new MultiBodyConstraintSolver();
MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration);
World.SetInternalTickCallback(TickCallback, null, true);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
var linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
var baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
var baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
MultiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//MultiBody.UseRK4Integration = true;
//MultiBody.BaseWorldTransform = Matrix.Identity;
//init the links
var hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for (int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
MultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
MultiBody.FinalizeMultiDof();
MultiBodyWorld.AddMultiBody(MultiBody);
MultiBody.CanSleep = canSleep;
MultiBody.HasSelfCollision = selfCollide;
MultiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
MultiBody.LinearDamping = 0.1f;
MultiBody.AngularDamping = 0.9f;
#else
MultiBody.LinearDamping = 0;
MultiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
var col = new MultiBodyLinkCollider(MultiBody, i);
col.CollisionShape = shape;
const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
MultiBody.GetLink(i).Collider = col;
}
}
/// <summary>
/// BMultiBodyLinks do not create their own native parts. That happens as part of the MultiBody creation.
/// After the native MultiBody has been created. The references to the native MultiBodyLinkColliders can be
/// obtatined and are assigned using this function.
/// </summary>
internal void AssignMultiBodyLinkColliderOnCreation(BMultiBody mb, MultiBodyLinkCollider linkCollider)
{
m_linkCollider = linkCollider;
}
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase)
{
BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[]
{
new BulletSharp.Math.Vector4(1, 0, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 1, 1),
new BulletSharp.Math.Vector4(1, 1, 0, 1),
};
int curColor = 0;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f);
BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f);
//mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
float baseMass = fixedBase ? 0.0f : 10.0f;
if (baseMass != 0)
{
//CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape.CalculateLocalInertia(baseMass, out baseInertiaDiag);
shape.Dispose();
}
MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
pMultiBody.BaseWorldTransform = baseWorldTrans;
BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0);
// pMultiBody.setBaseVel(vel);
//init the links
BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0);
//y-axis assumed up
BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset
BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
float q0 = 1.0f * Mathf.PI / 180.0f;
BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0);
quat0.Normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 1.0f;
//if (i==3 || i==2)
// linkMass= 1000;
BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
}
else
{
shape = new SphereShape(radius);
}
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
shape.Dispose();
if (!spherical)
{
//pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i == 0)
{
pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
else
{
parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset
currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset
parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
}
else
{
//pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody.FinalizeMultiDof();
world.AddMultiBody(pMultiBody);
MultiBody mbC = pMultiBody;
mbC.CanSleep = (canSleep);
mbC.HasSelfCollision = (selfCollide);
mbC.UseGyroTerm = (gyro);
//
if (!damping)
{
mbC.LinearDamping = (0.0f);
mbC.AngularDamping = (0.0f);
}
else
{
mbC.LinearDamping = (0.1f);
mbC.AngularDamping = (0.9f);
}
if (numLinks > 0)
{
q0 = 180.0f * Mathf.PI / 180.0f;
if (!spherical)
{
mbC.SetJointPosMultiDof(0, new float[] { q0 });
}
else
{
BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0);
vv.Normalize();
quat0 = new BulletSharp.Math.Quaternion(vv, q0);
quat0.Normalize();
float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W };
mbC.SetJointPosMultiDof(0, quat0fs);
}
}
///
BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion>
world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1];
BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3>
local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1];
world_to_local[0] = pMultiBody.WorldToBaseRot;
local_origin[0] = pMultiBody.BasePosition;
// double friction = 1;
{
if (true)
{
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
// guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1);
col.CollisionShape = shape;
Matrix tr = new Matrix();
tr.ScaleVector = BulletSharp.Math.Vector3.One;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.Origin = local_origin[0];
BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f);
tr.Rotation = (orn);
col.WorldTransform = (tr);
bool isDynamic = (baseMass > 0 && !fixedBase);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2);
BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f);
//guiHelper.createCollisionObjectGraphicsObject(col, color);
// col.setFriction(friction);
pMultiBody.BaseCollider = (col);
}
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
int parent = pMultiBody.GetParent(i);
world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1];
BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i));
local_origin[i + 1] = local_origin[parent + 1] + vv;
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
BulletSharp.Math.Vector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W };
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new SphereShape(radius);
}
//guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i);
col.CollisionShape = (shape);
Matrix tr = new Matrix();
tr.ScaleVector = new BulletSharp.Math.Vector3();
tr.Origin = (posr);
tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3]));
col.WorldTransform = (tr);
// col.setFriction(friction);
bool isDynamic = true;//(linkMass > 0);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
//if (i==0||i>numLinks-2)
{
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2);
BulletSharp.Math.Vector4 color = colors[curColor];
curColor++;
curColor &= 3;
//guiHelper.createCollisionObjectGraphicsObject(col, color);
pMultiBody.GetLink(i).Collider = col;
}
}
return(pMultiBody);
}
public virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
switch (Input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
GraphicsLibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (_world != null)
{
Vector3 rayFrom = Freelook.Eye;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (Input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
_world.AddConstraint(dof6);
pickConstraint = 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
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
pickConstraint = 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);
*/
}
}
}
else
{
MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
if (multiCol != null && multiCol.MultiBody != null)
{
MultiBody mb = multiCol.MultiBody;
prevCanSleep = mb.CanSleep;
mb.CanSleep = false;
Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);
MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
pickingMultiBodyPoint2Point = p2p;
}
}
oldPickingDist = (pickPos - rayFrom).Length;
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
MovePickedBody();
}
}