public static BulletSharp.Math.Matrix Convert(ref Matrix4 m)
{
BulletSharp.Math.Matrix r = new BulletSharp.Math.Matrix();
r.M11 = m.M11; r.M12 = m.M12; r.M13 = m.M13; r.M14 = m.M14;
r.M21 = m.M21; r.M22 = m.M22; r.M23 = m.M23; r.M24 = m.M24;
r.M31 = m.M31; r.M32 = m.M32; r.M33 = m.M33; r.M34 = m.M34;
r.M41 = m.M41; r.M42 = m.M42; r.M43 = m.M43; r.M44 = m.M44;
return r;
}
public static void SetOrientation(this MatrixB bm, QuaternionB q)
{
float xx = q.X * q.X;
float yy = q.Y * q.Y;
float zz = q.Z * q.Z;
float xy = q.X * q.Y;
float zw = q.Z * q.W;
float zx = q.Z * q.X;
float yw = q.Y * q.W;
float yz = q.Y * q.Z;
float xw = q.X * q.W;
bm.M11 = 1.0f - (2.0f * (yy + zz));
bm.M12 = 2.0f * (xy + zw);
bm.M13 = 2.0f * (zx - yw);
bm.M21 = 2.0f * (xy - zw);
bm.M22 = 1.0f - (2.0f * (zz + xx));
bm.M23 = 2.0f * (yz + xw);
bm.M31 = 2.0f * (zx + yw);
bm.M32 = 2.0f * (yz - xw);
bm.M33 = 1.0f - (2.0f * (yy + xx));
}
public static void ToBullet(this Matrix4x4 um, ref MatrixB bm)
{
bm[0, 0] = um[0, 0];
bm[0, 1] = um[1, 0];
bm[0, 2] = um[2, 0];
bm[0, 3] = um[3, 0];
bm[1, 0] = um[0, 1];
bm[1, 1] = um[1, 1];
bm[1, 2] = um[2, 1];
bm[1, 3] = um[3, 1];
bm[2, 0] = um[0, 2];
bm[2, 1] = um[1, 2];
bm[2, 2] = um[2, 2];
bm[2, 3] = um[3, 2];
bm[3, 0] = um[0, 3];
bm[3, 1] = um[1, 3];
bm[3, 2] = um[2, 3];
bm[3, 3] = um[3, 3];
}
public static Matrix4 ToOpenTk(this BulletSharp.Math.Matrix m)
{
return(new Matrix4
{
M11 = m.M11,
M12 = m.M12,
M13 = m.M13,
M14 = m.M14,
M21 = m.M21,
M22 = m.M22,
M23 = m.M23,
M24 = m.M24,
M31 = m.M31,
M32 = m.M32,
M33 = m.M33,
M34 = m.M34,
M41 = m.M41,
M42 = m.M42,
M43 = m.M43,
M44 = m.M44
});
}
/// <summary>
/// Shifts the robot by a set position vector
/// </summary>
public void TransposeRobot(Vector3 transposition)
{
foreach (RigidNode n in rootNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin += transposition.ToBullet();
r.WorldTransform = newTransform;
}
int isMixAndMatch = PlayerPrefs.GetInt("mixAndMatch"); // 0 is false, 1 is true
if (robotHasManipulator == 1 && isMixAndMatch == 1)
{
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin += transposition.ToBullet();
r.WorldTransform = newTransform;
}
}
}
/// <summary>
/// Called when the robot is moved.
/// </summary>
/// <param name="transposition"></param>
protected override void OnTransposeRobot(Vector3 transposition)
{
if (!RobotHasManipulator)
{
return;
}
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin += transposition.ToBullet();
r.WorldTransform = newTransform;
}
}
/// <summary>
/// Can move robot around in this state using WASD, update robotStartPosition if hit enter
/// </summary>
void Resetting()
{
if (Input.GetMouseButton(1))
{
//Transform rotation along the horizontal plane
Vector3 rotation = new Vector3(0f,
Input.GetKey(KeyCode.D) ? ResetVelocity : Input.GetKey(KeyCode.A) ? -ResetVelocity : 0f,
0f);
if (!rotation.Equals(Vector3.zero))
{
RotateRobot(rotation);
}
}
else
{
//Transform position
Vector3 transposition = new Vector3(
Input.GetKey(KeyCode.D) ? ResetVelocity : Input.GetKey(KeyCode.A) ? -ResetVelocity : 0f,
0f,
Input.GetKey(KeyCode.W) ? ResetVelocity : Input.GetKey(KeyCode.S) ? -ResetVelocity : 0f);
if (!transposition.Equals(Vector3.zero))
{
TransposeRobot(transposition);
}
}
//Update robotStartPosition when hit enter
if (Input.GetKey(KeyCode.Return))
{
robotStartOrientation = ((RigidNode)rootNode.ListAllNodes()[0]).MainObject.GetComponent <BRigidBody>().GetCollisionObject().WorldTransform.Basis;
robotStartPosition = new Vector3(transform.GetChild(0).transform.localPosition.x - nodeToRobotOffset.x, robotStartPosition.y,
transform.GetChild(0).transform.localPosition.z - nodeToRobotOffset.z);
EndReset();
}
}
/// <summary>
/// Called when the robot begins to reset.
/// </summary>
protected override void OnBeginReset()
{
if (!RobotHasManipulator)
{
return;
}
int i = 0;
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
if (i == 0)
{
Debug.Log("Transform Origin" + newTransform.Origin);
}
i++;
}
}
public static Matrix4x4 ToUnity(this MatrixB bm, ref Matrix4x4 um)
{
um[0, 0] = bm[0, 0];
um[0, 1] = bm[1, 0];
um[0, 2] = bm[2, 0];
um[0, 3] = bm[3, 0];
um[1, 0] = bm[0, 1];
um[1, 1] = bm[1, 1];
um[1, 2] = bm[2, 1];
um[1, 3] = bm[3, 1];
um[2, 0] = bm[0, 2];
um[2, 1] = bm[1, 2];
um[2, 2] = bm[2, 2];
um[2, 3] = bm[3, 2];
um[3, 0] = bm[0, 3];
um[3, 1] = bm[1, 3];
um[3, 2] = bm[2, 3];
um[3, 3] = bm[3, 3];
return(um);
}
public void TransformTest()
{
BulletSharp.Math.Matrix myMatrix = new BulletSharp.Math.Matrix();
//myMatrix.
}
/// <summary>
/// Rotates the active robot about its origin by a mathematical 4x4 matrix
/// </summary>
public void RotateRobot(BulletSharp.Math.Matrix rotationMatrix)
{
ActiveRobot.RotateRobot(rotationMatrix);
}
/// <summary>
/// Convert from bullet matrix to serializable math matrix
/// </summary>
public static GameSystem.GameCore.SerializableMath.Matrix4x4 ToSerializable(this BulletSharp.Math.Matrix m)
{
return(new GameSystem.GameCore.SerializableMath.Matrix4x4(m.ToArray()));
}
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 SetWorldTransform(ref BulletSharp.Math.Matrix transform)
{
Matrix asStrideMatrix = transform;
rigidBody.RigidBodySetWorldTransform(ref asStrideMatrix);
}
/// <summary>
/// Rotates the robot about its origin by a mathematical 4x4 matrix
/// </summary>
public void RotateRobot(BulletSharp.Math.Matrix rotationMatrix)
{
BulletSharp.Math.Vector3?origin = null;
foreach (RigidNode n in rootNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
if (origin == null)
{
origin = r.CenterOfMassPosition;
}
BulletSharp.Math.Matrix rotationTransform = new BulletSharp.Math.Matrix();
rotationTransform.Basis = rotationMatrix;
rotationTransform.Origin = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix currentTransform = r.WorldTransform;
BulletSharp.Math.Vector3 pos = currentTransform.Origin;
currentTransform.Origin -= origin.Value;
currentTransform *= rotationTransform;
currentTransform.Origin += origin.Value;
r.WorldTransform = currentTransform;
}
if (RobotHasManipulator)
{
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
if (origin == null)
{
origin = r.CenterOfMassPosition;
}
BulletSharp.Math.Matrix rotationTransform = new BulletSharp.Math.Matrix();
rotationTransform.Basis = rotationMatrix;
rotationTransform.Origin = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix currentTransform = r.WorldTransform;
BulletSharp.Math.Vector3 pos = currentTransform.Origin;
currentTransform.Origin -= origin.Value;
currentTransform *= rotationTransform;
currentTransform.Origin += origin.Value;
r.WorldTransform = currentTransform;
}
}
}
public static BulletSharp.Math.Matrix ToBullet(this Matrix4x4 um)
{
BulletSharp.Math.Matrix bm = new BulletSharp.Math.Matrix();
um.ToBullet(ref bm);
return(bm);
}
public CapsuleBody(float radius, float halfHeight, int upAxis, ref Matrix transform,
ref BulletSharp.Math.Vector3 color) : base(radius, halfHeight, upAxis, ref transform, ref color)
{
}
/// <summary>
/// Resets the robot orientation to how the CAD model was originally defined (should be standing upright and facing forward if CAD was done properly)
/// </summary>
public void ResetRobotOrientation()
{
robotStartOrientation = BulletSharp.Math.Matrix.Identity;
BeginReset();
EndReset();
}
public override void GetWorldTransform(out BulletMatrix worldTrans)
{
worldTrans = (BulletMatrix)_part.CFrame;
}
CompoundShape _CreateCompoundShape(bool copyChildren)
{
BCollisionShape[] css = GetComponentsInChildren <BCollisionShape>();
colliders = new BCollisionShape[css.Length - 1];
int ii = 0;
for (int i = 0; i < css.Length; i++)
{
if (css[i] == this)
{
//skip
}
else
{
colliders[ii] = css[i];
ii++;
}
}
if (colliders.Length == 0)
{
Debug.LogError("Compound collider");
}
//TODO
// some of the collider types (non-finite and other compound colliders) are probably not
// can only be added to game object with rigid body attached.
// allowed should check for these.
// what about scaling not sure if it is handled correctly
CompoundShape cs = new CompoundShape();
for (int i = 0; i < colliders.Length; i++)
{
CollisionShape chcs;
if (copyChildren == true)
{
chcs = colliders[i].CopyCollisionShape();
}
else
{
chcs = colliders[i].GetCollisionShape();
}
Vector3 up = Vector3.up;
Vector3 origin = Vector3.zero;
Vector3 forward = Vector3.forward;
//to world
up = colliders[i].transform.TransformDirection(up);
origin = colliders[i].transform.TransformPoint(origin);
forward = colliders[i].transform.TransformDirection(forward);
//to compound collider
up = transform.InverseTransformDirection(up);
origin = transform.InverseTransformPoint(origin);
forward = transform.InverseTransformDirection(forward);
Quaternion q = Quaternion.LookRotation(forward, up);
/*
* Some collision shapes can have local scaling applied. Use
* btCollisionShape::setScaling(vector3).Non uniform scaling with different scaling
* values for each axis, can be used for btBoxShape, btMultiSphereShape,
* btConvexShape, btTriangleMeshShape.Note that a non - uniform scaled
* sphere can be created by using a btMultiSphereShape with 1 sphere.
*/
BulletSharp.Math.Matrix m = BulletSharp.Math.Matrix.AffineTransformation(1f, q.ToBullet(), origin.ToBullet());
cs.AddChildShape(m, chcs);
}
cs.LocalScaling = m_localScaling.ToBullet();
return(cs);
}
//called by Physics World just before rigid body is added to world.
//the current rigid body properties are used to rebuild the rigid body.
internal override bool _BuildCollisionObject()
{
if (td == null)
{
Debug.LogError("Must be attached to an object with a terrain ");
return(false);
}
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveCollisionObject(m_collisionObject);
}
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogError("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent <BCollisionShape>();
if (m_collisionShape == null)
{
Debug.LogError("There was no collision shape component attached to this BRigidBody. " + name);
return(false);
}
if (!(m_collisionShape is BHeightfieldTerrainShape))
{
Debug.LogError("The collision shape needs to be a BHeightfieldTerrainShape. " + name);
return(false);
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
if (m_collisionObject == null)
{
m_collisionObject = new CollisionObject();
m_collisionObject.CollisionShape = cs;
m_collisionObject.UserObject = this;
BulletSharp.Math.Matrix worldTrans = BulletSharp.Math.Matrix.Identity;
Vector3 pos = transform.position + new Vector3(td.size.x * .5f, td.size.y * .5f, td.size.z * .5f);
worldTrans.Origin = pos.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionFlags = m_collisionFlags;
}
else
{
m_collisionObject.CollisionShape = cs;
BulletSharp.Math.Matrix worldTrans = BulletSharp.Math.Matrix.Identity;
Vector3 pos = transform.position + new Vector3(td.size.x * .5f, td.size.y * .5f, td.size.z * .5f);
worldTrans.Origin = pos.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.CollisionFlags = m_collisionFlags;
}
return(true);
}
public static unsafe System.Numerics.Matrix4x4 ToNumerics(this BulletSharp.Math.Matrix v) => *(System.Numerics.Matrix4x4 *) & v;
void InitRigidBodyInstance(CollisionObject colObj, CollisionShape shape, ref Matrix transform)
{
if (shape.ShapeType == BroadphaseNativeType.CompoundShape)
{
foreach (var child in (shape as CompoundShape).ChildList)
{
Matrix childTransform = child.Transform * transform;
InitRigidBodyInstance(colObj, child.ChildShape, ref childTransform);
}
}
else
{
var shapeData = InitShapeData(shape);
shapeData.Instances.Add(new InstanceData()
{
WorldTransform = transform,
Color = "Ground".Equals(colObj.UserObject) ? groundColor :
colObj.ActivationState == ActivationState.ActiveTag ? activeColor : passiveColor
});
}
}
void InitInstanceData(CollisionObject colObj, CollisionShape shape, ref Matrix transform)
{
if (shape.ShapeType == BroadphaseNativeType.CompoundShape)
{
foreach (CompoundShapeChild child in (shape as CompoundShape).ChildList)
{
Matrix childTransform = child.Transform * transform;
InitInstanceData(colObj, child.ChildShape, ref childTransform);
}
}
else if (shape.ShapeType == BroadphaseNativeType.SoftBodyShape)
{
ShapeData shapeData = InitShapeData(shape);
UpdateSoftBody(colObj as SoftBody, shapeData);
shapeData.InstanceDataList.Add(new InstanceData()
{
WorldTransform = transform,
Color = softBodyColor
});
}
else
{
InitShapeData(shape).InstanceDataList.Add(new InstanceData()
{
WorldTransform = transform,
Color = "Ground".Equals(colObj.UserObject) ? groundColor :
colObj.ActivationState == ActivationState.ActiveTag ? activeColor : passiveColor
});
}
}
public void AddChildShape(CollisionShape shape, BulletSharp.Math.Matrix offset)
{
collisionShape.AddChildShape(offset, shape);
}
public override void SetWorldTransform(ref BulletSharp.Math.Matrix worldTrans)
{
Mesh.World = (Matrix.Translation(-Mesh.Mesh.Extent.Center).ToBulletMatrix() * worldTrans).ToSharpDXMatrix();
}
public override void GetWorldTransform(out BulletSharp.Math.Matrix transform)
{
rigidBody.RigidBodyGetWorldTransform(out var strideMatrix);
transform = strideMatrix;
}
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();
}
public static BulletSharp.Math.Matrix ToBullet(this UnityEngine.Matrix4x4 um)
{
BulletSharp.Math.Matrix bm = new BulletSharp.Math.Matrix();
um.ToBullet(ref bm);
return bm;
}
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);
}
/// <summary>
/// Return the robot to robotStartPosition and destroy extra game pieces
/// </summary>
/// <param name="resetTransform"></param>
public void BeginReset()
{
//GetDriverPractice().DestroyAllGamepieces();
InputControl.freeze = true;
if (canvas == null)
{
canvas = GameObject.Find("Main Camera").transform.GetChild(0).gameObject;
}
if (resetCanvas == null)
{
resetCanvas = GameObject.Find("Main Camera").transform.GetChild(1).gameObject;
}
canvas.GetComponent <Canvas>().enabled = false;
resetCanvas.SetActive(true);
#region init
if (helpMenu == null)
{
helpMenu = Auxiliary.FindObject(resetCanvas, "Help");
}
if (toolbar == null)
{
toolbar = Auxiliary.FindObject(resetCanvas, "ResetStateToolbar");
}
if (overlay == null)
{
overlay = Auxiliary.FindObject(resetCanvas, "Overlay");
}
if (helpBodyText == null)
{
helpBodyText = Auxiliary.FindObject(resetCanvas, "BodyText").GetComponent <Text>();
}
#endregion
Button resetButton = Auxiliary.FindObject(resetCanvas, "ResetButton").GetComponent <Button>();
resetButton.onClick.RemoveAllListeners();
resetButton.onClick.AddListener(BeginRevertSpawnpoint);
Button helpButton = Auxiliary.FindObject(resetCanvas, "HelpButton").GetComponent <Button>();
helpButton.onClick.RemoveAllListeners();
helpButton.onClick.AddListener(HelpMenu);
Button returnButton = Auxiliary.FindObject(resetCanvas, "ReturnButton").GetComponent <Button>();
returnButton.onClick.RemoveAllListeners();
returnButton.onClick.AddListener(EndReset);
Button closeHelp = Auxiliary.FindObject(helpMenu, "CloseHelpButton").GetComponent <Button>();
closeHelp.onClick.RemoveAllListeners();
closeHelp.onClick.AddListener(CloseHelpMenu);
DynamicCamera dynamicCamera = UnityEngine.Camera.main.transform.GetComponent <DynamicCamera>();
lastCameraState = dynamicCamera.ActiveState;
dynamicCamera.SwitchCameraState(new DynamicCamera.OrbitState(dynamicCamera));
foreach (SimulatorRobot robot in state.SpawnedRobots)
{
foreach (BRigidBody rb in robot.GetComponentsInChildren <BRigidBody>())
{
if (rb != null && !rb.GetCollisionObject().IsActive)
{
rb.GetCollisionObject().Activate();
}
}
}
if (!state.DynamicCameraObject.GetComponent <DynamicCamera>().ActiveState.GetType().Equals(typeof(DynamicCamera.ConfigurationState)))
{
IsResetting = true;
foreach (RigidNode n in RootNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
}
OnBeginReset();
//Where "save orientation" works
RotateRobot(robotStartOrientation);
AttachMoveArrows();
}
else
{
UserMessageManager.Dispatch("Please don't reset robot during configuration!", 5f);
}
}
public static BulletSharp.Math.Quaternion GetOrientation(this BulletSharp.Math.Matrix bm)
{
/*
* float trace = M11 + M22 + M33;
*
* float[] temp = new float[4];
*
* if (trace > 0.0f)
* {
* float s = Mathf.Sqrt(trace + (1.0f));
* temp[3] = (s * (0.5f));
* s = (0.5f) / s;
*
* temp[0] = ((M32 - M23) * s);
* temp[1] = ((M13 - M31) * s);
* temp[2] = ((M21 - M12) * s);
*
* temp[0] = ((M23 - M32) * s);
* temp[1] = ((M31 - M13) * s);
* temp[2] = ((M12 - M21) * s);
* }
* else
* {
* int i = M11 < M22 ?
* (M22 < M33 ? 2 : 1) :
* (M11 < M33 ? 2 : 0);
* int j = (i + 1) % 3;
* int k = (i + 2) % 3;
*
* float s = Mathf.Sqrt(this[i,i] - this[j,j] - this[k,k] + 1.0f);
* temp[i] = s * 0.5f;
* s = 0.5f / s;
*
* temp[3] = (this[j,k] - this[k,j]) * s;
* temp[j] = (this[i,j] + this[j,i]) * s;
* temp[k] = (this[i,k] + this[k,i]) * s;
* }
* return new BulletSharp.Math.Quaternion(temp[0], temp[1], temp[2], temp[3]);
*/
//Scaling is the length of the rows.
BulletSharp.Math.Vector3 scale;
scale.X = (float)System.Math.Sqrt((bm.M11 * bm.M11) + (bm.M12 * bm.M12) + (bm.M13 * bm.M13));
scale.Y = (float)System.Math.Sqrt((bm.M21 * bm.M21) + (bm.M22 * bm.M22) + (bm.M23 * bm.M23));
scale.Z = (float)System.Math.Sqrt((bm.M31 * bm.M31) + (bm.M32 * bm.M32) + (bm.M33 * bm.M33));
//The rotation is the left over matrix after dividing out the scaling.
float mm11 = bm.M11 / scale.X;
float mm12 = bm.M12 / scale.X;
float mm13 = bm.M13 / scale.X;
float mm21 = bm.M21 / scale.Y;
float mm22 = bm.M22 / scale.Y;
float mm23 = bm.M23 / scale.Y;
float mm31 = bm.M31 / scale.Z;
float mm32 = bm.M32 / scale.Z;
float mm33 = bm.M33 / scale.Z;
//------------------------
float sqrt;
float half;
float trace = mm11 + mm22 + mm33;
BulletSharp.Math.Quaternion result = new BulletSharp.Math.Quaternion();
if (trace > 0.0f)
{
sqrt = (float)Mathf.Sqrt(trace + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (mm23 - mm32) * sqrt;
result.Y = (mm31 - mm13) * sqrt;
result.Z = (mm12 - mm21) * sqrt;
}
else if ((mm11 >= mm22) && (mm11 >= mm33))
{
sqrt = (float)Mathf.Sqrt(1.0f + mm11 - mm22 - mm33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (mm12 + mm21) * half;
result.Z = (mm13 + mm31) * half;
result.W = (mm23 - mm32) * half;
}
else if (mm22 > mm33)
{
sqrt = (float)Mathf.Sqrt(1.0f + mm22 - mm11 - mm33);
half = 0.5f / sqrt;
result.X = (mm21 + mm12) * half;
result.Y = 0.5f * sqrt;
result.Z = (mm32 + mm23) * half;
result.W = (mm31 - mm13) * half;
}
else
{
sqrt = (float)Mathf.Sqrt(1.0f + mm33 - mm11 - mm22);
half = 0.5f / sqrt;
result.X = (mm31 + mm13) * half;
result.Y = (mm32 + mm23) * half;
result.Z = 0.5f * sqrt;
result.W = (mm12 - mm21) * half;
}
//------------------------
return(result);
}
public override void GetWorldTransform(out BulletSharp.Math.Matrix worldTrans)
{
worldTrans = (Mesh.World * Matrix.Translation(Mesh.Mesh.Extent.Center)).ToBulletMatrix();
}
