public static void SetTransformationFromBulletMatrix(this UnityEngine.Transform transform, BulletSharp.Math.Matrix bm)
{
UnityEngine.Matrix4x4 matrix = bm.ToUnity(); //creates new Unity Matrix4x4
transform.localPosition = ExtractTranslationFromMatrix(ref matrix);
transform.localRotation = ExtractRotationFromMatrix(ref matrix);
transform.localScale = ExtractScaleFromMatrix(ref matrix);
}
public static void SetTransformationFromBulletMatrix(this Transform transform, MatrixB bm)
{
Matrix4x4 matrix = bm.ToUnity(); //creates new Unity Matrix4x4
transform.localPosition = ExtractTranslationFromMatrix(ref matrix);
transform.localRotation = ExtractRotationFromMatrix(ref matrix);
transform.localScale = ExtractScaleFromMatrix(ref matrix);
}
public override void OnDrawGizmosSelected()
{
if (!drawGizmo)
{
return;
}
Gizmos.color = Color.yellow;
CompoundShape compoundShape = GetCollisionShape() as CompoundShape;
Matrix4x4 parentMatrix = this.transform.localToWorldMatrix * Matrix4x4.Scale(transform.lossyScale).inverse;
for (int i = 0; i < compoundShape.NumChildShapes; i++)
{
CollisionShape collisionShape = compoundShape.GetChildShape(i);
BulletSharp.Math.Matrix childShapeTransform = compoundShape.GetChildTransform(i);
//childShapeTransform.Invert();
Gizmos.matrix = parentMatrix * childShapeTransform.ToUnity();
ConvexHullShape convexShape = collisionShape as ConvexHullShape;
if (convexShape != null)
{
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 = collisionShape as BvhTriangleMeshShape;
if (triangleShape != null)
{
DisplayTriangleCallback cb = new DisplayTriangleCallback();
triangleShape.MeshInterface.InternalProcessAllTriangles(cb, triangleShape.LocalAabbMin, triangleShape.LocalAabbMax);
}
}
}
public void PostOnInitializePhysics()
{
for (int i = 0; i < demo.World.CollisionObjectArray.Count; i++)
{
CollisionObject co = demo.World.CollisionObjectArray[i];
CollisionShape cs = co.CollisionShape;
GameObject go;
if (cs.ShapeType == BroadphaseNativeType.SoftBodyShape)
{
BulletSharp.SoftBody.SoftBody sb = (BulletSharp.SoftBody.SoftBody)co;
if (sb.Faces.Count == 0)
{
//rope
go = CreateUnitySoftBodyRope(sb);
}
else
{
go = CreateUnitySoftBodyCloth(sb);
}
}
else
{
//rigid body
if (cs.ShapeType == BroadphaseNativeType.CompoundShape)
{
BulletSharp.Math.Matrix transform = co.WorldTransform;
go = new GameObject("Compund Shape");
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co as RigidBody;
foreach (BulletSharp.CompoundShapeChild child in (cs as CompoundShape).ChildList)
{
BulletSharp.Math.Matrix childTransform = child.Transform;
GameObject ggo = new GameObject(child.ToString());
MeshFilter mf = ggo.AddComponent <MeshFilter>();
Mesh m = mf.mesh;
MeshFactory2.CreateShape(child.ChildShape, m);
MeshRenderer mr = ggo.AddComponent <MeshRenderer>();
mr.sharedMaterial = mat;
ggo.transform.SetParent(go.transform);
Matrix4x4 mt = childTransform.ToUnity();
ggo.transform.localPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref mt);
ggo.transform.localRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref mt);
ggo.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref mt);
/*
* BulletRigidBodyProxy rbp = ggo.AddComponent<BulletRigidBodyProxy>();
* rbp.target = body;
* return go;
*/
//InitRigidBodyInstance(colObj, child.ChildShape, ref childTransform);
}
}
else if (cs.ShapeType == BroadphaseNativeType.CapsuleShape)
{
GameObject ggo = GameObject.CreatePrimitive(PrimitiveType.Capsule);
Destroy(ggo.GetComponent <Collider>());
go = new GameObject();
ggo.transform.parent = go.transform;
ggo.transform.localPosition = Vector3.zero;
ggo.transform.localRotation = Quaternion.identity;
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co as RigidBody;
}
else
{
Debug.Log("Creating " + cs.ShapeType + " for " + co.ToString());
go = CreateUnityCollisionObjectProxy(co as CollisionObject);
}
}
createdObjs.Add(go);
Debug.Log("Created Unity Shape for " + co);
}
}
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);
* }*/
}
}
}
/// <summary>
/// Saves the robot's current orientation to be used whenever robot is reset
/// </summary>
public void SaveRobotOrientation()
{
robotStartOrientation = ((RigidNode)rootNode.ListAllNodes()[0]).MainObject.GetComponent <BRigidBody>().GetCollisionObject().WorldTransform.Basis;
robotStartOrientation.ToUnity();
EndReset();
}