public void RemoveConstraint(TypedConstraint constraint)
{
RigidBody rigidBody = constraint.RigidBodyA;
if (rigidBody._constraintRefs != null)
{
rigidBody._constraintRefs.Remove(constraint);
}
rigidBody = constraint.RigidBodyB;
if (rigidBody._constraintRefs != null)
{
rigidBody._constraintRefs.Remove(constraint);
}
int itemIndex = _constraints.IndexOf(constraint);
if (itemIndex == -1)
{
return;
}
int lastIndex = _constraints.Count - 1;
_constraints[itemIndex] = _constraints[lastIndex];
_constraints.RemoveAt(lastIndex);
btDynamicsWorld_removeConstraint(Native, constraint.Native);
}
public void RemoveConstraintRef(TypedConstraint constraint)
{
if (_constraintRefs != null)
{
_constraintRefs.Remove(constraint);
btRigidBody_removeConstraintRef(Native, constraint.Native);
}
}
public void RemoveConstraint(BulletSharp.TypedConstraint c)
{
if (!_isDisposed)
{
Debug.LogFormat("Removing constraint {0} from world", c);
World.RemoveConstraint(c);
}
}
public void AddConstraintRef(TypedConstraint constraint)
{
if (_constraintRefs == null)
{
_constraintRefs = new List<TypedConstraint>();
}
_constraintRefs.Add(constraint);
btRigidBody_addConstraintRef(_native, constraint._native);
}
public void RemoveConstraint(TypedConstraint constraint)
{
int itemIndex = _constraints.IndexOf(constraint);
int lastIndex = _constraints.Count - 1;
_constraints[itemIndex] = _constraints[lastIndex];
_constraints.RemoveAt(lastIndex);
btDynamicsWorld_removeConstraint(_native, constraint._native);
}
public void AddConstraintRef(TypedConstraint constraint)
{
if (_constraintRefs == null)
{
_constraintRefs = new List <TypedConstraint>();
}
_constraintRefs.Add(constraint);
btRigidBody_addConstraintRef(Native, constraint.Native);
}
public void RemoveConstraint(BulletSharp.TypedConstraint c)
{
if (!_isDisposed)
{
if (m_worldType < WorldType.RigidBodyDynamics)
{
Log.Warning("World type must not be collision only");
}
((DiscreteDynamicsWorld)m_world).RemoveConstraint(c);
if (c.Userobject is BTypedConstraint)
{
((BTypedConstraint)c.Userobject).m_isInWorld = false;
}
}
}
public void RemoveConstraint(BulletSharp.TypedConstraint c)
{
if (!_isDisposed)
{
if (m_worldType < WorldType.RigidBodyDynamics)
{
BDebug.LogError(debugType, "World type must not be collision only");
}
BDebug.Log(debugType, "Removing constraint {0} from world", c.Userobject);
((DiscreteDynamicsWorld)m_world).RemoveConstraint(c);
if (c.Userobject is BTypedConstraint)
{
((BTypedConstraint)c.Userobject).isInWorld = false;
}
}
}
public void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint2(_native, constraint._native, disableCollisionsBetweenLinkedBodies);
if (disableCollisionsBetweenLinkedBodies)
{
RigidBody rigidBody = constraint.RigidBodyA;
if (rigidBody._constraintRefs == null)
{
rigidBody._constraintRefs = new List<TypedConstraint>();
}
rigidBody._constraintRefs.Add(constraint);
rigidBody = constraint.RigidBodyB;
if (rigidBody._constraintRefs == null)
{
rigidBody._constraintRefs = new List<TypedConstraint>();
}
rigidBody._constraintRefs.Add(constraint);
}
}
void SerializeRigidBodies(Serializer serializer)
{
foreach (CollisionObject colObj in CollisionObjectArray)
{
if (colObj.InternalType == CollisionObjectTypes.RigidBody)
{
int len = colObj.CalculateSerializeBufferSize();
Chunk chunk = serializer.Allocate((uint)len, 1);
string structType = colObj.Serialize(chunk.OldPtr, serializer);
serializer.FinalizeChunk(chunk, structType, DnaID.RigidBody, colObj._native);
}
}
for (int i = 0; i < NumConstraints; i++)
{
TypedConstraint constraint = GetConstraint(i);
int len = constraint.CalculateSerializeBufferSize();
Chunk chunk = serializer.Allocate((uint)len, 1);
string structType = constraint.Serialize(chunk.OldPtr, serializer);
serializer.FinalizeChunk(chunk, structType, DnaID.Constraint, constraint._native);
}
}
public void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies = false)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint(Native, constraint.Native, disableCollisionsBetweenLinkedBodies);
if (disableCollisionsBetweenLinkedBodies)
{
RigidBody rigidBody = constraint.RigidBodyA;
if (rigidBody._constraintRefs == null)
{
rigidBody._constraintRefs = new List <TypedConstraint>();
}
rigidBody._constraintRefs.Add(constraint);
rigidBody = constraint.RigidBodyB;
if (rigidBody._constraintRefs == null)
{
rigidBody._constraintRefs = new List <TypedConstraint>();
}
rigidBody._constraintRefs.Add(constraint);
}
}
void RemovePickingConstraint()
{
if (pickConstraint != null && _world != null)
{
_world.RemoveConstraint(pickConstraint);
pickConstraint.Dispose();
pickConstraint = null;
pickedBody.ForceActivationState(ActivationState.ActiveTag);
pickedBody.DeactivationTime = 0;
pickedBody = null;
}
}
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();
LibraryManager.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;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
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);
*/
}
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
rayCallback.Dispose();
}
}
}
else if (_input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (_input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
public void AddConstraintRef(TypedConstraint c)
{
btRigidBody_addConstraintRef(_native, c._native);
}
public void DebugDrawConstraint(TypedConstraint constraint)
{
btDiscreteDynamicsWorld_debugDrawConstraint(_native, constraint._native);
}
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();
}
}
protected virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
switch (Input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Form.Close();
return;
case Keys.F1:
MessageBox.Show(
"Move using mouse and WASD + shift\n" +
"Space - Shoot box\n" +
"Q - Quit\n\n" +
"G - Toggle shadows\n" +
"L - Toggle deferred lighting\n",
"Help");
return;
case Keys.F3:
//IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F11:
//ToggleFullScreen();
break;
case (Keys.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize);
PhysicsContext.World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length);
System.IO.FileStream file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Dispose();
break;
case Keys.G:
shadowsEnabled = !shadowsEnabled;
break;
case Keys.L:
deferredLightingEnabled = !deferredLightingEnabled;
break;
case Keys.Space:
PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView));
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (PhysicsContext.World != null)
{
Vector3 rayFrom = Freelook.Eye;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
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
};
PhysicsContext.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);
PhysicsContext.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);
*/
}
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB);
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
public void Register(TypedConstraint cst)
{
this.Register(cst, true);
}
public void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint2(_native, constraint._native, disableCollisionsBetweenLinkedBodies);
}
public void RemoveConstraintRef(TypedConstraint c)
{
btRigidBody_removeConstraintRef(_native, c._native);
}
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\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.RayTestRef(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();
}
}
public void DebugDrawConstraint(TypedConstraint constraint)
{
btDiscreteDynamicsWorld_debugDrawConstraint(_native, constraint._native);
}
public void RemoveConstraint(TypedConstraint constraint)
{
RigidBody rigidBody = constraint.RigidBodyA;
if (rigidBody._constraintRefs != null)
{
rigidBody._constraintRefs.Remove(constraint);
}
rigidBody = constraint.RigidBodyB;
if (rigidBody._constraintRefs != null)
{
rigidBody._constraintRefs.Remove(constraint);
}
int itemIndex = _constraints.IndexOf(constraint);
int lastIndex = _constraints.Count - 1;
_constraints[itemIndex] = _constraints[lastIndex];
_constraints.RemoveAt(lastIndex);
btDynamicsWorld_removeConstraint(_native, constraint._native);
}
protected virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
Keys key = Input.KeysPressed[0];
switch (key)
{
case Keys.Escape:
case Keys.Q:
Quit();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F11:
ToggleFullScreen();
break;
case Keys.Space:
PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView));
break;
}
}
if (Input.MousePressed != MouseButtonFlags.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (Input.MousePressed == MouseButtonFlags.RightDown)
{
if (PhysicsContext.World != null)
{
Vector3 rayFrom = Freelook.Eye;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(rayFrom, rayTo);
PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (use6Dof)
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false);
dof6.LinearLowerLimit = Vector3.Zero;
dof6.LinearUpperLimit = Vector3.Zero;
dof6.AngularLowerLimit = Vector3.Zero;
dof6.AngularUpperLimit = Vector3.Zero;
PhysicsContext.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);
PhysicsContext.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);
*/
}
use6Dof = !use6Dof;
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
}
}
else if (Input.MousePressed == MouseButtonFlags.RightUp)
{
if (pickConstraint != null && PhysicsContext.World != null)
{
PhysicsContext.World.RemoveConstraint(pickConstraint);
pickConstraint.Dispose();
pickConstraint = null;
pickedBody.ForceActivationState(ActivationState.ActiveTag);
pickedBody.DeactivationTime = 0;
pickedBody = null;
}
}
}
// Mouse movement
if (Input.MouseDown == MouseButtonFlags.RightDown)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
public void RemoveConstraintRef(TypedConstraint constraint)
{
if (_constraintRefs != null)
{
_constraintRefs.Remove(constraint);
btRigidBody_removeConstraintRef(_native, constraint._native);
}
}
public void RemoveConstraintRef(TypedConstraint c)
{
btRigidBody_removeConstraintRef(_native, c._native);
}
public void Register(TypedConstraint cst, bool collideconnected)
{
this.World.AddConstraint(cst, !collideconnected);
this.constraints.Add(cst);
}
public void AddConstraintRef(TypedConstraint c)
{
btRigidBody_addConstraintRef(_native, c._native);
}
public void Unregister(TypedConstraint cst)
{
this.constraints.Remove(cst);
this.World.RemoveConstraint(cst);
cst.Dispose();
}
//override this one
protected virtual void Dispose(bool isdisposing)
{
if (m_constraintPtr != null) {
m_constraintPtr.Dispose();
m_constraintPtr = null;
}
}
public static void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenBodies)
{
LKernel.GetG<PhysicsMain>().World.AddConstraint(constraint, disableCollisionsBetweenBodies);
}
public void AddConstraint(TypedConstraint constraint)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint(_native, constraint._native);
}
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;
}
}
public void AddConstraint(TypedConstraint constraint, bool disableCollisionsBetweenLinkedBodies)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint2(_native, constraint._native, disableCollisionsBetweenLinkedBodies);
}
public void AddConstraint(TypedConstraint constraint)
{
_constraints.Add(constraint);
btDynamicsWorld_addConstraint(_native, constraint._native);
}
public void RemoveConstraint(TypedConstraint constraint)
{
int itemIndex = _constraints.IndexOf(constraint);
int lastIndex = _constraints.Count - 1;
_constraints[itemIndex] = _constraints[lastIndex];
_constraints.RemoveAt(lastIndex);
btDynamicsWorld_removeConstraint(_native, constraint._native);
}
public bool ConvertAllObjects(BulletFile file)
{
_shapeMap.Clear();
_bodyMap.Clear();
foreach (byte[] bvhData in file.Bvhs)
{
OptimizedBvh bvh = CreateOptimizedBvh();
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
// QuantizedBvhData is parsed in C++, so we need to actually fix pointers
GCHandle bvhDataHandle = GCHandle.Alloc(bvhData, GCHandleType.Pinned);
IntPtr bvhDataPinnedPtr = bvhDataHandle.AddrOfPinnedObject();
IntPtr contiguousNodesHandlePtr = IntPtr.Zero;
IntPtr quantizedContiguousNodesHandlePtr = IntPtr.Zero;
IntPtr subTreeInfoHandlePtr = IntPtr.Zero;
using (var stream = new MemoryStream(bvhData))
{
using (var reader = new BulletReader(stream))
{
long contiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("ContiguousNodesPtr"));
long quantizedContiguousNodesPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr"));
long subTreeInfoPtr = reader.ReadPtr(QuantizedBvhFloatData.Offset("SubTreeInfoPtr"));
using (var writer = new BulletWriter(stream))
{
if (contiguousNodesPtr != 0)
{
GCHandle contiguousNodesHandle = GCHandle.Alloc(file.LibPointers[contiguousNodesPtr], GCHandleType.Pinned);
contiguousNodesHandlePtr = GCHandle.ToIntPtr(contiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("ContiguousNodesPtr");
writer.Write(contiguousNodesHandle.AddrOfPinnedObject());
}
if (quantizedContiguousNodesPtr != 0)
{
GCHandle quantizedContiguousNodesHandle = GCHandle.Alloc(file.LibPointers[quantizedContiguousNodesPtr], GCHandleType.Pinned);
quantizedContiguousNodesHandlePtr = GCHandle.ToIntPtr(quantizedContiguousNodesHandle);
stream.Position = QuantizedBvhFloatData.Offset("QuantizedContiguousNodesPtr");
writer.Write(quantizedContiguousNodesHandle.AddrOfPinnedObject());
}
if (subTreeInfoPtr != 0)
{
GCHandle subTreeInfoHandle = GCHandle.Alloc(file.LibPointers[subTreeInfoPtr], GCHandleType.Pinned);
subTreeInfoHandlePtr = GCHandle.ToIntPtr(subTreeInfoHandle);
stream.Position = QuantizedBvhFloatData.Offset("SubTreeInfoPtr");
writer.Write(subTreeInfoHandle.AddrOfPinnedObject());
}
}
}
}
bvh.DeSerializeFloat(bvhDataPinnedPtr);
bvhDataHandle.Free();
if (contiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(contiguousNodesHandlePtr).Free();
}
if (quantizedContiguousNodesHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(quantizedContiguousNodesHandlePtr).Free();
}
if (subTreeInfoHandlePtr != IntPtr.Zero)
{
GCHandle.FromIntPtr(subTreeInfoHandlePtr).Free();
}
}
foreach (KeyValuePair <long, byte[]> lib in file.LibPointers)
{
if (lib.Value == bvhData)
{
_bvhMap.Add(lib.Key, bvh);
break;
}
}
}
foreach (byte[] shapeData in file.CollisionShapes)
{
CollisionShape shape = ConvertCollisionShape(shapeData, file.LibPointers);
if (shape != null)
{
foreach (KeyValuePair <long, byte[]> lib in file.LibPointers)
{
if (lib.Value == shapeData)
{
_shapeMap.Add(lib.Key, shape);
break;
}
}
using (var stream = new MemoryStream(shapeData, false))
{
using (var reader = new BulletReader(stream))
{
long namePtr = reader.ReadPtr(CollisionShapeFloatData.Offset("Name"));
if (namePtr != 0)
{
byte[] nameData = file.LibPointers[namePtr];
int length = Array.IndexOf(nameData, (byte)0);
string name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
_objectNameMap.Add(shape, name);
_nameShapeMap.Add(name, shape);
}
}
}
}
}
foreach (byte[] solverInfoData in file.DynamicsWorldInfo)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
//throw new NotImplementedException();
}
else
{
//throw new NotImplementedException();
}
}
foreach (byte[] bodyData in file.RigidBodies)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertRigidBodyFloat(bodyData, file.LibPointers);
}
}
foreach (byte[] colObjData in file.CollisionObjects)
{
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
using (var colObjStream = new MemoryStream(colObjData, false))
{
using (var colObjReader = new BulletReader(colObjStream))
{
long shapePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("CollisionShape"));
CollisionShape shape = _shapeMap[shapePtr];
Math.Matrix startTransform = colObjReader.ReadMatrix(CollisionObjectFloatData.Offset("WorldTransform"));
long namePtr = colObjReader.ReadPtr(CollisionObjectFloatData.Offset("Name"));
string name = null;
if (namePtr != 0)
{
byte[] nameData = file.FindLibPointer(namePtr);
int length = Array.IndexOf(nameData, (byte)0);
name = System.Text.Encoding.ASCII.GetString(nameData, 0, length);
}
CollisionObject colObj = CreateCollisionObject(ref startTransform, shape, name);
_bodyMap.Add(colObjData, colObj);
}
}
}
}
foreach (byte[] constraintData in file.Constraints)
{
var stream = new MemoryStream(constraintData, false);
using (var reader = new BulletReader(stream))
{
long collisionObjectAPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyA"));
long collisionObjectBPtr = reader.ReadPtr(TypedConstraintFloatData.Offset("RigidBodyB"));
RigidBody a = null, b = null;
if (collisionObjectAPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectAPtr))
{
a = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectAPtr];
a = RigidBody.Upcast(_bodyMap[coData]);
if (a == null)
{
a = TypedConstraint.GetFixedBody();
}
}
}
if (collisionObjectBPtr != 0)
{
if (!file.LibPointers.ContainsKey(collisionObjectBPtr))
{
b = TypedConstraint.GetFixedBody();
}
else
{
byte[] coData = file.LibPointers[collisionObjectBPtr];
b = RigidBody.Upcast(_bodyMap[coData]);
if (b == null)
{
b = TypedConstraint.GetFixedBody();
}
}
}
if (a == null && b == null)
{
stream.Dispose();
continue;
}
if ((file.Flags & FileFlags.DoublePrecision) != 0)
{
throw new NotImplementedException();
}
else
{
ConvertConstraintFloat(a, b, constraintData, file.Version, file.LibPointers);
}
}
stream.Dispose();
}
return(true);
}
public TypedConstraint GetConstraintRef(int index)
{
return(TypedConstraint.GetManaged(btRigidBody_getConstraintRef(_native, index)));
}