Vector3 RigidBodyContact::CalculateFrictionlessImpulse(Matrix3x3* inverseInertiaTensor)
{
Vector3 impulseContact;
float deltaVelocity = 0;
for (int i = 0; i < 2; i ++)
{
if (Body[i] != NULL)
{
// Calculate a vector that shows the change in velocity in world space for a unit impulse
// in the direction of the contact normal
Vector3 deltaVelocityWorldspace = m_relativeContactPosition[i].Cross(ContactNormal);
deltaVelocityWorldspace = inverseInertiaTensor[i] * deltaVelocityWorldspace;
deltaVelocityWorldspace = deltaVelocityWorldspace.Cross(m_relativeContactPosition[i]);
// Calculate the change in velocity in contact coordinates
deltaVelocity += deltaVelocityWorldspace.Dot(ContactNormal);
// Add the linear component of velocity change
deltaVelocity += Body[i]->GetInverseMass();
}
}
impulseContact.SetX(m_desiredDeltaVelocity / deltaVelocity);
impulseContact.SetY(0.0f);
impulseContact.SetZ(0.0f);
return impulseContact;
}
Vector3 RigidBodyContact::CalculateLocalVelocity(RigidBody* body, float deltaTime, Vector3 relativeContactPosition)
{
// Calculate velocity of the contact point
Vector3 velocity = body->GetAngularVelocity().Cross(relativeContactPosition);
velocity += body->GetVelocity();
// Convert velocity into contact coordinates
// Contact-to-world is rotation-only, so transpose is its inverse (world-to-contact)
Vector3 velocityContactSpace = m_contactToWorld.Transpose() * velocity;
// Calculate the amount of velocity that is due to forces without reactions
Vector3 accVelocity = body->GetPreviousAcceleration() * deltaTime;
// Calculate the velocity in contact coordinates
accVelocity = m_contactToWorld.Transpose() * accVelocity;
// Ignore any component of acceleration in the contact normal direction -
// we are only interested in planar acceleration
accVelocity.SetX(0);
// Add the planar velocities - if there's enough friction they will be removed
// during velocity resolution
velocityContactSpace += accVelocity;
return velocityContactSpace;
}
Vector3 Vector3::ComponentProduct(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X * other.GetX());
result.SetY(m_Y * other.GetY());
result.SetZ(m_Z * other.GetZ());
return result;
}
Vector3 Vector3::Subtract(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X - other.GetX());
result.SetY(m_Y - other.GetY());
result.SetZ(m_Z - other.GetZ());
return result;
}
Vector3 Vector3::CrossProduct(const Vector3& other) const
{
Vector3 result;
result.SetX( (m_Y * other.GetZ()) - (m_Z * other.GetY()) );
result.SetY( (m_Z * other.GetX()) - (m_X * other.GetZ()) );
result.SetZ( (m_X * other.GetY()) - (m_Y * other.GetX()) );
return result;
}
Vector3 Vector3::Add(const real& scalar) const
{
Vector3 result;
result.SetX(m_X + scalar);
result.SetY(m_Y + scalar);
result.SetZ(m_Z + scalar);
return result;
}
const Vector3 Vector3::operator-(const Vector3 &other) const
{
Vector3 result;
result.SetX(_x - other.GetX());
result.SetY(_y - other.GetY());
result.SetZ(_z - other.GetZ());
return result;
}
Vector3 Vector3::Add(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X + other.GetX());
result.SetY(m_Y + other.GetY());
result.SetZ(m_Z + other.GetZ());
return result;
}
Vector3 Vector3::Multiply(const real& scaler) const
{
Vector3 result;
result.SetX(m_X * scaler);
result.SetY(m_Y * scaler);
result.SetZ(m_Z * scaler);
return result;
}
const Vector3 Vector3::operator+(const Vector3 &other) const
{
Vector3 result;
result.SetX(_x + other.GetX());
result.SetY(_y + other.GetY());
result.SetZ(_z + other.GetZ());
return result;
}
Vector3 Matrix3::GetColumnVector(int i) const
{
Vector3 result;
result.SetX(m_Elems[i]);
result.SetY(m_Elems[i + 3]);
result.SetZ(m_Elems[i + 6]);
return result;
}
//cross product
Vector3 Vector3::crossProduct(const Vector3& v) const
{
Vector3 result;
result.SetX(_y * v.GetZ() - v.GetY() * _z);
result.SetY(_x * v.GetZ() - v.GetX() * _z);
result.SetZ(_x * v.GetY() - v.GetX() * _y);
return result;
}
//calculate the magnitude for normilisasion
const Vector3 Vector3::operator/(const float &mag) const
{
Vector3 result;
if(mag == 0)
{
result.SetX(0);
result.SetY(0);
result.SetZ(0);
}
else
{
result.SetX(_x / mag);
result.SetY(_y / mag);
result.SetZ(_z / mag);
}
return result;
}
Vector3 Vector3::Cross(Vector3 other)
{
Vector3 result;
result.SetX((mY * other.Z()) - (other.Y() * mZ));
result.SetY((mZ * other.X()) - (other.Z() * mX));
result.SetZ((mX * other.Y()) - (other.X() * mY));
return result;
}
Vector3 Vector3::Normalized()
{
Vector3 result;
float length = Length();
result.SetX(mX/length);
result.SetY(mY/length);
result.SetZ(mZ/length);
return result;
}
void VectorOp::VectorialProduct(const Vector3& first, const Vector3& second, Vector3& result)
{
//float X = (p3[1] * s3[2]) - (p3[2] * s3[1]);
//float Y = (p3[2] * s3[0]) - (p3[0] * s3[2]);
//float Z = (p3[0] * s3[1]) - (p3[1] * s3[0]);
float X = (first.getY() * second.getZ()) - (first.getZ() * second.getY());
float Y = (first.getZ() * second.getX()) - (first.getX() * second.getZ());
float Z = (first.getX() * second.getY()) - (first.getY() * second.getX());
result.SetX(X);
result.SetY(Y);
result.SetZ(Z);
}
Vector3 Quaternion::ToEuler() const {
float q0 = m_w;
float q1 = m_y;
float q2 = m_x;
float q3 = m_z;
Vector3 radAngles;
radAngles.SetY(atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (powf(q1, 2.0f) + powf(q2, 2.0f))));
radAngles.SetX(asinf(2.0f * (q0 * q2 - q3 * q1)));
radAngles.SetZ(atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (powf(q2, 2.0f) + powf(q3, 2.0f))));
return radAngles;
}
Vector3 Matrix3::Multiply(const Vector3& other) const
{
Vector3 result;
//@REF: Transform the matrix by the vector
result.SetX((other.GetX()*m_Elems[0]) +
(other.GetY()*m_Elems[1]) +
(other.GetZ()*m_Elems[2]));
result.SetY((other.GetX()*m_Elems[3]) +
(other.GetY()*m_Elems[4]) +
(other.GetZ()*m_Elems[5]));
result.SetZ((other.GetX()*m_Elems[6]) +
(other.GetY()*m_Elems[7]) +
(other.GetZ()*m_Elems[8]));
return result;
}
bool BasicVectorTests()
{
// This doesn't really test anything yet, just shows various usage patterns (and ensures they all compile)
Vector3 v;
v.x = 3;
v.y = 4;
v.z = 5;
v.SetX(1).SetY(2).SetZ(3);
Vector3 v1(v);
v1.Set(4, 5, 6);
Vector3 v2 = v.Cross(v1.SetY(144));
float d = v2.Dot(v1);
v2 = Vector3::Cross(v, v1);
float f = Vector3::Dot(v2, v);
if (v1.Normalize().Length() != 1)
return false;
Vector3 v3 = Vector3::Forward();
Vector3 v4 = v1 + v2;
Vector3 v5 = v4 - v;
v3 += v2;
v3 -= v;
v4 = v5 * f;
v3 = d * v4;
v2 *= f;
v *= d;
const Vector2 vec2;
return true;
}
void FloatingCamera::KeyboardControls()
{
bool temp = input_->IsKeyDown('W');
if (input_->IsKeyDown('W'))
{
Vector3 temp;
temp.SetX(forward_vec_.GetX() * (*dt_) * forward_speed_);
temp.SetY(forward_vec_.GetY() * (*dt_) * forward_speed_);
temp.SetZ(forward_vec_.GetZ() * (*dt_) * forward_speed_);
position_ = position_ + temp;
}
if (input_->IsKeyDown('S'))
{
Vector3 temp;
temp.SetX(forward_vec_.GetX() * (*dt_) * backward_speed_);
temp.SetY(forward_vec_.GetY() * (*dt_) * backward_speed_);
temp.SetZ(forward_vec_.GetZ() * (*dt_) * backward_speed_);
position_ = position_ - temp;
}
if (input_->IsKeyDown('A'))
{
right_vec_ = forward_vec_.Cross(up_vec_);
Vector3 temp;
temp.SetX(right_vec_.GetX() * (*dt_) * strafe_speed_);
temp.SetY(right_vec_.GetY() * (*dt_) * strafe_speed_);
temp.SetZ(right_vec_.GetZ() * (*dt_) * strafe_speed_);
position_ = position_ - temp;
}
if (input_->IsKeyDown('D'))
{
right_vec_ = forward_vec_.Cross(up_vec_);
Vector3 temp;
temp.SetX(right_vec_.GetX() * (*dt_) * strafe_speed_);
temp.SetY(right_vec_.GetY() * (*dt_) * strafe_speed_);
temp.SetZ(right_vec_.GetZ() * (*dt_) * strafe_speed_);
position_ = position_ + temp;
}
if (input_->IsKeyDown('C'))
{
Vector3 temp;
temp.SetX(up_vec_.GetX() * (*dt_) * vertical_speed_);
temp.SetY(up_vec_.GetY() * (*dt_) * vertical_speed_);
temp.SetZ(up_vec_.GetZ() * (*dt_) * vertical_speed_);
position_ = position_ + temp;
}
if (input_->IsKeyDown('Z'))
{
Vector3 temp;
temp.SetX(up_vec_.GetX() * (*dt_) * vertical_speed_);
temp.SetY(up_vec_.GetY() * (*dt_) * vertical_speed_);
temp.SetZ(up_vec_.GetZ() * (*dt_) * vertical_speed_);
position_ = position_ - temp;
}
if (input_->IsKeyDown('E'))
{
Vector3 temp;
temp.SetX(0);
temp.SetY(1 * (*dt_) * vertical_speed_);
temp.SetZ(0);
position_ = position_ + temp;
}
if (input_->IsKeyDown('Q'))
{
Vector3 temp;
temp.SetX(0);
temp.SetY(1 * (*dt_) * vertical_speed_);
temp.SetZ(0);
position_ = position_ - temp;
}
}
Vector3 RigidBodyContact::CalculateFrictionImpulse(Matrix3x3* inverseInertiaTensor)
{
float inverseMass = Body[0]->GetInverseMass();
// The equivalent of a cross-product in matrices is multiplication by a skew-symmetric matrix.
// We calculate the matrix for converting between linear and angular quantities.
Matrix3x3 impulseToTorque;
impulseToTorque.SetSkewSymmetric(m_relativeContactPosition[0]);
// Calculate the matrix to convert contact impulse to change in velocity in world coordinates
Matrix3x3 deltaVelocityWorldspace = impulseToTorque;
deltaVelocityWorldspace = deltaVelocityWorldspace * inverseInertiaTensor[0];
deltaVelocityWorldspace = deltaVelocityWorldspace * impulseToTorque;
deltaVelocityWorldspace *= -1;
if (Body[1] != NULL)
{
impulseToTorque.SetSkewSymmetric(m_relativeContactPosition[1]);
// Calculate the matrix to convert contact impulse to change in velocity in world coordinates
Matrix3x3 deltaVelocityWorldspace2 = impulseToTorque;
deltaVelocityWorldspace2 = deltaVelocityWorldspace2 * inverseInertiaTensor[1];
deltaVelocityWorldspace2 = deltaVelocityWorldspace2 * impulseToTorque;
deltaVelocityWorldspace2 *= -1;
// Add to the total delta velocity and inverse mass
deltaVelocityWorldspace = deltaVelocityWorldspace + deltaVelocityWorldspace2;
inverseMass += Body[1]->GetInverseMass();
}
// Perform a change of basis to convert into contact coordinates
Matrix3x3 deltaVelocity = m_contactToWorld.Transpose();
deltaVelocity = deltaVelocity * deltaVelocityWorldspace; // TODO multiplication order?
deltaVelocity = deltaVelocity * m_contactToWorld;
// Add in the linear velocity change
deltaVelocity[0][0] += inverseMass;
deltaVelocity[1][1] += inverseMass;
deltaVelocity[2][2] += inverseMass;
// Invert to get the impulse needed per unit velocity
Matrix3x3 impulseMatrix = deltaVelocity.Inverse();
// Find the target velocities to kill
Vector3 velKill(m_desiredDeltaVelocity, -m_contactVelocity.y(), -m_contactVelocity.z());
// Find the impulse to kill target velocities
Vector3 impulseContact = impulseMatrix * velKill;
// Check for exceeding friction
float planarImpulse = sqrtf(impulseContact.y()*impulseContact.y() + impulseContact.z()*impulseContact.z());
if (planarImpulse > impulseContact.x() * Friction)
{
// We need to use dynamic friction
impulseContact.SetY(impulseContact.y() / planarImpulse);
impulseContact.SetZ(impulseContact.z() / planarImpulse);
impulseContact.SetX(deltaVelocity[0][0] +
deltaVelocity[0][1] * Friction*impulseContact.y() +
deltaVelocity[0][2] * Friction*impulseContact.z());
impulseContact.SetX(m_desiredDeltaVelocity / impulseContact.x());
impulseContact.SetY(impulseContact.y()*Friction*impulseContact.x());
impulseContact.SetZ(impulseContact.z()*Friction*impulseContact.x());
}
return impulseContact;
}