public void Evaluate(int SpreadMax)
{
IConstraintContainer inputWorld = this.contraintContainer[0];
if (inputWorld != null)
{
this.persister.UpdateWorld(inputWorld);
for (int i = 0; i < SpreadMax; i++)
{
if (FCreate[i])
{
RigidBodyPair pair = FPairs[i];
if (pair.body1 != null && pair.body2 != null)
{
Point2PointConstraint cst = new Point2PointConstraint(pair.body1, pair.body2,
this.FPivot1[i].ToBulletVector(), this.FPivot2[i].ToBulletVector());
cst.Setting.Damping = this.FDamping[i];
cst.Setting.ImpulseClamp = this.FImpulseClamp[i];
cst.Setting.Tau = this.FTau[i];
this.persister.Append(cst);
}
}
}
this.persister.Flush();
}
else
{
this.constraintsOutput.SliceCount = 0;
}
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
WorldController world = GetWorld();
if (m_constraintPtr != null)
{
if (m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("BallSocketConstraint needs to be added to a component with a BRigidBody.");
return(false);
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body A");
return(false);
}
if (m_constraintType == ConstraintType.constrainToAnotherBody)
{
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body was not set");
return(false);
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
}
RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body B");
return(false);
}
Vector3 pivotInOther = m_otherRigidBody.transform.InverseTransformPoint(targetRigidBodyA.transform.TransformPoint(m_localConstraintPoint));
m_constraintPtr = new Point2PointConstraint(rbb, rba, pivotInOther.ToBullet(), m_localConstraintPoint.ToBullet());
}
else
{
m_constraintPtr = new Point2PointConstraint(rba, m_localConstraintPoint.ToBullet());
}
m_constraintPtr.Userobject = this;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.OverrideNumSolverIterations = m_overrideNumSolverIterations;
return(true);
}
public void moveObject(Vector3 rayFrom, Vector3 rayTo)
{
if (_pick_constraint != null)
{
Vector3 dir = rayTo - rayFrom;
dir.Normalize();
dir *= _picking_distance_current;
if (_pick_constraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = _pick_constraint as Generic6DofConstraint;
//keep it at the same picking distance
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.Origin = rayFrom + dir;
pickCon.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB);
}
else
{
Point2PointConstraint pickCon = _pick_constraint as Point2PointConstraint;
//keep it at the same picking distance
pickCon.PivotInB = rayFrom + dir;
}
}
}
void PickBody()
{
DropBody();
PickedEntity = RayCastEntity(out Vector3 PickPoint, out Vector3 Normal);
if (PickedEntity != null)
{
DbgDraw.DrawLine(FireOrigin, PickPoint, Time: 5000);
DbgDraw.DrawCircle(PickPoint + Normal * 0.1f, Normal, FishGfx.Color.Red, 5, 8, Time: 5000);
PickDist = Vector3.Distance(FireOrigin, PickPoint);
GlobalPickPoint = PickPoint;
Matrix4x4.Invert(PickedEntity.RigidBody.CenterOfMassTransform, out Matrix4x4 InvCenterOfMass);
LocalPickPoint = PickPoint = Vector3.Transform(PickPoint, InvCenterOfMass);
PickedEntity.Unfreeze();
PickedEntity.DisableSleep();
PointConst = new Point2PointConstraint(PickedEntity.RigidBody, PickPoint);
PointConst.Setting.ImpulseClamp = 10000;
PointConst.Setting.Tau = 0.01f;
PointConst.Setting.Damping = 0.99f;
Map.World.AddConstraint(PointConst);
OldAngularDamping = PickedEntity.RigidBody.AngularDamping;
OldLinearDamping = PickedEntity.RigidBody.LinearDamping;
PickedEntity.RigidBody.SetDamping(OldLinearDamping, 0.99f);
IsPickingBody = true;
}
}
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 Test()
//{
// BulletSharp.Point2PointConstraint pt;
// /BulletSharp.Generic6DofConstraint sdof = new Generic6DofConstraint(
// pt.Setting.
//sdof.sett
//}
public override TypedConstraint GetConstraint(RigidBody body)
{
Point2PointConstraint cst = new Point2PointConstraint(body, this.Pivot);
cst.Setting.Damping = this.Damping;
cst.Setting.ImpulseClamp = this.ImpulseClamp;
cst.Setting.Tau = this.Tau;
return(cst);
}
public void Evaluate(int SpreadMax)
{
for (int i = 0; i < SpreadMax; i++)
{
if (this.FApply[i] && this.FConstraint[i] != null)
{
Point2PointConstraint cst = this.FConstraint[i];
cst.PivotInA = this.FPivot1[i].ToBulletVector();
}
}
}
//Point2point
public IPoint2PointConstraintImp AddPoint2PointConstraint(IRigidBodyImp rigidBodyA, float3 pivotInA)
{
var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
var btRigidBodyA = rigidBodyAImp._rbi;
var btP2PConstraint = new Point2PointConstraint(btRigidBodyA, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z));
BtWorld.AddConstraint(btP2PConstraint);
var retval = new Point2PointConstraintImp();
retval._p2pci = btP2PConstraint;
btP2PConstraint.UserObject = retval;
return(retval);
}
void DropBody()
{
if (!IsPickingBody)
{
return;
}
Map.World.RemoveConstraint(PointConst);
PointConst.Dispose();
PointConst = null;
PickedEntity.Unfreeze();
PickedEntity.RigidBody.SetDamping(OldLinearDamping, OldAngularDamping);
PickedEntity = null;
IsPickingBody = false;
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (constraintPtr != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveConstraint(constraintPtr);
}
}
if (targetRigidBodyA == null)
{
Debug.LogError("Constraint target rigid body was not set.");
return(false);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body A");
return(false);
}
if (constraintType == ConstraintType.constrainToAnotherBody)
{
RigidBody rbb = (RigidBody)targetRigidBodyB.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body B");
return(false);
}
constraintPtr = new Point2PointConstraint(rba, rbb, pivotInA.ToBullet(), pivotInB.ToBullet());
}
else
{
constraintPtr = new Point2PointConstraint(rba, pivotInA.ToBullet());
}
constraintPtr.Userobject = this;
return(true);
}
void MovePickedBody()
{
if (pickConstraint != null)
{
Vector3 rayFrom = Freelook.Eye;
Vector3 newRayTo = GetRayTo(Input.MousePoint, rayFrom, Freelook.Target, Graphics.FieldOfView);
//keep it at the same picking distance
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.Origin = rayFrom + dir;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
pickCon.PivotInB = rayFrom + dir;
}
}
else if (pickingMultiBodyPoint2Point != null)
{
Vector3 rayFrom = Freelook.Eye;
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
Vector3 dir = (newRayTo - rayFrom);
dir.Normalize();
dir *= oldPickingDist;
pickingMultiBodyPoint2Point.PivotInB = rayFrom + dir;
}
}
public IPoint2PointConstraintImp AddPoint2PointConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float3 pivotInA,float3 pivotInB)
{
var rigidBodyAImp = (RigidBodyImp) rigidBodyA;
var btRigidBodyA = rigidBodyAImp._rbi;
var rigidBodyBImp = (RigidBodyImp) rigidBodyB;
var btRigidBodyB = rigidBodyBImp._rbi;
var btP2PConstraint = new Point2PointConstraint(btRigidBodyA, btRigidBodyB,
new Vector3(pivotInA.x, pivotInA.y, pivotInA.z), new Vector3(pivotInB.x, pivotInB.y, pivotInB.z));
BtWorld.AddConstraint(btP2PConstraint);
var retval = new Point2PointConstraintImp();
retval._p2pci = btP2PConstraint;
btP2PConstraint.UserObject = retval;
return retval;
}
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();
}
}
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
{
bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
float dbgDrawSize = constraint.GetDbgDrawSize();
if (dbgDrawSize <= 0f)
{
return;
}
switch (constraint.GetConstraintType())
{
case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE:
{
Point2PointConstraint p2pC = constraint as Point2PointConstraint;
IndexedMatrix tr = IndexedMatrix.Identity;
IndexedVector3 pivot = p2pC.GetPivotInA();
pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
debugDraw.DrawTransform(ref tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
}
break;
case TypedConstraintType.HINGE_CONSTRAINT_TYPE:
{
HingeConstraint pHinge = constraint as HingeConstraint;
IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
{
break;
}
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (drawLimits)
{
IndexedVector3 center = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(2);
IndexedVector3 axis = tr._basis.GetColumn(0);
IndexedVector3 zero = IndexedVector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect);
}
}
break;
case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE:
{
ConeTwistConstraint pCT = constraint as ConeTwistConstraint;
IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
//const float length = float(5);
float length = dbgDrawSize;
const int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments;
IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments;
IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
IndexedVector3 origin = tr._origin;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
}
else
{
tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
}
IndexedVector3 pivot = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
break;
case TypedConstraintType.D6_CONSTRAINT_TYPE:
case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE:
{
Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint;
IndexedMatrix tr = p6DOF.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin;
// up is axis 1 not 2 ?
IndexedVector3 up = tr._basis.GetColumn(1);
IndexedVector3 axis = tr._basis.GetColumn(0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = tr._basis.GetColumn(1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
float cy = (float)Math.Cos(ay);
float sy = (float)Math.Sin(ay);
float cz = (float)Math.Cos(az);
float sz = (float)Math.Sin(az);
IndexedVector3 ref1 = new IndexedVector3(
cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z,
-sz * axis.X + cz * axis.Y,
cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z);
tr = p6DOF.GetCalculatedTransformB();
IndexedVector3 normal = -tr._basis.GetColumn(0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
break;
case TypedConstraintType.SLIDER_CONSTRAINT_TYPE:
{
SliderConstraint pSlider = constraint as SliderConstraint;
IndexedMatrix tr = pSlider.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pSlider.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
IndexedMatrix tr2 = pSlider.GetCalculatedTransformA();
IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f);
IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true);
}
}
break;
default:
break;
}
return;
}
//Initialize and register the constraint
protected override void InitializeConstraint()
{
_constraint = new Point2PointConstraint(_rigidbody.BodyInstance, _connectedBody.BodyInstance, _localPivot.ToBullet(), _connectedPivot.ToBullet());
}
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.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize);
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
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.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)
{
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.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;
}
}
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
string bulletFile;
string[] args = Environment.GetCommandLineArgs();
if (args.Length == 1)
{
bulletFile = "testFile.bullet";
}
else
{
bulletFile = args[1];
}
fileLoader = new CustomBulletWorldImporter(World);
if (!fileLoader.LoadFile(bulletFile))
{
CollisionShape groundShape = new BoxShape(50);
CollisionShapes.Add(groundShape);
RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
Vector3[] positions = new Vector3[2] {
new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f)
};
float[] radi = new float[2] {
0.3f, 0.4f
};
CollisionShape colShape = new MultiSphereShape(positions, radi);
//CollisionShape colShape = new CapsuleShapeZ(1, 1);
//CollisionShape colShape = new CylinderShapeZ(1, 1, 1);
//CollisionShape colShape = new BoxShape(1);
//CollisionShape colShape = new SphereShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
DefaultSerializer serializer = new DefaultSerializer();
serializer.RegisterNameForObject(ground, "GroundName");
for (i = 0; i < CollisionShapes.Count; i++)
{
serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString());
}
Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0));
World.AddConstraint(p2p);
serializer.RegisterNameForObject(p2p, "constraintje");
World.Serialize(serializer);
BulletSharp.DataStream data = serializer.LockBuffer();
byte[] dataBytes = new byte[data.Length];
data.Read(dataBytes, 0, dataBytes.Length);
FileStream file = new FileStream("testFile.bullet", FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Close();
}
}
public Physics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
float mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
if (P2P)
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0,body1,pivotInA,pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0,body1,pivotInA,pivotInB,axisInA,axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
else
{
HingeConstraint hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.0f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
float angle = 0;//SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA);
spSlider6Dof.LinearLowerLimit = lowerSliderLimit;
spSlider6Dof.LinearUpperLimit = hiSliderLimit;
//range should be small, otherwise singularities will 'explode' the constraint
//spSlider6Dof.AngularLowerLimit = new Vector3(-1.5f,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(1.5f,0,0);
//spSlider6Dof.AngularLowerLimit = new Vector3(0,0,0);
//spSlider6Dof.AngularUpperLimit = new Vector3(0,0,0);
spSlider6Dof.AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0);
spSlider6Dof.AngularUpperLimit = new Vector3(1.5f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 PivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, btPivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
pGen6DOFSpring.LinearUpperLimit = new Vector3(5, 0, 0);
pGen6DOFSpring.LinearLowerLimit = new Vector3(-5, 0, 0);
pGen6DOFSpring.AngularLowerLimit = new Vector3(0, 0, -1.5f);
pGen6DOFSpring.AngularUpperLimit = new Vector3(0, 0, 1.5f);
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.DebugDrawSize = 5;
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
Vector3 pivotA = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
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();
}
}
public SerializeDemoSimulation()
{
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration);
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
string bulletFile;
string[] args = Environment.GetCommandLineArgs();
if (args.Length == 1)
{
bulletFile = "testFile.bullet";
}
else
{
bulletFile = args[1];
}
_fileLoader = new CustomBulletWorldImporter(World);
if (!_fileLoader.LoadFile(bulletFile))
{
var groundShape = new BoxShape(50);
_collisionShapes.Add(groundShape);
RigidBody ground = PhysicsHelper.CreateStaticBody(Matrix.Translation(0, -50, 0), groundShape, World);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
Vector3[] positions = new[] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) };
float[] radi = new float[2] {
0.3f, 0.4f
};
var colShape = new MultiSphereShape(positions, radi);
//var colShape = new CapsuleShapeZ(1, 1);
//var colShape = new CylinderShapeZ(1, 1, 1);
//var colShape = new BoxShape(1);
//var colShape = new SphereShape(1);
_collisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
float startX = StartPosX - NumObjectsX / 2;
float startY = StartPosY;
float startZ = StartPosZ - NumObjectsZ / 2;
for (int y = 0; y < NumObjectsY; y++)
{
for (int x = 0; x < NumObjectsX; x++)
{
for (int z = 0; z < NumObjectsZ; z++)
{
Matrix startTransform = Matrix.Translation(
2 * x + startX,
2 * y + startY,
2 * z + startZ
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
using (var serializer = new DefaultSerializer())
{
serializer.RegisterNameForObject(ground, "GroundName");
for (int i = 0; i < _collisionShapes.Count; i++)
{
serializer.RegisterNameForObject(_collisionShapes[i], "name" + i.ToString());
}
var p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2],
new Vector3(0, 1, 0));
World.AddConstraint(p2p);
serializer.RegisterNameForObject(p2p, "constraintje");
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length);
using (var file = new FileStream("testFile.bullet", FileMode.Create))
{
file.Write(dataBytes, 0, dataBytes.Length);
}
}
}
}
//------------------------------------------------------
// Picking
//------------------------------------------------------
public void pickObject(Vector3 rayFrom, Vector3 rayTo, bool use6Dof)
{
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_physics_world.world.RayTest(rayFrom, rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
_picked_body = body;
_picked_body.ActivationState = ActivationState.DisableDeactivation;
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;
_physics_world.world.AddConstraint(dof6);
_pick_constraint = 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);
_physics_world.world.AddConstraint(p2p);
_pick_constraint = 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);
*/
}
_picking_distance_original = (pickPos - rayFrom).Length;
_picking_distance_current = _picking_distance_original;
}
}
}
}
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;
using (var serializer = new DefaultSerializer(maxSerializeBufferSize))
{
PhysicsContext.World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
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.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;
var 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
};
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
{
var p2p = new Point2PointConstraint(body, localPivot);
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);
*/
}
PhysicsContext.World.AddConstraint(pickConstraint);
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;
}
}
}
}
protected virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
Keys key = Input.KeysPressed[0];
switch (key)
{
case Keys.Escape:
case Keys.Q:
Form.Close();
return;
case Keys.F3:
//IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F11:
//ToggleFullScreen();
break;
case Keys.G:
shadowsEnabled = !shadowsEnabled;
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(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.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.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;
}
}
}
}
private void HandleMouseInput()
{
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, FreeLook.Eye, FreeLook.Target, Graphics.FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
Vector3 rayFrom = FreeLook.Eye;
var rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
Simulation.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
};
Simulation.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);
Simulation.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
{
var 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);
var p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(Simulation.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 override void InitializeDemo()
{
//string filename = @"E:\users\man\bullet\xna-constraint-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
//maxiterations = 100;
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
SetCameraDistance(26f);
//CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f));
CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.Identity;
groundTransform._origin = new IndexedVector3(0, -56, 0);
RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape);
CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS));
m_collisionShapes.Add(shape);
IndexedMatrix trans = IndexedMatrix.Identity;
trans._origin = new IndexedVector3(0, 20, 0);
float mass = 1f;
#if true
//point to point constraint with a breaking threshold
{
trans = IndexedMatrix.Identity;
trans._origin = new IndexedVector3(1, 30, -5);
LocalCreateRigidBody(mass, trans, shape);
trans._origin = new IndexedVector3(0, 0, -5);
RigidBody body0 = LocalCreateRigidBody(mass, trans, shape);
trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0);
mass = 1.0f;
RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape);
IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, 0);
TypedConstraint p2p = new Point2PointConstraint(body0, ref pivotInA);
m_dynamicsWorld.AddConstraint(p2p);
p2p.SetBreakingImpulseThreshold(10.2f);
p2p.SetDbgDrawSize(5.0f);
}
#endif
#if true
//point to point constraint (ball socket)
//SEEMS OK
{
//trans = IndexedMatrix.Identity;
RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape);
trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0);
mass = 1f;
RigidBody body1 = null; //localCreateRigidBody( mass,trans,shape);
IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS);
IndexedVector3 axisInA = new IndexedVector3(0, 0, 1);
IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA;
IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) :
body0.GetCenterOfMassTransform()._basis *axisInA;
#if P2P
TypedConstraint p2p = new Point2PointConstraint(body0, ref pivotInA);
//btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB);
//btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
m_dynamicsWorld.AddConstraint(p2p);
p2p.SetDbgDrawSize(5.0f);
#else
HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false);
float targetVelocity = 1f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
m_dynamicsWorld.AddConstraint(hinge); //p2p);
hinge.SetDbgDrawSize(5f);
#endif
}
#endif
#if true
//create a slider, using the generic D6 constraint
// SEEMS OK
{
mass = 1f;
IndexedVector3 sliderWorldPos = new IndexedVector3(0, 10, 0);
IndexedVector3 sliderAxis = new IndexedVector3(1, 0, 0);
float angle = 0f;//SIMD_RADS_PER_DEG * 10.f;
IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(new IndexedQuaternion(sliderAxis, angle));
trans = IndexedMatrix.Identity;
trans._origin = sliderWorldPos;
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = LocalCreateRigidBody(mass, ref trans, shape);
d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null);
m_dynamicsWorld.AddRigidBody(fixedBody1);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.Identity;
frameInB = IndexedMatrix.Identity;
frameInA._origin = new IndexedVector3(0, 5, 0);
frameInB._origin = new IndexedVector3(0, 5, 0);
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA);
spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit);
spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0);
IndexedVector3 angularUpper = -angularLower;
spSlider6Dof.SetAngularLowerLimit(ref angularLower);
spSlider6Dof.SetAngularUpperLimit(ref angularUpper);
// slider.setAngularLowerLimit(IndexedVector3(0,0,0));
// slider.setAngularUpperLimit(IndexedVector3(0,0,0));
spSlider6Dof.SetAngularLowerLimit(new IndexedVector3(-MathUtil.SIMD_PI, 0, 0));
spSlider6Dof.SetAngularUpperLimit(new IndexedVector3(1.5f, 0, 0));
spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f;
spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f;
m_dynamicsWorld.AddConstraint(spSlider6Dof);
spSlider6Dof.SetDbgDrawSize(5f);
}
#endif
#if true
{ // create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.Add(pDoorShape);
IndexedMatrix doorTrans = IndexedMatrix.Identity;
doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape);
pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false);
spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f);
m_dynamicsWorld.AddConstraint(spDoorHinge);
spDoorHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a generic 6DOF constraint
// SEEMS OK - But debug draw a bit wrong?
IndexedMatrix tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(10f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
// RigidBody pBodyA = localCreateRigidBody( mass, tr, shape);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
// RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(0f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0);
frameInB = IndexedMatrix.CreateTranslation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1);
pGen6DOF.SetLinearLowerLimit(ref linearLower);
IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1);
pGen6DOF.SetLinearUpperLimit(ref linearUpper);
// ? why again?
//linearLower = new IndexedVector3(-10,0,0);
//pGen6DOF.setLinearLowerLimit(ref linearLower);
// pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.));
// pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.));
// pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.));
// pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true;
// pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f;
// pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f;
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI));
IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f);
IndexedVector3 angularUpper = -angularLower;
pGen6DOF.SetAngularLowerLimit(ref angularLower);
pGen6DOF.SetAngularUpperLimit(ref angularUpper);
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.));
m_dynamicsWorld.AddConstraint(pGen6DOF, true);
pGen6DOF.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a ConeTwist constraint
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.CreateTranslation(-10, -5, 0);
RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape);
IndexedMatrix frameInA, frameInB;
frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInA._origin = new IndexedVector3(0, -5, 0);
frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInB._origin = new IndexedVector3(0, 5, 0);
ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB);
pCT.SetLimit(MathUtil.SIMD_QUARTER_PI * 0.6f, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 0.5f); // soft limit == hard limit
m_dynamicsWorld.AddConstraint(pCT, true);
pCT.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
// WORKS OK
IndexedMatrix tr = IndexedMatrix.Identity;
RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape);
pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f);
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false);
// pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
m_dynamicsWorld.AddConstraint(pHinge);
pHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(20, 4, 0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(20, 0, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some (arbitrary) data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f);
IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f);
pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.SetDbgDrawSize(5.0f);
}
#endif
#if true
// WORKS OK
{ // create a generic 6DOF constraint with springs
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
tr = IndexedMatrix.CreateTranslation(-10, 16, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f);
IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f);
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f));
pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f));
pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f));
pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f));
m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.SetDbgDrawSize(5.0f);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
}
#endif
#if true
{
// WORKS OK
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f);
pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, -2f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB:
tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f);
IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB);
spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // 6DOF connected to the world, with motor
IndexedMatrix tr = IndexedMatrix.CreateTranslation(10, -15, 0);
RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape);
pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameB = IndexedMatrix.Identity;
Generic6DofConstraint pGen6Dof = new Generic6DofConstraint(pBody, ref frameB, false);
m_dynamicsWorld.AddConstraint(pGen6Dof);
pGen6Dof.SetDbgDrawSize(5.0f);
pGen6Dof.SetAngularLowerLimit(new IndexedVector3(0, 0, 0));
pGen6Dof.SetAngularUpperLimit(new IndexedVector3(0, 0, 0));
pGen6Dof.SetLinearLowerLimit(new IndexedVector3(-10.0f, 0, 0));
pGen6Dof.SetLinearUpperLimit(new IndexedVector3(10.0f, 0, 0));
pGen6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
pGen6Dof.GetTranslationalLimitMotor().m_targetVelocity[0] = 5.0f;
pGen6Dof.GetTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f;
}
#endif
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI / 4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2 / L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
/// <summary>
/// Called when the game has determined that game logic needs to be processed.
/// </summary>
/// <param name="gameTime">Time passed since the last call to this function.</param>
protected override void Update(GameTime gameTime)
{
MouseState mouseState = Mouse.GetState();
Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y);
if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released)
{
shootBox(rayTo);
}
if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released)
{
if (_world != null)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo);
_world.RayTest(_camera.Position, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null)
{
//other exclusions?
if (!(body.IsStaticObject || body.IsKinematicObject))
{
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform));
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
_pickConstraint = p2p;
//save mouse position for dragging
_oldPickingPos = rayTo;
Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z);
_oldPickingDist = (eyePos - pickPos).Length();
//very weak constraint for picking
p2p.Settings.Tau = 1.1f;
}
}
}
}
}
else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed)
{
if (_pickConstraint != null && _world != null)
{
_world.RemoveConstraint(_pickConstraint);
_pickConstraint = null;
_pickedBody.ForceActivationState(ActivationState.Active);
_pickedBody.DeactivationTime = 0f;
_pickedBody = null;
}
}
if (_pickConstraint != null)
{
//move the constraint pivot
Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint;
if (p2p != null)
{
//keep it at the same picking distance
Vector3 dir = rayTo - _camera.Position;
dir.Normalize();
dir *= _oldPickingDist;
Vector3 newPos = _camera.Position + dir;
p2p.PivotInB = newPos;
}
}
_prevMouseState = mouseState;
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
//world.stepSimulation(1.0f/60.0f,0);
int numObjects = _world.CollisionObjectsCount;
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = _world.CollisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.MotionState != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState;
myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform;
colObj.WorldTransform = myMotionState.GraphicsWorldTransform;
colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform;
colObj.Activate();
}
//removed cached contact points
_world.Broadphase.CleanProxyFromPairs(colObj.Broadphase);
if (body != null && !body.IsStaticObject)
{
RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0);
RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0);
}
}
}
}
else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
Exit();
}
else
{
//world.stepSimulation(1.0f / 60.0f, 1);
}
base.Update(gameTime);
}
