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(); }