public static Vector3 GetFixedAngularError(
SimulationObject objectA,
SimulationObject objectB,
Quaternion relativeOrientation)
{
Quaternion currentRelativeOrientation = objectB.RotationStatus.Inverse () *
objectA.RotationStatus;
Quaternion relativeOrientationError = relativeOrientation.Inverse () *
currentRelativeOrientation;
var angularError = new Vector3 (
relativeOrientationError.b,
relativeOrientationError.c,
relativeOrientationError.d);
if (relativeOrientationError.a < 0.0)
{
angularError = new Vector3 (
-angularError.x,
-angularError.y,
-angularError.z);
}
return objectA.RotationMatrix * angularError;
}
public FixedJointConstraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
double restoreCoefficient,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
SpringCoefficient = springCoefficient;
RestoreCoefficient = restoreCoefficient;
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
StartAnchorPoint = (objectB.Position - objectA.Position) * 0.5;
Vector3 relativePos = StartAnchorPoint - objectA.StartPosition;
relativePos = objectA.RotationMatrix * relativePos;
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
RelativeOrientation = objectB.RotationStatus.Inverse() *
objectA.RotationStatus;
}
public PistonConstraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
Vector3 startAnchorPosition,
Vector3 pistonAxis,
double restoreCoefficient,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
RestoreCoefficient = restoreCoefficient;
SpringCoefficient = springCoefficient;
StartAnchorPoint = startAnchorPosition;
PistonAxis = -1.0 * pistonAxis.Normalize ();
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
Vector3 relativePos = objectA.RotationMatrix *
(startAnchorPosition - objectA.StartPosition);
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
RelativeOrientation = objectB.RotationStatus.Inverse() *
objectA.RotationStatus;
}
public Hinge2Constraint(
int indexA,
int indexB,
SimulationObject[] simulationObject,
Vector3 startAnchorPosition,
Vector3 hingeAxis,
Vector3 rotationAxis,
double restoreCoefficient,
double springCoefficientHingeAxis,
double springCoefficient)
{
IndexA = indexA;
IndexB = indexB;
KeyIndex = GetHashCode();
RestoreCoefficient = restoreCoefficient;
SpringCoefficientHingeAxis = springCoefficientHingeAxis;
SpringCoefficient = springCoefficient;
StartAnchorPoint = startAnchorPosition;
HingeAxis = hingeAxis.Normalize ();
RotationAxis = rotationAxis.Normalize ();
SimulationObject objectA = simulationObject[IndexA];
SimulationObject objectB = simulationObject[IndexB];
Vector3 relativePos = startAnchorPosition - objectA.StartPosition;
relativePos = objectA.RotationMatrix * relativePos;
AnchorPoint = relativePos + objectA.Position;
StartErrorAxis1 = objectA.RotationMatrix.Transpose() *
(AnchorPoint - objectA.Position);
StartErrorAxis2 = objectB.RotationMatrix.Transpose() *
(AnchorPoint - objectB.Position);
Vector3 rHingeAxis = objectA.RotationMatrix * HingeAxis;
Vector3 rRotationAxis = objectB.RotationMatrix * RotationAxis;
RelativeOrientation1 = CalculateRelativeOrientation(
rHingeAxis,
rRotationAxis,
objectA.RotationStatus);
RelativeOrientation2 = CalculateRelativeOrientation(
rRotationAxis,
rHingeAxis,
objectB.RotationStatus);
}
public static double GetAngle(
SimulationObject simulationObjectA,
SimulationObject simulationObjectB,
Quaternion relativeRotation,
Vector3 rotationAxis)
{
Quaternion currentRelativeOrientation = simulationObjectA.RotationStatus.Inverse () *
simulationObjectB.RotationStatus;
Quaternion relativeOrientation = relativeRotation.Inverse () *
currentRelativeOrientation;
var quaternionVectorPart = new Vector3 (
relativeOrientation.b,
relativeOrientation.c,
relativeOrientation.d);
quaternionVectorPart = simulationObjectA.RotationMatrix * quaternionVectorPart;
return GetRotationAngle (
quaternionVectorPart,
relativeOrientation.a,
rotationAxis);
}
private void UpdateObjectPosition(
SimulationObject[] simulationObj)
{
int index = 0;
foreach (SimulationObject simObj in simulationObj)
{
if (simObj.ObjectType != ObjectType.StaticRigidBody)
{
#region Linear Velocity
double linearVelocity = simObj.TempLinearVelocity.Length();
simObj.SetPosition(
simObj.Position +
TimeStep *
simObj.TempLinearVelocity);
simObj.SetTempLinearVelocity(new Vector3());
#endregion
#region Angular Velocity
double angularVelocity = simObj.TempAngularVelocity.Length();
Vector3 versor = simObj.TempAngularVelocity.Normalize();
double rotationAngle = angularVelocity * TimeStep;
var rotationQuaternion = new Quaternion(versor, rotationAngle);
simObj.SetRotationStatus(
(rotationQuaternion * simObj.RotationStatus).Normalize());
simObj.SetRotationMatrix(simObj.RotationStatus.ConvertToMatrix());
simObj.SetInertiaTensor(
(simObj.RotationMatrix * simObj.BaseInertiaTensor) *
simObj.RotationMatrix.Transpose());
simObj.SetTempAngularVelocity(new Vector3());
#endregion
#region Update AABB
if (simObj.ObjectGeometry != null &&
(linearVelocity > 0.0 || angularVelocity > 0.0))
{
simObj.SetAABB();
simObj.SetTempLinearVelocity(new Vector3());
simObj.SetTempAngularVelocity(new Vector3());
}
#endregion
}
simulationObjects[index] = simObj;
index++;
}
}
/// <summary>
/// Integrates the objects position.
/// </summary>
private void IntegrateObjectsPosition(
SimulationObject[] simulationObj)
{
int index = 0;
foreach (SimulationObject simObj in simulationObj)
{
if (simObj.ObjectType != ObjectType.StaticRigidBody)
{
#region Linear Velocity
simObj.SetPosition (
simObj.Position +
TimeStep *
simObj.LinearVelocity);
simObj.SetLinearVelocity(simObj.LinearVelocity +
(simObj.ForceValue * simObj.InverseMass) +
(TimeStep * SimulationEngineParameters.ExternalForce));
simObj.SetForce(new Vector3());
double linearVelocity = simObj.LinearVelocity.Length();
#endregion
#region Angular Velocity
double angularVelocity = simObj.AngularVelocity.Length();
Vector3 versor = simObj.AngularVelocity.Normalize ();
double rotationAngle = angularVelocity * TimeStep;
var rotationQuaternion = new Quaternion(versor, rotationAngle);
simObj.SetRotationStatus ((rotationQuaternion * simObj.RotationStatus).Normalize());
simObj.SetRotationMatrix (simObj.RotationStatus.ConvertToMatrix ());
simObj.SetInertiaTensor (
(simObj.RotationMatrix * simObj.BaseInertiaTensor) *
simObj.RotationMatrix.Transpose ());
simObj.SetAngularVelocity(simObj.AngularVelocity +
simObj.InertiaTensor *
simObj.TorqueValue);
angularVelocity = simObj.AngularVelocity.Length();
simObj.SetTorque(new Vector3());
#endregion
#region Sleeping Object
if (SimulationEngineParameters.SleepingObject)
ObjectSleep(simObj);
#endregion
#region Update AABB
if (simObj.ObjectGeometry != null &&
(linearVelocity > 0.0 || angularVelocity > 0.0))
{
simObj.SetAABB();
}
#endregion
}
simulationObjects [index] = simObj;
index++;
}
}
public static Vector3 GetEuler(Quaternion q)
{
double test = q.b * q.c + q.d * q.a;
double heading, attitude, bank;
// singularity at north pole
if (test > 0.499999)
{
heading = 2.0 * Math.Atan2 (q.b, q.a);
attitude = Math.PI / 2.0;
bank = 0.0;
return new Vector3 (bank, heading, attitude);
}
// singularity at south pole
if (test < -0.499999) {
heading = -2.0 * Math.Atan2 (q.b, q.a);
attitude = -Math.PI / 2.0;
bank = 0.0;
return new Vector3 (bank, heading, attitude);
}
double sqx = q.b * q.b;
double sqy = q.c * q.c;
double sqz = q.d * q.d;
// y-axis
heading = Math.Atan2 (2.0 * q.c * q.a - 2.0 * q.b * q.d,
1.0 - 2.0 * sqy - 2.0 * sqz);
// z-axis
attitude = Math.Asin (2.0 * test);
// x-axis
bank = Math.Atan2 (2.0 * q.b * q.a - 2.0 * q.c * q.d,
1.0 - 2.0 * sqx - 2.0 * sqz);
return new Vector3 (bank, heading, attitude);
}
public static Quaternion IntegrateQuaternion(
Quaternion q,
Vector3 v,
double delta)
{
Vector3 theta = v * (delta * 0.5);
double ra = 0.0;
double rb = theta.x;
double rc = theta.y;
double rd = theta.z;
return Normalize (new Quaternion(ra,rb,rc,rd) * q);
}
public SimulationObject(
ObjectType type,
ObjectGeometry[] geometry,
double[] mass,
Vector3[] startCompositePosition,
Vector3 position,
Quaternion rotationStatus)
{
ObjectType = type;
ObjectGeometry = geometry;
PartialMass = new double[geometry.Length];
Array.Copy(mass, PartialMass, geometry.Length);
for (int i = 0; i < geometry.Length; i++)
Mass += mass[i];
if (ObjectType == ObjectType.StaticRigidBody)
{
Mass = 0.0;
InverseMass = 0.0;
}
else if (Mass > 0.0)
InverseMass = 1.0 / Mass;
RotationStatus = rotationStatus;
StartCompositePositionObjects = startCompositePosition;
SetObjectProperties();
Position = position;
SetAABB();
SleepingFrameCount = 0;
}
double GetAngle1(
Vector3 axis1,
Vector3 axis2,
Vector3 startAxis,
Quaternion rotationStatus,
Quaternion startRelativeRotation)
{
Matrix3x3 rotationMatrix = Matrix3x3.GetRotationMatrix(axis1, axis2);
Quaternion rotationQ = Quaternion.GetQuaternion(rotationMatrix);
Quaternion mult1 = Quaternion.Multiply1(rotationStatus, rotationQ);
Quaternion mult2 = Quaternion.Multiply2(mult1, startRelativeRotation);
var quaternionVectorPart = new Vector3(
mult2.b,
mult2.c,
mult2.d);
return JacobianCommon.GetRotationAngle(quaternionVectorPart, mult2.a, startAxis);
}
public static Quaternion Inverse(Quaternion q)
{
double d = 1.0 / (q.a * q.a + q.b * q.b + q.c * q.c + q.d * q.d);
return new Quaternion (q.a * d, -q.b * d, -q.c * d, -q.d * d);
}
public static Quaternion Multiply2(Quaternion q1, Quaternion q2)
{
double a = (q1.a * q2.a + q1.b * q2.b + q1.c * q2.c + q1.d * q2.d);
double b = (-q1.a * q2.b + q1.b * q2.a - q1.c * q2.d + q1.d * q2.c);
double c = (-q1.a * q2.c + q1.c * q2.a - q1.d * q2.b + q1.b * q2.d);
double d = (-q1.a * q2.d + q1.d * q2.a - q1.b * q2.c + q1.c * q2.b);
return new Quaternion (a, b, c, d);
}
/// <summary>
/// Gets the vector.
/// </summary>
/// <returns>The vector.</returns>
/// <param name="q1">Q1.</param>
/// <param name="q2">Q2.</param>
public static Vector3 GetVector(Quaternion q1, Quaternion q2)
{
double x = (q1.a * q2.b + q1.b * q2.a + q1.c * q2.d - q1.d * q2.c);
double y = (q1.a * q2.c - q1.b * q2.d + q1.c * q2.a + q1.d * q2.b);
double z = (q1.a * q2.d + q1.b * q2.c - q1.c * q2.b + q1.d * q2.a);
return new Vector3 (x, y, z);
}
public void SetRotationStatus(Quaternion inputRotationStatus)
{
RotationStatus = inputRotationStatus;
}
double GetAngle2(
Vector3 axis1,
Vector3 axis2,
Vector3 startAxis,
Quaternion rotationStatus,
Quaternion startRelativeRotation)
{
return -GetAngle1(axis2, axis1, startAxis, rotationStatus, startRelativeRotation);
}
public static Matrix3x3 ConvertToMatrix(Quaternion q)
{
double xx = 2.0 * q.b * q.b;
double yy = 2.0 * q.c * q.c;
double zz = 2.0 * q.d * q.d;
double s = q.a;
double r1x = 1.0 - yy - zz;
double r1y = 2.0 * (q.b * q.c - s * q.d);
double r1z = 2.0 * (q.b * q.d + s * q.c);
double r2x = 2.0 * (q.b * q.c + s * q.d);
double r2y = 1.0 - xx - zz;
double r2z = 2.0 * (q.c * q.d - s * q.b);
double r3x = 2.0 * (q.b * q.d - s * q.c);
double r3y = 2.0 * (q.c * q.d + s * q.b);
double r3z = 1.0 - xx - yy;
return new Matrix3x3 (
r1x, r1y, r1z,
r2x, r2y, r2z,
r3x, r3y, r3z);
}
Quaternion CalculateRelativeOrientation(
Vector3 axis1,
Vector3 axis2,
Quaternion bodyRotationStatus)
{
Matrix3x3 rotationMatrix = Matrix3x3.GetRotationMatrix(axis1, axis2);
Quaternion rotationQ = Quaternion.GetQuaternion(rotationMatrix);
return Quaternion.Multiply1(bodyRotationStatus, rotationQ);
}
public static double Length(Quaternion q)
{
return Math.Sqrt (
q.a * q.a +
q.b * q.b +
q.c * q.c +
q.d * q.d);
}
public static Quaternion Normalize(Quaternion q)
{
double l = Length (q);
if (l > 0.0) {
double inv = 1.0 / l;
double ra = q.a * inv;
double rb = q.b * inv;
double rc = q.c * inv;
double rd = q.d * inv;
return new Quaternion (ra, rb, rc, rd);
}
return q;
}
//TODO Refactoring costruttori(inserire possibilità di creeare oggetti con centro di massa e inertia tensor prestabiliti)
public SimulationObject(
ObjectType type,
ObjectGeometry geometry,
double mass,
Vector3 position,
Quaternion rotationStatus)
{
ObjectType = type;
ObjectGeometry = new ObjectGeometry[1] { geometry };
PartialMass = new double[1] { mass };
Mass = mass;
if (ObjectType == ObjectType.StaticRigidBody)
{
Mass = 0.0;
InverseMass = 0.0;
}
else if (Mass > 0.0)
InverseMass = 1.0 / Mass;
StartCompositePositionObjects = new Vector3[1];
RotationStatus = rotationStatus;
Position = position;
SetObjectProperties();
SetAABB();
SleepingFrameCount = 0;
}
