public static BM.Matrix Cast(this Matrix4x4 mat)
{
var output = new BM.Matrix();
*(Matrix4x4 *)&output = mat;
return(output);
}
public static Matrix4x4 Cast(this BM.Matrix mat)
{
var output = new Matrix4x4();
*(BM.Matrix *)&output = mat;
return(output);
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
_xx = transform.rotation.x * transform.rotation.x;
_yy = transform.rotation.y * transform.rotation.y;
_zz = transform.rotation.z * transform.rotation.z;
_xy = transform.rotation.x * transform.rotation.y;
_zw = transform.rotation.z * transform.rotation.w;
_zx = transform.rotation.z * transform.rotation.x;
_yw = transform.rotation.y * transform.rotation.w;
_yz = transform.rotation.y * transform.rotation.z;
_xw = transform.rotation.x * transform.rotation.w;
matrix.M11 = 1.0f - (2.0f * (_yy + _zz));
matrix.M12 = 2.0f * (_xy + _zw);
matrix.M13 = 2.0f * (_zx - _yw);
matrix.M21 = 2.0f * (_xy - _zw);
matrix.M22 = 1.0f - (2.0f * (_zz + _xx));
matrix.M23 = 2.0f * (_yz + _xw);
matrix.M31 = 2.0f * (_zx + _yw);
matrix.M32 = 2.0f * (_yz - _xw);
matrix.M33 = 1.0f - (2.0f * (_yy + _xx));
matrix.M41 = transform.position.x;
matrix.M42 = transform.position.y;
matrix.M43 = transform.position.z;
worldTrans = matrix;
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
//Matrix4x4 trans = transform.localToWorldMatrix;
//worldTrans = trans.ToBullet();
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
}
public override void DrawSphere(float radius, ref BM.Matrix trans, ref BM.Vector3 color)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawSphere(pos, rot, scale, UnityEngine.Vector3.one * radius, c);
}
public override void DrawPlane(ref BM.Vector3 planeNormal, float planeConst, ref BM.Matrix trans, ref BM.Vector3 color)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawPlane(pos, rot, scale, planeNormal.ToUnity(), planeConst, c);
}
public override void DrawCapsule(float radius, float halfHeight, int upAxis, ref BM.Matrix trans, ref BM.Vector3 color)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawCapsule(pos, rot, scale, radius, halfHeight, upAxis, c);
}
public override void DrawCapsule(float radius, float halfHeight, int upAxis, ref BM.Matrix trans, ref BM.Vector3 color)
{
var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
var c = new Color(color.X, color.Y, color.Z);
BUtility.DebugDrawCapsule(pos, rot, scale, radius, halfHeight, upAxis, c);
}
public override void DrawSphere(float radius, ref BM.Matrix trans, ref BM.Vector3 color)
{
var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
var c = new Color(color.X, color.Y, color.Z);
BUtility.DebugDrawSphere(pos, rot, scale, Vector3.one * radius, c);
}
public override void DrawBox(ref BM.Vector3 bbMin, ref BM.Vector3 bbMax, ref BM.Matrix trans, ref BM.Vector3 color)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
UnityEngine.Vector3 size = (bbMax - bbMin).ToUnity() / 2;
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawBox(pos, rot, scale, size, c);
}
public override void DrawPlane(ref BM.Vector3 planeNormal, float planeConst, ref BM.Matrix trans, ref BM.Vector3 color)
{
var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
var c = new Color(color.X, color.Y, color.Z);
BUtility.DebugDrawPlane(pos, rot, scale, planeNormal.ToUnity(), planeConst, c);
}
public override void DrawBox(ref BM.Vector3 bbMin, ref BM.Vector3 bbMax, ref BM.Matrix trans, ref BM.Vector3 color)
{
var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
var size = (bbMax - bbMin).ToUnity();
var c = new Color(color.X, color.Y, color.Z);
BUtility.DebugDrawBox(pos, rot, scale, size, c);
}
//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref BM.Matrix m)
{
// gRally
IsChanged = true;
NewPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
NewRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
// OPT NewScale = Native.UtoB(BSExtensionMethods2.ExtractScaleFromMatrix(ref m));
//transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
//transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
//transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
}
public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
{
UnityEngine.Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
UnityEngine.Vector3 p1 = pos + rot * UnityEngine.Vector3.up * orthoLen;
UnityEngine.Vector3 p2 = pos - rot * UnityEngine.Vector3.up * orthoLen;
UnityEngine.Gizmos.DrawLine(p1, p2);
p1 = pos + rot * UnityEngine.Vector3.right * orthoLen;
p2 = pos - rot * UnityEngine.Vector3.right * orthoLen;
UnityEngine.Gizmos.DrawLine(p1, p2);
p1 = pos + rot * UnityEngine.Vector3.forward * orthoLen;
p2 = pos - rot * UnityEngine.Vector3.forward * orthoLen;
UnityEngine.Gizmos.DrawLine(p1, p2);
}
//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref BM.Matrix m)
{
double timeStamp = -1;
if (threadHelper != null)
{
timeStamp = threadHelper.TotalSimulationTime;
}
lock (lck)
{
previousBulletTransform = lastBulletTransform;
lastBulletTransform = new BulletTransformData(m, timeStamp);
mustUpdateTransform = true;
}
}
public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
{
var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
var p1 = pos + rot * Vector3.up * orthoLen;
var p2 = pos - rot * Vector3.up * orthoLen;
Gizmos.DrawLine(p1, p2);
p1 = pos + rot * Vector3.right * orthoLen;
p2 = pos - rot * Vector3.right * orthoLen;
Gizmos.DrawLine(p1, p2);
p1 = pos + rot * Vector3.forward * orthoLen;
p2 = pos - rot * Vector3.forward * orthoLen;
Gizmos.DrawLine(p1, p2);
}
/// <summary>
/// 位置更新。获取bullet的位置信息，赋值给服务端及unity
/// </summary>
/// <param name="m"></param>
//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref BM.Matrix m)
{
i++;
if (this.i == 1)
{
sw = new Stopwatch();
sw.Start();
}
unit.Position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
unit.Quaternion = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
if (unit.Position.y < -5000)
{
sw.Stop();
long xx = this.sw.ElapsedMilliseconds;
}
}
public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
{
Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
Vector3 p1 = pos + rot * Vector3.up * orthoLen;
Vector3 p2 = pos - rot * Vector3.up * orthoLen;
Gizmos.matrix = Matrix4x4.identity;
Gizmos.DrawLine(p1, p2);
p1 = pos + rot * Vector3.right * orthoLen;
p2 = pos - rot * Vector3.right * orthoLen;
Gizmos.DrawLine(p1, p2);
p1 = pos + rot * Vector3.forward * orthoLen;
p2 = pos - rot * Vector3.forward * orthoLen;
Gizmos.DrawLine(p1, p2);
}
public Matrix(BsMatrix m)
{
this.M11 = m.M11;
this.M12 = m.M12;
this.M13 = m.M13;
this.M14 = m.M14;
this.M21 = m.M21;
this.M22 = m.M22;
this.M23 = m.M23;
this.M24 = m.M24;
this.M31 = m.M31;
this.M32 = m.M32;
this.M33 = m.M33;
this.M34 = m.M34;
this.M41 = m.M41;
this.M42 = m.M42;
this.M43 = m.M43;
this.M44 = m.M44;
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
BulletSharp.Math.Vector3 pos = unit.Position.ToBullet();
BulletSharp.Math.Quaternion rot = unit.Quaternion.ToBullet();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out worldTrans);
}
//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 (m_constraintPtr != null)
{
if (m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("BSliderConstraint 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");
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");
return(false);
}
BM.Matrix frameInA, frameInOther;
string errormsg = "";
if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
{
m_constraintPtr = new SliderConstraint(rbb, rba, frameInOther, frameInA, true);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
else
{
BulletSharp.Math.Matrix frameInA = BM.Matrix.Identity;
string errormsg = "";
if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInA, ref errormsg))
{
m_constraintPtr = new SliderConstraint(rba, frameInA, true);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
SliderConstraint sl = (SliderConstraint)m_constraintPtr;
sl.LowerLinearLimit = m_lowerLinearLimit;
sl.UpperLinearLimit = m_upperLinearLimit;
sl.LowerAngularLimit = m_lowerAngularLimitRadians;
sl.UpperAngularLimit = m_upperAngularLimitRadians;
m_constraintPtr.Userobject = this;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
return(true);
}
//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref BM.Matrix m)
{
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m, ref tempVector3);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m, ref tempQuaternion);
}
//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref BM.Matrix m)
{
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
}
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_constraintPtr != null && m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("Fixed Constraint 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");
return(false);
}
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body is 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");
return(false);
}
initialTransform = BM.Matrix.AffineTransformation(1f, localRotationOffset.ToBullet(), m_localConstraintPoint.ToBullet());
m_constraintPtr = new FixedConstraint(rba, rbb, BM.Matrix.Identity, initialTransform)
{
Userobject = this,
DebugDrawSize = m_debugDrawSize,
BreakingImpulseThreshold = m_breakingImpulseThreshold,
OverrideNumSolverIterations = m_overrideNumSolverIterations
};
return(true);
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out worldTrans);
}
//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps);
//Create a World
Debug.Log("Initialize physics");
CollisionConfiguration CollisionConf;
CollisionDispatcher Dispatcher;
BroadphaseInterface Broadphase;
CollisionWorld cw;
ConstraintSolver Solver;
BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;
//This should create a copy of the BPhysicsWorld with the same settings
BPhysicsWorld bw = BPhysicsWorld.Get();
bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
World = (DiscreteDynamicsWorld)cw;
//Copy all existing rigidbodies in scene
// IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
// this is because collisions must be resolved in the same order for the sim to be deterministic
DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world;
BulletSharp.RigidBody bulletBallRb = null;
BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity;
for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
{
CollisionObject co = sourceWorld.CollisionObjectArray[i];
if (co != null && co.UserObject is BRigidBody)
{
BRigidBody rb = (BRigidBody)co.UserObject;
float mass = rb.isDynamic() ? rb.mass : 0f;
BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
CollisionShape shape = null;
if (existingShape is BSphereShape)
{
shape = ((BSphereShape)existingShape).CopyCollisionShape();
}
else if (existingShape is BBoxShape)
{
shape = ((BBoxShape)existingShape).CopyCollisionShape();
}
RigidBody bulletRB = null;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin;
BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
bulletRB.WorldTransform = mm;
World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
if (rb == ballRb)
{
bulletBallRb = bulletRB;
bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
}
}
}
//Step the simulation numberOfSimulationSteps times
for (int i = 0; i < numberOfSimulationSteps; i++)
{
int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
UnityEngine.Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
//called by Physics World just before constraint is added to world.
//the current constraint properties are used to rebuild the constraint.
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
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("B6DOFConstraint 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");
return(false);
}
if (m_constraintType == ConstraintType.constrainToAnotherBody)
{
if (m_otherRigidBody == null)
{
Debug.LogError("Other Rigid Body is not set.");
return(false);
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
return(false);
}
RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
BM.Matrix frameInA, frameInOther;
string errormsg = "";
if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
{
m_constraintPtr = new Generic6DofConstraint(rbb, rba, frameInOther, frameInA, true);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
else
{
//TODO think about this
BM.Matrix frameInA = BulletSharp.Math.Matrix.Identity;
m_constraintPtr = new Generic6DofConstraint(rba, frameInA, false);
}
m_constraintPtr.Userobject = this;
Generic6DofConstraint sl = (Generic6DofConstraint)m_constraintPtr;
sl.LinearLowerLimit = m_linearLimitLower.ToBullet();
sl.LinearUpperLimit = m_linearLimitUpper.ToBullet();
sl.AngularLowerLimit = m_angularLimitLowerRadians.ToBullet();
sl.AngularUpperLimit = m_angularLimitUpperRadians.ToBullet();
sl.TranslationalLimitMotor.TargetVelocity = m_motorLinearTargetVelocity.ToBullet();
sl.TranslationalLimitMotor.MaxMotorForce = m_motorLinearMaxMotorForce.ToBullet();
sl.BreakingImpulseThreshold = m_breakingImpulseThreshold;
sl.DebugDrawSize = m_debugDrawSize;
return(true);
}
public bool CreateFrame(Vector3 forward, Vector3 up, Vector3 constraintPoint, ref BM.Matrix m, ref string errorMsg)
{
BM.Vector4 x;
BM.Vector4 y;
BM.Vector4 z;
if (forward == Vector3.zero)
{
errorMsg = "forward vector must not be zero";
return(false);
}
forward.Normalize();
if (up == Vector3.zero)
{
errorMsg = "up vector must not be zero";
return(false);
}
Vector3 right = Vector3.Cross(forward, up);
if (right == Vector3.zero)
{
errorMsg = "forward and up vector must not be colinear";
return(false);
}
up = Vector3.Cross(right, forward);
right.Normalize();
up.Normalize();
x.X = forward.x; x.Y = forward.y; x.Z = forward.z; x.W = 0f;
y.X = up.x; y.Y = up.y; y.Z = up.z; y.W = 0f;
z.X = right.x; z.Y = right.y; z.Z = right.z; z.W = 0f;
m.Row1 = x;
m.Row2 = y;
m.Row3 = z;
m.Origin = constraintPoint.ToBullet();
return(true);
}
// Object A has the constraint compontent and is constrainted to object B
// The constraint frame is defined relative to A by three vectors.
// This method calculates the frames A and B that need to be passed to bullet
public bool CreateFramesA_B(Vector3 forwardInA, Vector3 upInA, Vector3 constraintPivotInA, out BM.Matrix frameInA, out BM.Matrix frameInB, ref string errorMsg)
{
frameInA = BM.Matrix.Identity;
if (!CreateFrame(forwardInA, upInA, constraintPivotInA, ref frameInA, ref errorMsg))
{
frameInB = BM.Matrix.Identity;
return(false);
}
BM.Vector4 x = frameInA.Row1;
BM.Vector4 y = frameInA.Row2;
BM.Vector4 z = frameInA.Row3;
Vector3 xx = new Vector3(x.X, x.Y, x.Z);
Vector3 yy = new Vector3(y.X, y.Y, y.Z);
Vector3 zz = new Vector3(z.X, z.Y, z.Z);
Quaternion q = transform.localRotation * Quaternion.Inverse(m_otherRigidBody.transform.localRotation);
xx = q * xx;
yy = q * yy;
zz = q * zz;
frameInB = BM.Matrix.Identity;
frameInB.Row1 = new BM.Vector4(xx.ToBullet(), 0f);
frameInB.Row2 = new BM.Vector4(yy.ToBullet(), 0f);
frameInB.Row3 = new BM.Vector4(zz.ToBullet(), 0f);
frameInB.Origin = m_otherRigidBody.transform.InverseTransformPoint(transform.TransformPoint(constraintPivotInA)).ToBullet();
return(true);
}
//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 (m_constraintPtr != null)
{
if (m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
return(false);
}
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("ConeTwistConstraint needs to be added to a component with a BRigidBody.");
return(false);
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
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);
}
BM.Matrix frameInA, frameInOther;
string errormsg = "";
if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
{
m_constraintPtr = new ConeTwistConstraint((RigidBody)m_otherRigidBody.GetCollisionObject(), (RigidBody)targetRigidBodyA.GetCollisionObject(), frameInOther, frameInA);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
else
{
//TODO this is broken
string errormsg = "";
BM.Matrix frameInB = BM.Matrix.Identity;
if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInB, ref errormsg))
{
m_constraintPtr = new ConeTwistConstraint((RigidBody)targetRigidBodyA.GetCollisionObject(), frameInB);
}
else
{
Debug.LogError(errormsg);
return(false);
}
}
ConeTwistConstraint sl = (ConeTwistConstraint)m_constraintPtr;
sl.SetLimit(m_swingSpan1Radians, m_swingSpan2Radians, m_twistSpanRadians, m_softness, m_biasFactor, m_relaxationFactor);
m_constraintPtr.Userobject = this;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
return(true);
}
