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