//Bullet calls this so I can copy bullet data to unity
public override void SetWorldTransform(ref Matrix m)
{
/*
* BulletSharp.Math.Vector3 pos = m.Origin;
* UnityEngine.Quaternion q = new UnityEngine.Quaternion();
* q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2;
* q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2;
* q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2;
* q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2;
* q.x *= Mathf.Sign(q.x * (m[1, 2] - m[2, 1]));
* q.y *= Mathf.Sign(q.y * (m[2, 0] - m[0, 2]));
* q.z *= Mathf.Sign(q.z * (m[0, 1] - m[1, 2]));
*/
//todo not very efficient
/*
* Matrix4x4 mu = m.ToUnity();
* UnityEngine.Vector3 p = BSExtensionMethods.ExtractTranslationFromMatrix(ref mu);
* UnityEngine.Quaternion q = BSExtensionMethods.ExtractRotationFromMatrix(ref mu);
* UnityEngine.Vector3 sc = BSExtensionMethods.ExtractScaleFromMatrix(ref mu);
*
* UnityEngine.Vector3 p1 = BSExtensionMethods.ExtractTranslationFromMatrix(ref m);
* UnityEngine.Quaternion q1 = BSExtensionMethods.ExtractRotationFromMatrix(ref m);
* UnityEngine.Vector3 sc1 = BSExtensionMethods.ExtractScaleFromMatrix(ref m);
*
* if (p != p1) Debug.Log("Dont match p " + p + " " + p1);
* if (q != q1) Debug.Log("Dont match q " + q + " " + q1);
* if (sc != sc1) Debug.Log("Dont match p " + sc + " " + sc1);
*/
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
}
// Update is called once per frame
public void Update()
{
lock (lck)
{
if (mustUpdateTransform)
{
UnityEngine.Vector3 position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref lastBulletTransform.Transform);
UnityEngine.Quaternion rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref lastBulletTransform.Transform);
// Interpolation is needed in threaded mode
// Maybe we can have a call to the physics engine in an update method instead ?
if (threadHelper != null && previousBulletTransform != null)
{
double currentTime = threadHelper.TotalSimulationTime;
UnityEngine.Vector3 previousPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref previousBulletTransform.Transform);
UnityEngine.Quaternion previousRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref previousBulletTransform.Transform);
double interpolationFactor = (currentTime - lastBulletTransform.TimeStamp) / (lastBulletTransform.TimeStamp - previousBulletTransform.TimeStamp);
transform.position = UnityEngine.Vector3.LerpUnclamped(previousPosition, position, (float)interpolationFactor);
transform.rotation = UnityEngine.Quaternion.SlerpUnclamped(previousRotation, rotation, (float)interpolationFactor);
}
else
{
transform.position = position;
transform.rotation = rotation;
}
mustUpdateTransform = false;
}
pos = transform.position.ToBullet();
rot = transform.rotation.ToBullet();
}
}
public override void DrawSphere(float radius, ref Matrix trans, ref 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 Vector3 planeNormal, float planeConst, ref Matrix trans, ref 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 DrawCylinder(float radius, float halfHeight, int upAxis, ref Matrix trans, ref 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.DebugDrawCylinder(pos, rot, scale, radius, halfHeight, upAxis, c);
}
public void FixedUpdate()
{
BulletSharp.Math.Matrix trans;
m_collisionObject.GetWorldTransform(out trans);
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
}
public override void DrawBox(ref Vector3 bbMin, ref Vector3 bbMax, ref Matrix trans, ref 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();
UnityEngine.Color c = new UnityEngine.Color(color.X, color.Y, color.Z);
BUtility.DebugDrawBox(pos, rot, scale, size, c);
}
public override void DrawSphere(float radius, ref BM.Matrix trans, ref BM.Vector3 color)
{
Vector3 pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
Vector3 scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
Color c = new Color(color.X, color.Y, color.Z);
BUtility.DebugDrawSphere(pos, rot, scale, radius, 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);
}
private void Update()
{
if (m_baseCollider != null && isInWorld)
{
Matrix4x4 m = m_baseCollider.WorldTransform.ToUnity();
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
for (int i = 0; i < m_links.Count; i++)
{
MultiBodyLinkCollider linkCollider = m_links[i].GetLinkCollider();
m = linkCollider.WorldTransform.ToUnity();
m_links[i].transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
m_links[i].transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
}
}
}
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);
}
//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);
}