public Vector3 GetPosition() { Matrix4x4 mat = Matrix4x4.identity; NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat); return(new Vector3(mat.m03, mat.m13, mat.m23)); }
static void NewtonBodyIterator(IntPtr pBody, IntPtr userData) { Matrix4x4 mat = Matrix4x4.identity; NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat); IntPtr pColl = NewtonAPI.NewtonBodyGetCollision(pBody); NewtonAPI.NewtonCollisionForEachPolygonDo(pColl, ref mat, NewtonCollisionIterator, IntPtr.Zero); }
public Quaternion GetRotation() { Matrix4x4 mat = Matrix4x4.identity; NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat); Quaternion q = new Quaternion(); q.w = Mathf.Sqrt(Mathf.Max(0, 1 + mat[0, 0] + mat[1, 1] + mat[2, 2])) / 2; q.x = Mathf.Sqrt(Mathf.Max(0, 1 + mat[0, 0] - mat[1, 1] - mat[2, 2])) / 2; q.y = Mathf.Sqrt(Mathf.Max(0, 1 - mat[0, 0] + mat[1, 1] - mat[2, 2])) / 2; q.z = Mathf.Sqrt(Mathf.Max(0, 1 - mat[0, 0] - mat[1, 1] + mat[2, 2])) / 2; q.x *= Mathf.Sign(q.x * (mat[2, 1] - mat[1, 2])); q.y *= Mathf.Sign(q.y * (mat[0, 2] - mat[2, 0])); q.z *= Mathf.Sign(q.z * (mat[1, 0] - mat[0, 1])); return(q); }
// Update is called once per frame void Update() { //TODO: Use Newton transform callback instead, not need to fetch the transform if the body hasn't moved. Matrix4x4 mat = Matrix4x4.identity; NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat); Quaternion q = new Quaternion(); q.w = Mathf.Sqrt(Mathf.Max(0, 1 + mat[0, 0] + mat[1, 1] + mat[2, 2])) / 2; q.x = Mathf.Sqrt(Mathf.Max(0, 1 + mat[0, 0] - mat[1, 1] - mat[2, 2])) / 2; q.y = Mathf.Sqrt(Mathf.Max(0, 1 - mat[0, 0] + mat[1, 1] - mat[2, 2])) / 2; q.z = Mathf.Sqrt(Mathf.Max(0, 1 - mat[0, 0] - mat[1, 1] + mat[2, 2])) / 2; q.x *= Mathf.Sign(q.x * (mat[2, 1] - mat[1, 2])); q.y *= Mathf.Sign(q.y * (mat[0, 2] - mat[2, 0])); q.z *= Mathf.Sign(q.z * (mat[1, 0] - mat[0, 1])); transform.position = new Vector3(mat.m03, mat.m13, mat.m23); transform.rotation = q; }
void RenderDebugLines() { Matrix4x4 mat = Matrix4x4.identity; IntPtr pBody = NewtonAPI.NewtonWorldGetFirstBody(pWorld); while (pBody != IntPtr.Zero) { NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat); IntPtr pColl = NewtonAPI.NewtonBodyGetCollision(pBody); NewtonAPI.NewtonCollisionForEachPolygonDo(pColl, ref mat, NewtonCollisionIterator, IntPtr.Zero); pBody = NewtonAPI.NewtonWorldGetNextBody(pWorld, pBody); } //Debug.Log("BodyCount:" + bodyCount.ToString()); //Vector3 p0 = new Vector3(-100, -100, -100); //Vector3 p1 = new Vector3(100, 100, 100); //WorldForEachBodyInAABBDo(ref p0, ref p1, NewtonBodyIterator, IntPtr.Zero); }