//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);
}
public override void OnDrawGizmosSelected()
{
if (!drawGizmo)
{
return;
}
Gizmos.color = Color.yellow;
CollisionShape shape = GetCollisionShape();
ConvexHullShape convexShape = shape as ConvexHullShape;
Gizmos.matrix = transform.localToWorldMatrix * Matrix4x4.Scale(transform.lossyScale).inverse;
if (convexShape != null)
{
//BulletSharp.Math.Matrix childShapeTransform = this.transform;
//childShapeTransform.Invert();
//BulletSharp.Math.Matrix shapeTransform = childShapeTransform * this.transform.localToWorldMatrix.ToBullet();
int nbEdges = convexShape.NumEdges;
for (int j = 0; j < nbEdges; j++)
{
BulletSharp.Math.Vector3 vertex1;
BulletSharp.Math.Vector3 vertex2;
convexShape.GetEdge(j, out vertex1, out vertex2);
Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
Gizmos.DrawLine(vertexUnity1, vertexUnity2);
}
}
BvhTriangleMeshShape triangleShape = shape as BvhTriangleMeshShape;
if (triangleShape != null)
{
DisplayTriangleCallback cb = new DisplayTriangleCallback();
triangleShape.MeshInterface.InternalProcessAllTriangles(cb, triangleShape.LocalAabbMin, triangleShape.LocalAabbMax);
}
}
public override void OnDrawGizmosSelected()
{
if (!drawGizmo)
{
return;
}
Vector3 position = transform.position;
Quaternion rotation = transform.rotation;
Vector3 scale = m_localScaling;
Gizmos.color = Color.yellow;
BoxShape shape = GetCollisionShape() as BoxShape;
Gizmos.matrix = this.transform.localToWorldMatrix * Matrix4x4.Scale(transform.lossyScale).inverse;
for (int i = 0; i < shape.NumEdges; i++)
{
BulletSharp.Math.Vector3 vertex1;
BulletSharp.Math.Vector3 vertex2;
shape.GetEdge(i, out vertex1, out vertex2);
Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
Gizmos.DrawLine(vertexUnity1, vertexUnity2);
}
//BUtility.DebugDrawBox(position, rotation, scale, extents, Color.yellow);
}
// 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 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 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 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);
}
public override void InternalProcessTriangleIndex(ref BulletSharp.Math.Vector3 point0, ref BulletSharp.Math.Vector3 point1, ref BulletSharp.Math.Vector3 point2, int partId, int triangleIndex)
{
Vector3 point0Unity = BSExtensionMethods2.ToUnity(point0);
Vector3 point1Unity = BSExtensionMethods2.ToUnity(point1);
Vector3 point2Unity = BSExtensionMethods2.ToUnity(point2);
Gizmos.DrawLine(point0Unity, point1Unity);
Gizmos.DrawLine(point1Unity, point2Unity);
Gizmos.DrawLine(point2Unity, point0Unity);
}
//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);
}
//todo draw the hull when not in the world
public override void OnDrawGizmosSelected()
{
Gizmos.color = Color.yellow;
// Gizmos.matrix =
ConvexHullShape cs = GetCollisionShape() as ConvexHullShape;
Matrix4x4 matrix = Matrix4x4.TRS(transform.position, transform.rotation, transform.lossyScale);
Gizmos.matrix = matrix;
int nbEdges = cs.NumEdges;
for (int i = 0; i < nbEdges; i++)
{
BulletSharp.Math.Vector3 vertex1;
BulletSharp.Math.Vector3 vertex2;
cs.GetEdge(i, out vertex1, out vertex2);
Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
Gizmos.DrawLine(vertexUnity1, vertexUnity2);
}
//Gizmos.DrawWireMesh(hullMesh, transform.position, transform.rotation, transform.lossyScale);
}
//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);
}
public override void OnDrawGizmosSelected()
{
if (!drawGizmo)
{
return;
}
Gizmos.color = Color.yellow;
CompoundShape compoundShape = GetCollisionShape() as CompoundShape;
for (int i = 0; i < compoundShape.NumChildShapes; i++)
{
CollisionShape collisionShape = compoundShape.GetChildShape(i);
ConvexHullShape convexShape = collisionShape as ConvexHullShape;
if (convexShape != null)
{
BulletSharp.Math.Matrix childShapeTransform = compoundShape.GetChildTransform(i);
//childShapeTransform.Invert();
BulletSharp.Math.Matrix shapeTransform = childShapeTransform * this.transform.localToWorldMatrix.ToBullet();
Gizmos.matrix = shapeTransform.ToUnity();
int nbEdges = convexShape.NumEdges;
for (int j = 0; j < nbEdges; j++)
{
BulletSharp.Math.Vector3 vertex1;
BulletSharp.Math.Vector3 vertex2;
convexShape.GetEdge(j, out vertex1, out vertex2);
Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
Gizmos.DrawLine(vertexUnity1, vertexUnity2);
}
/*Mesh collisionMesh = new Mesh();
* Vector3[] newVertices = new Vector3[convexShape.NumVertices];
* int[] triangles = new int[convexShape.NumVertices * 3];
* for (int j = 0; j < convexShape.NumVertices; j++)
* {
* BulletSharp.Math.Vector3 vertex1;
* convexShape.GetVertex(j, out vertex1);
* newVertices[j] = vertex1.ToUnity();
* triangles[j] = j;
* }
* collisionMesh.vertices = newVertices;
* collisionMesh.triangles = triangles;
* collisionMesh.RecalculateNormals();
* Gizmos.color = Color.blue;
* Gizmos.DrawMesh(collisionMesh); */
}
BvhTriangleMeshShape triangleShape = collisionShape as BvhTriangleMeshShape;
if (triangleShape != null)
{
BulletSharp.Math.Matrix shapeTransform = this.transform.localToWorldMatrix.ToBullet() * compoundShape.GetChildTransform(i);
Gizmos.matrix = BSExtensionMethods2.ToUnity(shapeTransform);
/*int nbEdges = triangleShape.;
* for (int j = 0; j < nbEdges; j++)
* {
* BulletSharp.Math.Vector3 vertex1;
* BulletSharp.Math.Vector3 vertex2;
* triangleShape.GetEdge(j, out vertex1, out vertex2);
* Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1);
* Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2);
* Gizmos.DrawLine(vertexUnity1, vertexUnity2);
* }*/
}
}
}
//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);
}
