public AeroControl(Matrix3 mbase, Matrix3 mintens, Matrix3 maxtens, Vector3 position, Vector3 windspeed)
: base(mbase, position, windspeed)
{
MaxTensor = maxtens;
MinTensor = mintens;
ControlSetting = 0.0f;
}
internal void updateforcefromtensor(RigidBody body, float duration, Matrix3 tensor)
{
Vector3 velocity = body.GetVelocity();
velocity += WindSpeed;
Vector3 bodyVel = body.GetTransform().TransformInverseDirection(velocity);
Vector3 bodyForce = tensor.Transform(bodyVel);
Vector3 force = body.GetTransform().TransformDirection(bodyForce);
body.AddForceAtBodyPoint(force, Position);
}
public void SetInverse(Matrix3 m)
{
float t4 = m.data[0] * m.data[4];
float t6 = m.data[0] * m.data[5];
float t8 = m.data[1] * m.data[3];
float t10 = m.data[2] * m.data[3];
float t12 = m.data[1] * m.data[6];
float t14 = m.data[2] * m.data[6];
float t16 = (t4 * m.data[8] - t6 * m.data[7] - t8 * m.data[8] + t10 * m.data[7] + t12 * m.data[5] - t14 * m.data[4]);
if (t16 == (float)0.0f) return;
float t17 = 1.0f / t16;
if (data == null) data = new float[9];
data[0] = (m.data[4] * m.data[8] - m.data[5] * m.data[7]) * t17;
data[1] = -(m.data[1] * m.data[8] - m.data[2] * m.data[7]) * t17;
data[2] = (m.data[1] * m.data[5] - m.data[2] * m.data[4]) * t17;
data[3] = -(m.data[3] * m.data[8] - m.data[5] * m.data[6]) * t17;
data[4] = (m.data[0] * m.data[8] - t14) * t17;
data[5] = -(t6 - t10) * t17;
data[6] = (m.data[3] * m.data[7] - m.data[4] * m.data[6]) * t17;
data[7] = -(m.data[0] * m.data[7] - t12) * t17;
data[8] = (t4 - t8) * t17;
}
public void SetTranspose(Matrix3 m)
{
data[0] = m.data[0];
data[1] = m.data[3];
data[2] = m.data[6];
data[3] = m.data[1];
data[4] = m.data[4];
data[5] = m.data[7];
data[6] = m.data[2];
data[7] = m.data[5];
data[8] = m.data[8];
}
public static Matrix3 LinearInterpolate(Matrix3 a, Matrix3 b, float prop)
{
Matrix3 result = Matrix3.Identity;
for (int i = 0; i < 9; i++)
{
result.data[i] = a.data[i] * (1 - prop) + b.data[i] * prop;
}
return result;
}
public void CalculateDerivedData()
{
Orientation.Normalize();
TransformMatrix = _calculateTransformMatrix(Position, Orientation);
InverseInertiaTensorWorld = _transformInertiaTensor(Orientation, InverseInertiaTensor, TransformMatrix);
}
static Matrix3 _transformInertiaTensor(Quaternion q, Matrix3 iitBody, Matrix4 rotmat)
{
float t4 = rotmat[0] * iitBody[0] + rotmat[1] * iitBody[3] + rotmat[2] * iitBody[6];
float t9 = rotmat[0] * iitBody[1] + rotmat[1] * iitBody[4] + rotmat[2] * iitBody[7];
float t14 = rotmat[0] * iitBody[2] + rotmat[1] * iitBody[5] + rotmat[2] * iitBody[8];
float t28 = rotmat[4] * iitBody[0] + rotmat[5] * iitBody[3] + rotmat[6] * iitBody[6];
float t33 = rotmat[4] * iitBody[1] + rotmat[5] * iitBody[4] + rotmat[6] * iitBody[7];
float t38 = rotmat[4] * iitBody[2] + rotmat[5] * iitBody[5] + rotmat[6] * iitBody[8];
float t52 = rotmat[8] * iitBody[0] + rotmat[9] * iitBody[3] + rotmat[10] * iitBody[6];
float t57 = rotmat[8] * iitBody[1] + rotmat[9] * iitBody[4] + rotmat[10] * iitBody[7];
float t62 = rotmat[8] * iitBody[2] + rotmat[9] * iitBody[5] + rotmat[10] * iitBody[8];
Matrix3 iitWorld = Matrix3.Identity;
iitWorld[0] = t4 * rotmat[0] + t9 * rotmat[1] + t14 * rotmat[2];
iitWorld[1] = t4 * rotmat[4] + t9 * rotmat[5] + t14 * rotmat[6];
iitWorld[2] = t4 * rotmat[8] + t9 * rotmat[9] + t14 * rotmat[10];
iitWorld[3] = t28 * rotmat[0] + t33 * rotmat[1] + t38 * rotmat[2];
iitWorld[4] = t28 * rotmat[4] + t33 * rotmat[5] + t38 * rotmat[6];
iitWorld[5] = t28 * rotmat[8] + t33 * rotmat[9] + t38 * rotmat[10];
iitWorld[6] = t52 * rotmat[0] + t57 * rotmat[1] + t62 * rotmat[2];
iitWorld[7] = t52 * rotmat[4] + t57 * rotmat[5] + t62 * rotmat[6];
iitWorld[8] = t52 * rotmat[8] + t57 * rotmat[9] + t62 * rotmat[10];
return iitWorld;
}
void _checkInverseInertiaTensor(Matrix3 InverseInertiaTensor)
{
}
public void SetInertiaTensor(Matrix3 inertiaTensor)
{
InverseInertiaTensor.SetInverse(inertiaTensor);
_checkInverseInertiaTensor(InverseInertiaTensor);
}
internal Vector3 calculateFrictionImpulse(Matrix3[] inverseInertiaTensor)
{
Vector3 impulseContact = new Vector3();
float inverseMass = Bodies[0].InverseMass;
Matrix3 impulseToTorque = Matrix3.Identity;
impulseToTorque.SetSkewSymmetric(RelativeContactPosition[0]);
Matrix3 deltaVelWorld = impulseToTorque;
deltaVelWorld *= inverseInertiaTensor[0];
deltaVelWorld *= impulseToTorque;
deltaVelWorld *= -1;
if (Bodies[1] != null)
{
impulseToTorque.SetSkewSymmetric(RelativeContactPosition[1]);
Matrix3 deltaVelWorld2 = impulseToTorque;
deltaVelWorld2 *= inverseInertiaTensor[1];
deltaVelWorld2 *= impulseToTorque;
deltaVelWorld2 *= -1;
deltaVelWorld += deltaVelWorld2;
inverseMass += Bodies[1].InverseMass;
}
Matrix3 deltaVelocity = ContactToWorld.Transpose();
deltaVelocity *= deltaVelWorld;
deltaVelocity *= ContactToWorld;
deltaVelocity[0] += inverseMass;
deltaVelocity[4] += inverseMass;
deltaVelocity[8] += inverseMass;
Matrix3 impulseMatrix = deltaVelocity.Inverse();
Vector3 velKill = new Vector3(DesiredDeltaVelocity, -ContactVelocity.Y, -ContactVelocity.Z);
impulseContact = impulseMatrix.Transform(velKill);
float planarImpulse = MathHelper.Sqrt(impulseContact.Y * impulseContact.Y +
impulseContact.Z * impulseContact.Z
);
if (planarImpulse > impulseContact.X * Friction)
{
impulseContact.Y /= planarImpulse;
impulseContact.Z /= planarImpulse;
impulseContact.X = deltaVelocity[0] +
deltaVelocity[1] * Friction * impulseContact.Y +
deltaVelocity[2] * Friction * impulseContact.Z;
impulseContact.X = DesiredDeltaVelocity / impulseContact.X;
impulseContact.Y *= Friction * impulseContact.X;
impulseContact.Z *= Friction * impulseContact.X;
}
return impulseContact;
}
internal Vector3 calculateFrictionlessImpulse(Matrix3[] inverseInertiaTensor)
{
Vector3 impulseContact = new Vector3();
Vector3 deltaVelWorld = Vector3.Cross(RelativeContactPosition[0], ContactNormal);
deltaVelWorld = inverseInertiaTensor[0].Transform(deltaVelWorld);
deltaVelWorld = Vector3.Cross(deltaVelWorld, RelativeContactPosition[0]);
float deltaVelocity = Vector3.Dot(deltaVelWorld, ContactNormal);
deltaVelocity += Bodies[0].InverseMass;
if (Bodies[1] != null)
{
Vector3 vdeltaVelWorld = Vector3.Cross(RelativeContactPosition[1], ContactNormal);
vdeltaVelWorld = inverseInertiaTensor[1].Transform(vdeltaVelWorld);
vdeltaVelWorld = Vector3.Cross(vdeltaVelWorld, RelativeContactPosition[1]);
deltaVelocity += Vector3.Dot(vdeltaVelWorld, ContactNormal);
deltaVelocity += Bodies[1].InverseMass;
}
impulseContact.X = DesiredDeltaVelocity / deltaVelocity;
impulseContact.Y = 0;
impulseContact.Z = 0;
return impulseContact;
}
//void applyImpulse(Vector3 impulse, RigidBody body, Vector3 velocityChange, Vector3 rotationChange)
//{
//}
internal void applyVelocityChange(Vector3[] velocityChange, Vector3[] rotationChange)
{
Matrix3[] inverseInertiaTensor = new Matrix3[2];
inverseInertiaTensor[0] = Bodies[0].InverseInertiaTensorWorld;
if (Bodies[1] != null)
inverseInertiaTensor[1] = Bodies[1].InverseInertiaTensorWorld;
Vector3 impulseContact;
if (Friction == 0.0f)
{
impulseContact = calculateFrictionlessImpulse(inverseInertiaTensor);
}
else
{
impulseContact = calculateFrictionImpulse(inverseInertiaTensor);
}
Vector3 impulse = ContactToWorld.Transform(impulseContact);
Vector3 impulsiveTorque = Vector3.Cross(RelativeContactPosition[0], impulse);
rotationChange[0] = inverseInertiaTensor[0].Transform(impulsiveTorque);
velocityChange[0] = new Vector3();
velocityChange[0] += (impulse * Bodies[0].InverseMass);
Bodies[0].AddVelocity(velocityChange[0]);
Bodies[0].AddRotation(rotationChange[0]);
if (Bodies[1] != null)
{
Vector3 impulsiveTorque2 = Vector3.Cross(impulse, RelativeContactPosition[1]);
rotationChange[1] = inverseInertiaTensor[1].Transform(impulsiveTorque2);
velocityChange[1] = new Vector3();
velocityChange[1] += (impulse * -Bodies[1].InverseMass);
Bodies[1].AddVelocity(velocityChange[1]);
Bodies[1].AddRotation(rotationChange[1]);
}
}
public static Vector3 Transform(Vector3 vector, Matrix3 matrix)
{
float[] data = matrix;
return new Vector3(
vector.X * data[0] + vector.Y * data[1] + vector.Z * data[2],
vector.X * data[3] + vector.Y * data[4] + vector.Z * data[5],
vector.X * data[6] + vector.Y * data[7] + vector.Z * data[8]
);
}
public void SetInertiaTensor(Matrix3 matrix)
{
inverseInertiaTensor.SetInverse(matrix);
}
void addboat()
{
buoyancy = new Buoyancy(1.0f, 3.0f, 1.6f, 1000, new Disque.Math.Vector3(0, 0.5f, 0));
sail = new Aero(new D.Matrix3(new float[]{ 0,0,0, 0,0,0, 0,0,-1.0f}), new D.Vector3(2.0f, 0, 0), windspeed);
cube = new Cube(new Vector3(0, 30, 0), 3, 3, 3, Color.Blue, GraphicsDevice);
drawables.Add(cube);
body = new RigidBody();
body.SetOrientation(new Disque.Math.Quaternion(1, 0, 0, 0));
body.LinearDrag = 0.8f;
body.AngularDrag = 0.8f;
body.Position = new Disque.Math.Vector3(0, 1.6f, 0);
body.Mass = 200;
D.Matrix3 it = new D.Matrix3();
it.SetBlockInertiaTensor(new D.Vector3(1.5f, 1.5f, 1.5f), 100.0f);
body.BoundingSphere = new D.BoundingSphere(body.Position, 1.5f);
body.SetInertiaTensor(it);
body.CalculateDerivedData();
body.SetAwake();
body.SetCanSleep(true);
body.ForceGenerators.Add(buoyancy);
body.ForceGenerators.Add(sail);
rbw.RigidBodies.Add(body);
}
public Aero(Matrix3 tensor, Vector3 position, Vector3 windspeed)
{
Tensor = tensor;
Position = position;
WindSpeed = windspeed;
}