public bool Collide(NewtonBody body1, NewtonBody body2, out NewtonCollideInfo hitInfo)
{
dMatrix matrix1 = Utils.ToMatrix(body1.Position, body1.Rotation);
dMatrix matrix2 = Utils.ToMatrix(body2.Position, body2.Rotation);
var hitInfoPtr = m_world.Collide(matrix1, matrix2, body1.m_collision.GetShape(), body2.m_collision.GetShape(), 1);// m_world.Raycast(startPos.x, startPos.y, startPos.z, endPos.x, endPos.y, endPos.z, layerMask);
//mat_handle.Free();
if (hitInfoPtr != IntPtr.Zero)
{
_InternalConvexCastInfo info = (_InternalConvexCastInfo)Marshal.PtrToStructure(hitInfoPtr, typeof(_InternalConvexCastInfo));
hitInfo.body = body2;
hitInfo.position = info.point;
hitInfo.normal = info.normal;
hitInfo.collisionID1 = (uint)info.contactID1;
hitInfo.collisionID2 = (uint)info.contactID2;
hitInfo.penetration = info.penetration;
hitInfo.timeOfImpact = 0;
return(true);
}
hitInfo.body = null;
hitInfo.position = Vector3.zero;
hitInfo.normal = Vector3.zero;
hitInfo.collisionID1 = 0;
hitInfo.collisionID2 = 0;
hitInfo.penetration = 0;
hitInfo.timeOfImpact = 0;
return(false);
}
override public void OnPostCollision(NewtonBody otherBody)
{
if (largestImpact > 1)
{
Debug.Log("Bounce! (" + largestImpact + ")");
}
}
internal void RegisterBody(NewtonBody nb)
{
if (!m_bodies.Contains(nb)) // Had to add this
{
m_bodies.Add(nb);
}
}
override public void OnContact(NewtonBody otherBody, float normalImpact)
{
if (normalImpact > largestImpact)
{
largestImpact = normalImpact;
}
}
override public void OnCollision(NewtonBody otherBody)
{
Vector4 plane = new Vector4(0, 1, 0, -mr.bounds.max.y);
Vector3 force = Vector3.zero;
Vector3 torque = Vector3.zero;
float density = 0.8f;
if (otherBody.gameObject.tag == "Ice")
{
density = 0.9f;
}
else if (otherBody.gameObject.tag == "Wood")
{
density = 0.6f;
}
else if (otherBody.gameObject.tag == "Metal")
{
density = 1.2f;
}
otherBody.CalculateBuoyancyForces(plane, ref force, ref torque, density);
otherBody.m_forceAcc += force;
otherBody.m_torqueAcc += torque;
}
private void ValidateEditorShape()
{
if (m_editorShape == null)
{
NewtonBody body = null;
Transform gameTransform = transform;
while (gameTransform != null)
{
// this is a child body we need to find the root rigid body owning the shape
if (body == null)
{
body = gameTransform.gameObject.GetComponent <NewtonBody>();
}
gameTransform = gameTransform.parent;
}
if (body != null)
{
if (body.m_world != null)
{
m_editorShape = Create(body.m_world);
}
}
}
}
public NewtonBodyCollision(NewtonBody body)
{
List <ColliderShapePair> colliderList = new List <ColliderShapePair>();
TraverseColliders(body.gameObject, colliderList, body.gameObject, body);
if (body.m_isScene)
{
m_collidersArray = new ColliderShapePair[colliderList.Count + 1];
NewtonSceneCollider sceneCollider = body.gameObject.AddComponent <NewtonSceneCollider>();
dNewtonCollisionScene sceneShape = (dNewtonCollisionScene)sceneCollider.Create(body.m_world);
m_collidersArray[0].m_shape = sceneShape;
m_collidersArray[0].m_collider = sceneCollider;
int index = 1;
sceneShape.BeginAddRemoveCollision();
foreach (ColliderShapePair pair in colliderList)
{
m_collidersArray[index] = pair;
sceneShape.AddCollision(pair.m_shape);
index++;
}
sceneShape.EndAddRemoveCollision();
}
else if (colliderList.Count == 0)
{
m_collidersArray = new ColliderShapePair[1];
NewtonCollider collider = body.gameObject.AddComponent <NewtonNullCollider>();
m_collidersArray[0].m_collider = collider;
m_collidersArray[0].m_shape = collider.Create(body.m_world);
}
else if (colliderList.Count == 1)
{
m_collidersArray = new ColliderShapePair[1];
m_collidersArray[0] = colliderList[0];
}
else
{
m_collidersArray = new ColliderShapePair[colliderList.Count + 1];
NewtonCompoundCollider compoundCollider = body.gameObject.AddComponent <NewtonCompoundCollider>();
dNewtonCollisionCompound compoundShape = (dNewtonCollisionCompound)compoundCollider.Create(body.m_world);
m_collidersArray[0].m_shape = compoundShape;
m_collidersArray[0].m_collider = compoundCollider;
int index = 1;
compoundShape.BeginAddRemoveCollision();
foreach (ColliderShapePair pair in colliderList)
{
m_collidersArray[index] = pair;
compoundShape.AddCollision(pair.m_shape);
index++;
}
compoundShape.EndAddRemoveCollision();
}
}
override public void OnCollision(NewtonBody otherBody)
{
//Debug.Log("xxxxxx woohoo collision notification script !!");
if (GetComponent <NewtonCollider>().m_isTrigger)
{
Debug.Log(gameObject.name + " is trigger");
}
//Debug.Log(this.gameObject.name + " colliding with " + otherBody.gameObject.name);
}
public override void Create()
{
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointBallAndSocket(matrix, child.GetBody().GetBody(), otherBody);
Stiffness = m_stiffness;
}
public override void InitJoint()
{
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointDoubleHinge(matrix, child.GetBody().GetBody(), otherBody);
Stiffness = m_stiffness;
EnableLimits_0 = m_enableLimits_0;
}
public override void Create()
{
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointHingeActuator(matrix, child.GetBody().GetBody(), otherBody);
TargetAngle = m_targetAngle;
AngularRate = m_angularRate;
MaxTorque = m_maxTorque;
}
public override void Create()
{
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointSliderActuator(matrix, child.GetBody().GetBody(), otherBody);
Speed = m_speed;
MaxForce = m_maxForce;
TargetPosition = m_targetPosition;
}
public override void Create()
{
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointSlider(matrix, child.GetBody().GetBody(), otherBody);
Stiffness = m_stiffness;
EnableLimits = m_enableLimits;
SetSpringDamper = m_setSpringDamper;
}
private void InitPhysicsScene(GameObject root)
{
NewtonBody bodyPhysics = root.GetComponent <NewtonBody>();
if (bodyPhysics != null)
{
bodyPhysics.InitRigidBody();
}
foreach (Transform child in root.transform)
{
InitPhysicsScene(child.gameObject);
}
}
public int OnAABBOverlap(IntPtr body0, IntPtr body1)
{
NewtonBody body0_ = (NewtonBody)GCHandle.FromIntPtr(body0).Target;
NewtonBody body1_ = (NewtonBody)GCHandle.FromIntPtr(body1).Target;
if (m_aabb_overlap_callback != null)
{
return(m_aabb_overlap_callback(body0_, body1_) ? 1 : 0);
}
else
{
return(1);
}
}
static void NewtonUpdate(NewtonBody body)
{
acceleration = Vector3.zero;
foreach(NewtonBody otherBody in bodies) {
if(body == otherBody) continue;
direction = (otherBody._transform.position - body._transform.position);
acceleration += self.g * (direction.normalized * otherBody.mass) / direction.sqrMagnitude;
}
body.velocity += acceleration * fixedDeltaTime;
body._transform.position += body.velocity * fixedDeltaTime;
}
public override void Create()
{
NewtonBody child = GetComponent <NewtonBody>();
Matrix4x4 localMatrix = Matrix4x4.identity;
localMatrix.SetTRS(m_posit, Quaternion.Euler(m_rotation), Vector3.one);
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
Vector3 pin = localMatrix.GetColumn(0);
dVector normal = new dVector(pin.x, pin.y, pin.z, 0.0f);
dVector posit = new dVector(m_posit.x, m_posit.y, m_posit.z, 1.0f);
m_joint = new dNewtonJointPlane3DOF(posit, normal, child.GetBody().GetBody(), otherBody);
Stiffness = m_stiffness;
}
static void NewtonUpdate(NewtonBody body)
{
acceleration = Vector3.zero;
foreach (NewtonBody otherBody in bodies)
{
if (body == otherBody)
{
continue;
}
direction = (otherBody._transform.position - body._transform.position);
acceleration += self.g * (direction.normalized * otherBody.mass) / direction.sqrMagnitude;
}
body.velocity += acceleration * fixedDeltaTime;
body._transform.position += body.velocity * fixedDeltaTime;
}
public void OnInteraction(IntPtr properties, IntPtr body0, IntPtr body1, IntPtr contact, float normalImpact)
{
MaterialContactInfo contactInfo = (MaterialContactInfo)Marshal.PtrToStructure(contact, typeof(MaterialContactInfo));
MaterialProperties properties_ = (MaterialProperties)Marshal.PtrToStructure(properties, typeof(MaterialProperties));
//NewtonBody body0_ = (NewtonBody)Marshal.PtrToStructure(body0, typeof(NewtonBody));
//NewtonBody body1_ = (NewtonBody)Marshal.PtrToStructure(body1, typeof(NewtonBody));
NewtonBody body0_ = (NewtonBody)GCHandle.FromIntPtr(body0).Target;
NewtonBody body1_ = (NewtonBody)GCHandle.FromIntPtr(body1).Target;
//Debug.Log("On interaction");
if (m_callback != null)
{
m_callback(ref properties_, contactInfo, normalImpact, body0_, body1_);
}
Marshal.StructureToPtr(properties_, properties, false);
}
// Use this for initialization
void Start()
{
baseNB = transform.Find("RobotBase").GetComponent <NewtonBody>();
baseActuator = transform.Find("RobotBase").GetComponent <NewtonHingeActuator>();
bodyNB = transform.Find("RobotBody").GetComponent <NewtonBody>();
bodyActuator = transform.Find("RobotBody").GetComponent <NewtonHingeActuator>();
armNB = transform.Find("RobotArm").GetComponent <NewtonBody>();
armActuator = transform.Find("RobotArm").GetComponent <NewtonHingeActuator>();
handANB = transform.Find("RobotHandA").GetComponent <NewtonBody>();
handAActuator = transform.Find("RobotHandA").GetComponent <NewtonSliderActuator>();
handBNB = transform.Find("RobotHandB").GetComponent <NewtonBody>();
handBActuator = transform.Find("RobotHandB").GetComponent <NewtonSliderActuator>();
}
public bool Collide(/*Matrix4x4 matrix,*/ NewtonBody body, out NewtonCollideInfo hitInfo, int layerMask = 0)
{
//dMatrix matrix = Utils.ToMatrix(body.transform.position, body.transform.rotation);
//GCHandle mat_handle = GCHandle.Alloc(matrix);
//dVector vec = new dVector(transform.position.x, transform.position.y, transform.position.z);
//dQuaternion quat = new dQuaternion(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
dMatrix matrix = Utils.ToMatrix(body.transform.position, body.transform.rotation);
var hitInfoPtr = m_world.Collide(matrix, body.m_collision.GetShape(), layerMask);// m_world.Raycast(startPos.x, startPos.y, startPos.z, endPos.x, endPos.y, endPos.z, layerMask);
//mat_handle.Free();
if (hitInfoPtr != IntPtr.Zero)
{
_InternalConvexCastInfo info = (_InternalConvexCastInfo)Marshal.PtrToStructure(hitInfoPtr, typeof(_InternalConvexCastInfo));
if (info.hitBody != IntPtr.Zero)
{
hitInfo.body = (NewtonBody)GCHandle.FromIntPtr(info.hitBody).Target;
}
else
{
hitInfo.body = null;
}
//hitInfo.collider = null;
hitInfo.position = info.point;
hitInfo.normal = info.normal;
//hitInfo.collisionID = 0;
hitInfo.collisionID1 = 0;
hitInfo.collisionID2 = (uint)info.contactID2;
hitInfo.penetration = info.penetration;
hitInfo.timeOfImpact = 0;
return(true);
}
hitInfo.body = null;
//hitInfo.collider = null;
hitInfo.position = Vector3.zero;
hitInfo.normal = Vector3.zero;
//hitInfo.collisionID = 0;
hitInfo.collisionID1 = 0;
hitInfo.collisionID2 = 0;
hitInfo.penetration = 0;
hitInfo.timeOfImpact = 0;
return(false);
}
public override void InitJoint()
{
if (initialized)
{
return;
}
NewtonBody child = GetComponent <NewtonBody>();
dMatrix matrix = Utils.ToMatrix(m_posit, Quaternion.Euler(m_rotation));
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
m_joint = new dNewtonJointHinge(matrix, child.GetBody().GetBody(), otherBody);
Stiffness = m_stiffness;
EnableLimits = m_enableLimits;
SetSpringDamper = m_setSpringDamper;
initialized = true;
}
public override void Create()
{
Matrix4x4 localMatrix0 = Matrix4x4.identity;
Matrix4x4 localMatrix1 = Matrix4x4.identity;
localMatrix0.SetTRS(Vector3.zero, Quaternion.Euler(m_rotation), Vector3.one);
localMatrix1.SetTRS(Vector3.zero, Quaternion.Euler(m_parentRotation), Vector3.one);
Vector4 childPin = localMatrix0.GetColumn(0);
Vector4 parentPin = localMatrix1.GetColumn(0);
NewtonBody child = GetComponent <NewtonBody>();
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
dVector childPin_ = new dVector(childPin.x, childPin.y, childPin.z, 0.0f);
dVector parentPin_ = new dVector(parentPin.x, parentPin.y, parentPin.z, 0.0f);
m_joint = new dNewtonJointGear(m_gearRatio, childPin_, parentPin_, child.GetBody().GetBody(), otherBody);
}
public override void InitJoint()
{
Matrix4x4 localMatrix0 = Matrix4x4.identity;
Matrix4x4 localMatrix1 = Matrix4x4.identity;
Matrix4x4 localMatrix2 = Matrix4x4.identity;
localMatrix0.SetTRS(Vector3.zero, Quaternion.Euler(m_rotation), Vector3.one);
localMatrix1.SetTRS(Vector3.zero, Quaternion.Euler(m_parentRotation), Vector3.one);
localMatrix2.SetTRS(Vector3.zero, Quaternion.Euler(m_referenceRotation), Vector3.one);
Vector4 childPin = localMatrix0.GetColumn(0);
Vector4 parentPin = localMatrix1.GetColumn(0);
Vector4 referencePin = localMatrix2.GetColumn(0);
NewtonBody child = GetComponent <NewtonBody>();
IntPtr otherBody = (m_otherBody != null) ? m_otherBody.GetBody().GetBody() : new IntPtr(0);
IntPtr referenceBody = (m_referenceBody != null) ? m_referenceBody.GetBody().GetBody() : new IntPtr(0);
dVector dChildPin = new dVector(childPin.x, childPin.y, childPin.z, 0.0f);
dVector dParentPin = new dVector(parentPin.x, parentPin.y, parentPin.z, 0.0f);
dVector dReferencePin = new dVector(referencePin.x, referencePin.y, referencePin.z, 0.0f);
m_joint = new dNewtonJointDifferentialGear(m_gearRatio, dChildPin, dParentPin, dReferencePin, child.GetBody().GetBody(), otherBody, referenceBody);
}
// Use this for initialization
void Start()
{
body = GetComponent <NewtonBody>();
}
public virtual void OnContact(NewtonBody otherBody, float normalImpact)
{
// do nothing
//Debug.Log("do nothing");
}
public virtual void OnPostCollision(NewtonBody otherBody)
{
// do nothing
//Debug.Log("do nothing");
}
internal void UnregisterBody(NewtonBody nb)
{
m_bodies.Remove(nb);
}
internal void RegisterBody(NewtonBody nb)
{
m_bodies.Add(nb);
}
public NewtonBodyCollision(NewtonBody body)
{
if (body.m_world.GetWorld() == null)
{
throw new NullReferenceException("Native world instance is null. The World component was probably destroyed");
}
List <ColliderShapePair> colliderList = new List <ColliderShapePair>();
TraverseColliders(body.gameObject, colliderList, body.gameObject, body);
if (body.m_isScene)
{
m_collidersArray = new ColliderShapePair[colliderList.Count + 1];
NewtonSceneCollider sceneCollider = body.gameObject.AddComponent <NewtonSceneCollider>();
dNewtonCollisionScene sceneShape = (dNewtonCollisionScene)sceneCollider.Create(body.m_world);
m_collidersArray[0].m_shape = sceneShape;
m_collidersArray[0].m_collider = sceneCollider;
int index = 1;
sceneShape.BeginAddRemoveCollision();
foreach (ColliderShapePair pair in colliderList)
{
m_collidersArray[index] = pair;
sceneShape.AddCollision(pair.m_shape);
index++;
}
sceneShape.EndAddRemoveCollision();
}
else if (colliderList.Count == 0)
{
m_collidersArray = new ColliderShapePair[1];
NewtonCollider collider = body.gameObject.AddComponent <NewtonNullCollider>();
m_collidersArray[0].m_collider = collider;
m_collidersArray[0].m_shape = collider.Create(body.m_world);
}
else if (colliderList.Count == 1)
{
m_collidersArray = new ColliderShapePair[1];
m_collidersArray[0] = colliderList[0];
}
else
{
m_collidersArray = new ColliderShapePair[colliderList.Count + 1];
NewtonCompoundCollider compoundCollider = body.gameObject.AddComponent <NewtonCompoundCollider>();
dNewtonCollisionCompound compoundShape = (dNewtonCollisionCompound)compoundCollider.Create(body.m_world);
m_collidersArray[0].m_shape = compoundShape;
m_collidersArray[0].m_collider = compoundCollider;
int index = 1;
compoundShape.BeginAddRemoveCollision();
foreach (ColliderShapePair pair in colliderList)
{
m_collidersArray[index] = pair;
compoundShape.AddCollision(pair.m_shape);
index++;
}
compoundShape.EndAddRemoveCollision();
}
}
private void TraverseColliders(GameObject gameObject, List <ColliderShapePair> colliderList, GameObject rootObject, NewtonBody body)
{
// Don't fetch colliders from children with NewtonBodies
if ((gameObject == rootObject) || (gameObject.GetComponent <NewtonBody>() == null))
{
//Fetch all colliders
foreach (NewtonCollider collider in gameObject.GetComponents <NewtonCollider>())
{
dNewtonCollision shape = collider.CreateBodyShape(body.m_world);
if (shape != null)
{
ColliderShapePair pair;
pair.m_collider = collider;
pair.m_shape = shape;
colliderList.Add(pair);
}
}
Terrain terrain = gameObject.GetComponent <Terrain>();
if (terrain)
{
NewtonHeighfieldCollider heighfield = gameObject.GetComponent <NewtonHeighfieldCollider>();
if (heighfield)
{
TerrainData data = terrain.terrainData;
int treesCount = data.treeInstanceCount;
TreeInstance[] treeInstanceArray = data.treeInstances;
TreePrototype[] treeProtoArray = data.treePrototypes;
Vector3 posit = Vector3.zero;
for (int i = 0; i < treesCount; i++)
{
TreeInstance tree = treeInstanceArray[i];
posit.x = tree.position.x * data.size.x;
posit.y = tree.position.y * data.size.y;
posit.z = tree.position.z * data.size.z;
//Debug.Log("xxx0 " + posit);
TreePrototype treeProto = treeProtoArray[tree.prototypeIndex];
GameObject treeGameObject = treeProto.prefab;
foreach (NewtonCollider treeCollider in treeGameObject.GetComponents <NewtonCollider>())
{
dNewtonCollision treeShape = treeCollider.CreateBodyShape(body.m_world);
if (treeShape != null)
{
ColliderShapePair pair;
Vector3 treePosit = terrain.transform.position + treeCollider.m_posit + posit;
//Debug.Log("xxx1 " + treePosit);
dMatrix matrix = Utils.ToMatrix(treePosit, Quaternion.identity);
treeShape.SetMatrix(matrix);
pair.m_collider = treeCollider;
pair.m_shape = treeShape;
colliderList.Add(pair);
}
}
}
}
}
foreach (Transform child in gameObject.transform)
{
TraverseColliders(child.gameObject, colliderList, rootObject, body);
}
}
}
