private void UpdateRotationEulerExplicit(float dt)
{
rotation += .5f * angularVelocity * rotation * dt;
rotation.Normalize();
angularVelocity += angularAcceleration * dt;
}
private void UpdateRotationKinematic(float dt)
{
//rotation += .5f * velQuat * rotation * dt + (accQuat * rotation + .5f * velQuat * velQuat * rotation) * dt * dt;
rotation += angularVelocity * rotation * dt * .5f + angularAcceleration * rotation * dt * dt * .25f - rotation * angularVelocity.sqrMagnitude * dt * dt * .125f;
rotation.Normalize();
angularVelocity += angularAcceleration * dt;
}
public static PhysicsQuaternion operator+(PhysicsQuaternion a, PhysicsQuaternion b)
{
PhysicsQuaternion returnQuat = new PhysicsQuaternion();
returnQuat.w = a.w + b.w;
returnQuat.x = a.x + b.x;
returnQuat.y = a.y + b.y;
returnQuat.z = a.z + b.z;
return(returnQuat);
}
public static PhysicsQuaternion operator-(PhysicsQuaternion a)
{
PhysicsQuaternion returnQuat = new PhysicsQuaternion();
returnQuat.x = -a.x;
returnQuat.y = -a.y;
returnQuat.z = -a.z;
returnQuat.w = -a.w;
return(returnQuat);
}
// Start is called before the first frame update
void Start()
{
position = transform.position;
rotation = new PhysicsQuaternion(transform.rotation);
SetMass(startMass);
SetMomentOfInertia(inertiaBody);
worldTransformMatrix = new Matrix4x4();
centerOfMassLocal = new Vector3(transform.localScale.x / 2f, transform.localScale.y / 2f, transform.localScale.z / 2f);
centerOfMassGlobal = transform.position;
forceOfGravity = ForceGenerator.GenerateForce_gravity(Vector3.up, accelerationGravity, mass);
}
public void ReInit()
{
position = Vector3.zero;
velocity = Vector3.zero;
acceleration = Vector3.zero;
rotation = PhysicsQuaternion.identity;
angularVelocity = Vector3.zero;
angularAcceleration = Vector3.zero;
totalForce = Vector3.zero;
totalTorque = Vector3.zero;
}
public static PhysicsQuaternion operator*(PhysicsQuaternion a, PhysicsQuaternion b)
{
PhysicsQuaternion returnQuat = new PhysicsQuaternion();
Vector3 vector = Vector3.zero;
Vector3 vectorA = new Vector3(a.x, a.y, a.z), vectorB = new Vector3(b.x, b.y, b.z);
vector = a.w * vectorB + b.w * vectorA + Vector3.Cross(vectorA, vectorB);
returnQuat.w = a.w * b.w - Vector3.Dot(vectorA, vectorB);
returnQuat.x = vector.x;
returnQuat.y = vector.y;
returnQuat.z = vector.z;
return(returnQuat);
}
public static PhysicsQuaternion operator*(PhysicsQuaternion quat, Vector3 vec)
{
PhysicsQuaternion returnQuat = new PhysicsQuaternion();
Vector3 vector = Vector3.zero;
Vector3 quatVector = new Vector3(quat.x, quat.y, quat.z);
vector = quat.w * vec + Vector3.Cross(quatVector, vec);
returnQuat.w = -Vector3.Dot(quatVector, vec);
returnQuat.x = vector.x;
returnQuat.y = vector.y;
returnQuat.z = vector.z;
return(returnQuat);
}
// Update is called once per frame
void FixedUpdate()
{
//https://www.khanacademy.org/science/ap-physics-1/ap-forces-newtons-laws/friction-ap/v/static-and-kinetic-friction-example
float dirtWoodStatFricCoeff = .6f, dirtWoodKinFricCoeff = .55f;
float cubeDragCoeff = 1.05f, airFluidDensity = .001225f;
UpdateWorldMatrix();
if (simulate)
{
UpdateParticleData();
switch (updateType)
{
case UpdateType.EULER:
{
UpdatePositionEulerExplicit(Time.deltaTime);
UpdateRotationEulerExplicit(Time.deltaTime);
break;
}
case UpdateType.KINEMATIC:
{
UpdatePositionKinematic(Time.deltaTime);
UpdateRotationKinematic(Time.deltaTime);
break;
}
}
switch (forceType)
{
case ForceType.GRAVITY:
{
AddForce(forceOfGravity);
break;
}
case ForceType.NORMAL:
{
AddForce(forceOfGravity);
//AddForce(normalForceUp);
break;
}
case ForceType.SLIDING:
{
//AddForce(ForceGenerator.GenerateForce_sliding(forceOfGravity, normalForce45));
break;
}
case ForceType.FRICTION_STATIC:
{
//AddForce(ForceGenerator.GenerateForce_friction_static(normalForceLeft, forceOfGravity, dirtWoodStatFricCoeff));
break;
}
case ForceType.FRICTION_KINETIC:
{
//AddForce(ForceGenerator.GenerateForce_friction_kinetic(normalForceUp, velocity, dirtWoodKinFricCoeff));
break;
}
case ForceType.DRAG:
{
AddForce(forceOfGravity);
AddForce(ForceGenerator.GenerateForce_drag(velocity, new Vector3(0, 0, 0), airFluidDensity, 1, cubeDragCoeff));
break;
}
case ForceType.SPRING:
{
AddForce(ForceGenerator.GenerateForce_spring(position, new Vector3(0, 100, 0), 5, .2f));
break;
}
case ForceType.TORQUE:
{
AddTorque(torqueForce, centerOfMassGlobal + torquePointApplied);
break;
}
}
UpdateAcceleration();
UpdateAngularAcceleration();
}
else
{
position = transform.position;
rotation = new PhysicsQuaternion(transform.rotation);
}
}