public Vector3 GetPosition()
{
Matrix4x4 mat = Matrix4x4.identity;
NewtonAPI.NewtonBodyGetMatrix(pBody, ref mat);
return(new Vector3(mat.m03, mat.m13, mat.m23));
}
void Start()
{
Debug.Log("Creating ballandsocket");
childBody = GetComponentInParent <NewtonBody>();
Matrix4x4 matrix = Matrix4x4.identity;
matrix.SetTRS(transform.position, transform.rotation, Vector3.one);
Debug.Log("ChildBodyPtr:" + childBody.pBody.ToString());
IntPtr parentBodyPtr = IntPtr.Zero;
if (ConnectedBody != null)
{
parentBodyPtr = ConnectedBody.pBody;
Debug.Log("ParentBodyPtr:" + parentBodyPtr.ToString());
}
if (useFriction)
{
joint = NewtonAPI.NewtonCreateBallAndSocketWithFriction(ref matrix, childBody.pBody, parentBodyPtr, friction);
}
else
{
joint = NewtonAPI.NewtonCreateBallAndSocket(ref matrix, childBody.pBody, parentBodyPtr);
}
Debug.Log("Created Joint(" + joint.ToString() + ")");
}
void Start()
{
Debug.Log("Creating rolling friction");
childBody = GetComponentInParent <NewtonBody>();
joint = NewtonAPI.NewtonCreateRollingFriction(childBody.pBody, Radius, Friction);
}
public override IntPtr CreateCollider(bool applyOffset)
{
Matrix4x4 offsetMatrix = Matrix4x4.identity;
IntPtr collider = NewtonAPI.NewtonCreateSphere(NewtonWorld.Instance.pWorld, Radius, 0, ref offsetMatrix);
NewtonAPI.NewtonCollisionSetScale(collider, Scale.x, Scale.y, Scale.z);
return(collider);
}
void FixedUpdate()
{
NewtonAPI.NewtonUpdate(pWorld, Time.deltaTime);
if (DebugRender)
{
RenderDebugLines();
}
}
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);
}
static void ApplyForceAndTorque(IntPtr body, float timestep, int threadIndex)
{
float mass = 0, iXX = 0, iYY = 0, iZZ = 0;
NewtonAPI.NewtonBodyGetMassMatrix(body, ref mass, ref iXX, ref iYY, ref iZZ);
Vector3 force = new Vector3(0.0f, -9.8f * mass, 0.0f);
NewtonAPI.NewtonBodyAddForce(body, ref force);
}
public override IntPtr CreateCollider(bool applyOffset)
{
Matrix4x4 offsetMatrix = Matrix4x4.identity;
Quaternion rotation = Quaternion.Euler(0.0f, 0.0f, 90.0f);
offsetMatrix.SetTRS(Vector3.zero, rotation, Vector3.one);
IntPtr collider = NewtonAPI.NewtonCreateCone(NewtonWorld.Instance.pWorld, Radius, Height, 0, ref offsetMatrix);
return(collider);
}
public override IntPtr CreateCollider(bool applyOffset)
{
Matrix4x4 offsetMatrix = Matrix4x4.identity;
if (applyOffset)
{
offsetMatrix.SetTRS(transform.localPosition, transform.localRotation, Vector3.one);
}
IntPtr collider = NewtonAPI.NewtonCreateBox(NewtonWorld.Instance.pWorld, Size.x, Size.y, Size.z, 0, ref offsetMatrix);
NewtonAPI.NewtonCollisionSetScale(collider, Scale.x, Scale.y, Scale.z);
return(collider);
}
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);
}
void OnDestroy()
{
//Debug.Log("Destroying body");
//gcHandle_instance.Free();
NewtonWorld world = NewtonWorld.Instance;
// No need to destroy the body if the application is shutting down, Newton will have destroyed all remaining bodies.
if (world != null)
{
//Debug.Log("Destroyed body(" + pBody.ToString() + ")");
NewtonAPI.NewtonDestroyBody(pBody);
}
//else
// Debug.Log("World already destroyed");
}
// 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;
}
public override IntPtr CreateCollider(bool applyOffset)
{
Matrix4x4 offsetMatrix = Matrix4x4.identity;
Quaternion rotation = Quaternion.Euler(0.0f, 0.0f, 90.0f);
offsetMatrix.SetTRS(Vector3.zero, rotation, Vector3.one);
IntPtr collider = IntPtr.Zero;
if (Chamfered)
{
collider = NewtonAPI.NewtonCreateChamferCylinder(NewtonWorld.Instance.pWorld, Radius0, Height, 0, ref offsetMatrix);
}
else
{
collider = NewtonAPI.NewtonCreateCylinder(NewtonWorld.Instance.pWorld, Radius0, Radius1, Height, 0, ref offsetMatrix);
}
NewtonAPI.NewtonCollisionSetScale(collider, Scale.x, Scale.y, Scale.z);
return(collider);
}
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);
}
void Start()
{
Debug.Log("Creating hinge");
childBody = GetComponentInParent <NewtonBody>();
Matrix4x4 matrix = Matrix4x4.identity;
matrix.SetTRS(transform.position, transform.rotation, Vector3.one);
IntPtr parentBodyPtr = IntPtr.Zero;
if (ConnectedBody != null)
{
parentBodyPtr = ConnectedBody.pBody;
}
joint = NewtonAPI.NewtonCreateHinge(ref matrix, childBody.pBody, parentBodyPtr);
NewtonAPI.NewtonHingeEnableLimits(joint, EnableLimits);
NewtonAPI.NewtonHingeSetLimits(joint, MinAngle * Mathf.Deg2Rad, MaxAngle * Mathf.Deg2Rad);
NewtonAPI.NewtonHingeSetFriction(joint, Friction);
}
public override IntPtr CreateCollider(bool applyOffset)
{
if (mesh == null)
{
return(IntPtr.Zero);
}
Matrix4x4 offsetMatrix = Matrix4x4.identity;
if (applyOffset)
{
offsetMatrix.SetTRS(transform.localPosition, transform.localRotation, Vector3.one);
}
Vector3[] vertices = mesh.vertices;
IntPtr collider = NewtonAPI.NewtonCreateConvexHull(NewtonWorld.Instance.pWorld, vertices.Length, vertices, 12, tolerance, 0, ref offsetMatrix);
NewtonAPI.NewtonCollisionSetScale(collider, Scale.x, Scale.y, Scale.z);
return(collider);
}
public override IntPtr CreateCollider(bool applyOffset)
{
if (mesh == null)
{
return(IntPtr.Zero);
}
Vector3[] vertices = mesh.vertices;
int[] triangles = mesh.triangles;
int numTris = triangles.Length / 3;
IntPtr collider = NewtonAPI.NewtonCreateTreeCollision(NewtonWorld.Instance.pWorld, 0);
NewtonAPI.NewtonTreeCollisionBeginBuild(collider);
Vector3[] triVertices = new Vector3[3];
for (int i = 0; i < numTris; i++)
{
triVertices[0] = vertices[triangles[i * 3 + 0]];
triVertices[1] = vertices[triangles[i * 3 + 1]];
triVertices[2] = vertices[triangles[i * 3 + 2]];
triVertices[0].Scale(transform.localScale);
triVertices[1].Scale(transform.localScale);
triVertices[2].Scale(transform.localScale);
NewtonAPI.NewtonTreeCollisionAddFace(collider, 3, triVertices, 12, 0);
}
int opt = (Optimize) ? 1 : 0;
NewtonAPI.NewtonTreeCollisionEndBuild(collider, opt);
return(collider);
}
void Start()
{
IntPtr pWorld = NewtonWorld.Instance.pWorld;
IntPtr pColl = IntPtr.Zero;
//NewtonCollider[] colliders = GetComponentsInChildren<NewtonCollider>();
NewtonCollider[] colliders = GetAllColliders();
if (colliders.Length == 0) // No collider found, create null collision
{
pColl = NewtonAPI.NewtonCreateNull(pWorld);
}
else if (colliders.Length == 1) // One collider found
{
NewtonCollider coll = colliders[0];
if (coll.transform == transform)
{
pColl = coll.CreateCollider(false); // Collider is a component of the same GameObject the Body is attached to. No offset available in this case.
}
else
{
pColl = coll.CreateCollider(true); // Collider is a component of a child GameObject, apply the child GameObjects offset transform.
}
}
else // Several colliders found, create a compound.
{
pColl = NewtonAPI.NewtonCreateCompoundCollision(pWorld, 0);
NewtonAPI.NewtonCompoundCollisionBeginAddRemove(pColl);
foreach (NewtonCollider coll in colliders)
{
IntPtr pSubColl;
if (coll.transform == transform)
{
pSubColl = coll.CreateCollider(false);
}
else
{
pSubColl = coll.CreateCollider(true);
}
NewtonAPI.NewtonCompoundCollisionAddSubCollision(pColl, pSubColl);
NewtonAPI.NewtonDestroyCollision(pSubColl);
}
NewtonAPI.NewtonCompoundCollisionEndAddRemove(pColl);
}
Matrix4x4 matrix = Matrix4x4.identity;
matrix.SetTRS(transform.position, transform.rotation, Vector3.one);
if (!Kinematic)
{
pBody = NewtonAPI.NewtonCreateDynamicBody(pWorld, pColl, ref matrix);
}
else
{
pBody = NewtonAPI.NewtonCreateKinematicBody(pWorld, pColl, ref matrix);
if (KinematicCollidable)
{
NewtonAPI.NewtonBodySetCollidable(pBody, 1);
}
}
NewtonAPI.NewtonBodySetMassProperties(pBody, Mass, pColl);
NewtonAPI.NewtonBodySetForceAndTorqueCallback(pBody, ApplyForceAndTorque);
//NewtonAPI.NewtonBodySetTransformCallback(pBody, SetTransform);
//gcHandle_instance = GCHandle.Alloc(this, GCHandleType.Normal);
//NewtonAPI.NewtonBodySetUserData(pBody, GCHandle.ToIntPtr(gcHandle_instance));
NewtonAPI.NewtonDestroyCollision(pColl); // Release the reference
//Debug.Log("Created body(" + pBody.ToString() + ")");
}
