public static void VectorAfterNormalForce(float forceX, float forceY, float forceZ, float normalX, float normalY, float normalZ, out float resX, out float resY, out float resZ)
{
float rejX, rejY, rejZ;
SimpleRigidbody3D.VectorRejection(forceX, forceY, forceZ, normalX, normalY, normalZ, out rejX, out rejY, out rejZ);
float projX, projY, projZ;
SimpleRigidbody3D.VectorProjection(forceX, forceY, forceZ, normalX, normalY, normalZ, out projX, out projY, out projZ);
float dotProd = forceX * normalX + forceY * normalY + forceZ * normalZ;
float pointingInSameDirection = dotProd / Mathf.Abs(dotProd);
// Not pointing in same direction
if (pointingInSameDirection <= 0.0f)
{
resX = rejX;
resY = rejY;
resZ = rejZ;
}
// Pointing in same direction
else
{
resX = rejX + projX;
resY = rejY + projY;
resZ = rejZ + projZ;
}
}
// Accessing zero is slow for some reason?
public bool SphereSphereCollision(SimpleRigidbody3D me, SimpleRigidbody3D other, out float pointX, out float pointY, out float pointZ, out float normalX, out float normalY, out float normalZ)
{
// Accessing properties of compoments are slow
float myRadius = me.radius;
float otherRadius = other.radius;
// I need to do this because for some reason unity Vector3f.Distance is slow
float dx = me.tmpX - other.tmpX;
float dy = me.tmpY - other.tmpY;
float dz = me.tmpZ - other.tmpZ;
float distance = Mathf.Sqrt(dx * dx + dy * dy + dz * dz);
float nx, ny, nz;
Normalize(1, 1, 1, out nx, out ny, out nz);
if (distance <= myRadius + otherRadius)
{
if (distance == 0.0f)
{
pointX = me.tmpX + nx / me.radius / 10.0f;
pointY = me.tmpY + ny / me.radius / 10.0f;
pointZ = me.tmpZ + nz / me.radius / 10.0f;
float tmpNormalX = me.tmpX - pointX;
float tmpNormalY = me.tmpY - pointY;
float tmpNormalZ = me.tmpZ - pointZ;
Normalize(tmpNormalX, tmpNormalY, tmpNormalZ, out normalX, out normalY, out normalZ);
}
else
{
pointX = (me.tmpX * myRadius + other.tmpX * otherRadius) / (myRadius + otherRadius);
pointY = (me.tmpY * myRadius + other.tmpY * otherRadius) / (myRadius + otherRadius);
pointZ = (me.tmpZ * myRadius + other.tmpZ * otherRadius) / (myRadius + otherRadius);
float tmpNormalX = me.tmpX - pointX;
float tmpNormalY = me.tmpY - pointY;
float tmpNormalZ = me.tmpZ - pointZ;
Normalize(tmpNormalX, tmpNormalY, tmpNormalZ, out normalX, out normalY, out normalZ);
}
return(true);
}
else
{
pointX = 0; pointY = 0; pointZ = 0;
normalX = 0; normalY = 0; normalZ = 0;
return(false);
}
}
public bool SphereBoxCollision(SimpleRigidbody3D me, BoxCollider other, out float pointX, out float pointY, out float pointZ, out float normalX, out float normalY, out float normalZ)
{
Vector3 closePoint = other.ClosestPointOnBounds(new Vector3(me.tmpX, me.tmpY, me.tmpZ));
float dx = me.tmpX - closePoint.x;
float dy = me.tmpY - closePoint.y;
float dz = me.tmpZ - closePoint.z;
float distance = Mathf.Sqrt(dx * dx + dy * dy + dz * dz);
if (distance <= me.radius)
{
float nx, ny, nz;
Normalize(1, 1, 1, out nx, out ny, out nz);
if (distance == 0.0f)
{
pointX = me.tmpX + nx / me.radius / 10.0f;
pointY = me.tmpY + ny / me.radius / 10.0f;
pointZ = me.tmpZ + nz / me.radius / 10.0f;
float tmpNormalX = me.tmpX - pointX;
float tmpNormalY = me.tmpY - pointY;
float tmpNormalZ = me.tmpZ - pointZ;
Normalize(tmpNormalX, tmpNormalY, tmpNormalZ, out normalX, out normalY, out normalZ);
}
else
{
pointX = closePoint.x;
pointY = closePoint.y;
pointZ = closePoint.z;
float tmpNormalX = me.tmpX - pointX;
float tmpNormalY = me.tmpY - pointY;
float tmpNormalZ = me.tmpZ - pointZ;
Normalize(tmpNormalX, tmpNormalY, tmpNormalZ, out normalX, out normalY, out normalZ);
}
return(true);
}
else
{
pointX = 0; pointY = 0; pointZ = 0;
normalX = 0; normalY = 0; normalZ = 0;
return(false);
}
}
public void ApplyBone(SimpleRigidbody3D left, SimpleRigidbody3D right, float desiredDist, ref float prevDistance, float distThreshold)
{
if (left == right)
{
return;
}
float pdx = left.tmpX - right.tmpX;
float pdy = left.tmpY - right.tmpY;
float pdz = left.tmpZ - right.tmpZ;
float distanceNow = Mathf.Sqrt(pdx * pdx + pdy * pdy + pdz * pdz);
// float distanceNow = Vector3f.Distance(left.tmpPosition, right.tmpPosition);
bool changing = false;
if (Mathf.Abs(desiredDist - distanceNow) >= distThreshold)
{
changing = true;
}
else
{
prevDistance = desiredDist;
return;
}
float npdx, npdy, npdz;
SimpleRigidbody3D.Normalize(pdx, pdy, pdz, out npdx, out npdy, out npdz);
float normX, normY, normZ;
float normLen = ((distanceNow - desiredDist) / Mathf.Abs(desiredDist - distanceNow));
normX = npdx * normLen;
normY = npdy * normLen;
normZ = npdz * normLen;
if (changing)
{
float myNotNormX, myNotNormY, myNotNormZ;
VectorAfterNormalForce(left.velocityX, left.velocityY, left.velocityZ, -normX, -normY, -normZ, out myNotNormX, out myNotNormY, out myNotNormZ);
float otherNotAlongX, otherNotAlongY, otherNotAlongZ;
VectorAfterNormalForce(right.velocityX, right.velocityY, right.velocityZ, normX, normY, normZ, out otherNotAlongX, out otherNotAlongY, out otherNotAlongZ);
float myAlongNormX = left.velocityX - myNotNormX;
float myAlongNormY = left.velocityY - myNotNormY;
float myAlongNormZ = left.velocityZ - myNotNormZ;
float otherAlongNormX = right.velocityX - otherNotAlongX;
float otherAlongNormY = right.velocityY - otherNotAlongY;
float otherAlongNormZ = right.velocityZ - otherNotAlongZ;
float avgAlongNormX = (myAlongNormX + otherAlongNormX) / 2.0f;
float avgAlongNormY = (myAlongNormY + otherAlongNormY) / 2.0f;
float avgAlongNormZ = (myAlongNormZ + otherAlongNormZ) / 2.0f;
float colNormMoveX = npdx;
float colNormMoveY = npdy;
float colNormMoveZ = npdz;
left.velocityX = myNotNormX + avgAlongNormX;
left.velocityY = myNotNormY + avgAlongNormY;
left.velocityZ = myNotNormZ + avgAlongNormZ;
right.velocityX = otherNotAlongX + avgAlongNormX;
right.velocityY = otherNotAlongY + avgAlongNormY;
right.velocityZ = otherNotAlongZ + avgAlongNormZ;
if (prevDistance != desiredDist)
{
right.velocityX += -colNormMoveX * (desiredDist - prevDistance);
right.velocityY += -colNormMoveY * (desiredDist - prevDistance);
right.velocityZ += -colNormMoveZ * (desiredDist - prevDistance);
left.velocityX += colNormMoveX * (desiredDist - prevDistance);
left.velocityY += colNormMoveY * (desiredDist - prevDistance);
left.velocityZ += colNormMoveZ * (desiredDist - prevDistance);
}
prevDistance = distanceNow;
}
else
{
prevDistance = desiredDist;
}
}
public bool FixCollisions()
{
bool fixedSomething = false;
for (int i = 0; i < otherColliders.Count; i++)
{
SimpleRigidbody3D other = otherColliders[i];
float colX, colY, colZ;
float normX, normY, normZ;
if (SphereSphereCollision(this, other, out colX, out colY, out colZ, out normX, out normY, out normZ))
{
fixedSomething = true;
float newVelX, newVelY, newVelZ;
SimplePhysics.VectorAfterNormalForce(velocityX, velocityY, velocityZ, normX, normY, normZ, out newVelX, out newVelY, out newVelZ);
float lostVelX = velocityX - newVelX;
float lostVelY = velocityY - newVelY;
float lostVelZ = velocityZ - newVelZ;
float otherNewVelX, otherNewVelY, otherNewVelZ;
SimplePhysics.VectorAfterNormalForce(other.velocityX, other.velocityY, other.velocityZ, -normX, -normY, -normZ, out otherNewVelX, out otherNewVelY, out otherNewVelZ);
float otherLostVelX = other.velocityX - otherNewVelX;
float otherLostVelY = other.velocityY - otherNewVelY;
float otherLostVelZ = other.velocityZ - otherNewVelZ;
float avgLostVelX = (lostVelX + otherLostVelX) / 2.0f;
float avgLostVelY = (lostVelY + otherLostVelY) / 2.0f;
float avgLostVelZ = (lostVelZ + otherLostVelZ) / 2.0f;
velocityX = newVelX * time.friction + avgLostVelX;
velocityY = newVelY * time.friction + avgLostVelY;
velocityZ = newVelZ * time.friction + avgLostVelZ;
other.velocityX = otherNewVelX * time.friction + avgLostVelX;
other.velocityY = otherNewVelY * time.friction + avgLostVelY;
other.velocityZ = otherNewVelZ * time.friction + avgLostVelZ;
float dx = colX - tmpX;
float dy = colY - tmpY;
float dz = colZ - tmpZ;
float dist = Mathf.Sqrt(dx * dx + dy * dy + dz * dz);
float moveOffsetX = normX * (radius - dist);
float moveOffsetY = normY * (radius - dist);
float moveOffsetZ = normZ * (radius - dist);
tmpX += moveOffsetX;
tmpY += moveOffsetY;
tmpZ += moveOffsetZ;
other.tmpX -= moveOffsetX;
other.tmpY -= moveOffsetY;
other.tmpZ -= moveOffsetZ;
}
}
for (int i = 0; i < staticColliders.Count; i++)
{
BoxCollider other = staticColliders[i];
float colX, colY, colZ;
float normX, normY, normZ;
if (SphereBoxCollision(this, other, out colX, out colY, out colZ, out normX, out normY, out normZ))
{
fixedSomething = true;
float newVelX, newVelY, newVelZ;
SimplePhysics.VectorAfterNormalForce(velocityX, velocityY, velocityZ, normX, normY, normZ, out newVelX, out newVelY, out newVelZ);
velocityX = newVelX * time.friction;
velocityY = newVelY * time.friction;
velocityZ = newVelZ * time.friction;
float dx = colX - tmpX;
float dy = colY - tmpY;
float dz = colZ - tmpZ;
float dist = Mathf.Sqrt(dx * dx + dy * dy + dz * dz);
float moveOffsetX = normX * (radius - dist);
float moveOffsetY = normY * (radius - dist);
float moveOffsetZ = normZ * (radius - dist);
tmpX += moveOffsetX;
tmpY += moveOffsetY;
tmpZ += moveOffsetZ;
}
}
return(fixedSomething);
}
public void Awake()
{
myRigidbody = GetComponent <SimpleRigidbody3D>();
initialDistance = Vector3.Distance(myRigidbody.transform.position, other.transform.position);
}
