//Fimd the angle between in Radians
float Vector3::AngleBetweenRadians( Vector3 const &_rhs)
{
Vector3 c = xyz - _rhs;
float magA = Magnitude();
float magB = _rhs.Magnitude();
float magC = c.Magnitude();
if( Math::isZero(magA) || Math::isZero(magB) || Math::isZero(magC) )
return 0;
return acosf( Dot(_rhs) / (magA * magB) );
}
bool AddShadow(Light light, Vector3& color, Vector3& pos, void* shape)
{
Vector3 lightLoc = Vector3(light.position[0], light.position[1], light.position[2]);
Vector3 lightDir = lightLoc - pos;
double lightMagnitude = lightDir.Magnitude();
Normalize(lightDir);
Ray ray = Ray(pos, lightDir, lightMagnitude);
Vector3 furthestIntersect = Vector3::Zero();
Vector3 intersect = Vector3::Zero();
Vector3 normal = Vector3::Zero();
bool collision = false;
for (int i = 0; i < num_spheres; i++)
{
if (ray.IntersectsSphere(spheres[i], intersect, normal) && &spheres[i] != shape)
{
collision = true;
}
}
for (int i = 0; i < num_triangles; i++)
{
if (ray.IntersectsTriangle(triangles[i], intersect, normal) && &triangles[i] != shape)
{
collision = true;
}
}
if (collision && (furthestIntersect - pos).Magnitude() > 0.5)
{
return true;
}
else
{
return false;
}
}
Vector4 Shading(const Vector3 &pos, const Vector3 &normal, const Material &mat, const Light &light)
{
// 光线方向，由物体指向光源
Vector3 L = light.position - pos;
// 光源距离
float dist = L.Magnitude();
L.SetNormalize();
// 光强，按点光源计算
float I = 1.0f / (light.attenuation0 + light.attenuation1 * dist + light.attenuation2 * dist * dist);
// 环境光
Vector4 amb_color = light.ambient * mat.ambient * I;
amb_color = clamp(amb_color, 0.0f, 1.0f);
Vector3 N = normal;
N.SetNormalize();
// 顶点法线
float light_angle = DotProduct(L, N);
light_angle = max_t(light_angle, 0.0f);
// 漫反射
Vector4 diff_color = light.diffuse * mat.diffuse * light_angle * I;
diff_color = clamp(diff_color, 0.0f, 1.0f);
// 观察方向（相机空间）
Vector3 V = -pos;
V.SetNormalize();
// 半角向量
Vector3 H = L + V;
H.SetNormalize();
// 镜面反射
float view_angle = DotProduct(H, N);
view_angle = max_t(view_angle, 0.0f);
Vector4 spec_color = light.specular * mat.specular * powf(view_angle, mat.specular.w) * I;
spec_color = clamp(spec_color, 0.0f, 1.0f);
Vector4 ret = amb_color + diff_color + spec_color;
return ret;
}
Vector3 Centripetal(const Point3& origin, const Point3& pos, const Vector3& vel, float mass)
{
const Vector3 dir = origin - pos;
float radius = dir.Magnitude();
float mag = mass * vel.MagnitudeSq() / radius;
return dir.Normalise() * mag;
}
//Find the angle between in Degrees
float Vector3::AngleBetweenDegrees( Vector3 const &_rhs)
{
Vector3 c = xyz - _rhs;
float magA = Magnitude();
float magB = _rhs.Magnitude();
float magC = c.Magnitude();
if( Math::isZero(magA) || Math::isZero(magB) || Math::isZero(magC) )
return 0;
float resultInRadians = acosf( Dot(_rhs) / (magA * magB) );
return Math::RadiansToDegrees(resultInRadians) ;
}
GLfloat Vector3::Angle ( const Vector3& myVector ) const
{
GLfloat dot = this->DotProduct(myVector);
GLfloat a = this->Magnitude();
GLfloat b = myVector.Magnitude();
return( dot/(a*b) );
}
TEST(VectorTest, Magnitude){
//test default
Vector3<double> v;
EXPECT_EQ(0.0, v.Magnitude());
v = Vector3<double>(1.0, 2.0, 3.0);
EXPECT_EQ(sqrt(14.0), v.Magnitude());
v.x = 10.0;
EXPECT_EQ(sqrt(113.0), v.Magnitude());
v.y = 5;
EXPECT_EQ(sqrt(134.0), v.Magnitude());
v.z = 8;
EXPECT_EQ(sqrt(189.0), v.Magnitude());
}
float AngleBetweenRadians(Vector3 const &_lhs, Vector3 const &_rhs)
{
Vector3 c = _lhs - _rhs;
float magA = _lhs.Magnitude();
float magB = _rhs.Magnitude();
float magC = c.Magnitude();
if( Math::isZero(magA) || Math::isZero(magB) || Math::isZero(magC) )
return 0;
float resultInRadians = acosf( Dot(_lhs,_rhs) / (magA * magB) );
return resultInRadians;
}
DerivVector Centripetal(const StateVector& v, const Vector3& origin, float mass)
{
const Vector3 vel = v.mom / mass;
const Vector3 dir = origin - v.pos;
float radius = dir.Magnitude();
float mag = mass * vel.MagnitudeSq() / radius;
return DerivVector(vel, dir * mag);
}
//Get the Normalization of the passed in Vector
Vector3 Normalize( Vector3 _vector )
{
float fMagnitude = _vector.Magnitude();
if (Math::isZero(fMagnitude) )
fMagnitude = 1.0f;
return Vector3(_vector.xyz / fMagnitude);
}
void V3Normalize( Vector3 &v )
{
float len = v.Magnitude();
if ( len > 0.0f )
len = 1.0f / len;
else
len = 0.0f;
v.x *= len;
v.y *= len;
v.z *= len;
}
//Normalize
Vector3 Vector3::NormalizeReturn(){
Vector3 vect;
vect.x = x;
vect.y = y;
vect.z = z;
float len = vect.Magnitude();
vect.x/=len;
vect.y/=len;
vect.z/=len;
return vect;
}
Vector3 Entity::GetScale()
{
//get x axis
Vector3 xAxis;
xAxis.x = m_oLocalTransform.m_afM[0][0];
xAxis.y = m_oLocalTransform.m_afM[0][1];
xAxis.z = 0;
//get y axis
Vector3 yAxis;
yAxis.x = m_oLocalTransform.m_afM[1][0];
yAxis.y = m_oLocalTransform.m_afM[1][1];
yAxis.z = 0;
//calculate scale
Vector3 ret;
ret.x = xAxis.Magnitude();
ret.y = yAxis.Magnitude();
ret.z = 0;
return ret; // return scale
}
// DRAG GENERATOR - OVERLOADED UpdateForce - Applies drag.
void ParticleDrag::UpdateForce(Particle* particle, marb duration) {
Vector3 force;
force = particle->GetVelocity();
// Calculate total drag coefficient
marb dragCoeff = force.Magnitude();
dragCoeff = k1 * dragCoeff + k2 * dragCoeff * dragCoeff;
// Calculate and apply final force
force.Normalize();
force *= -dragCoeff;
particle->AddForce(force);
}
void Camera::RenderPath(Scene &scn, int n) {
RayTrace rayTrace(scn);
for (int y = 0; y < _img.YRes; y++) {
for (int x = 0; x < _img.XRes; x++) {
// compute the primary ray
Vector3 cy;
cy.Cross(_worldMatrix.c, _worldMatrix.b);
Vector3 cx = cy / pow(cy.Magnitude(), 2);
cy.Cross(cx, _worldMatrix.c);
float hfov = 2.f * atanf(_aspect * tanf(_verticalFOV / 2.f));
float cw = 2.f * tanf(hfov/2.f);
float ch = cw / _aspect;
Ray ray;
ray.Origin = _worldMatrix.d;
ray.Direction =
_worldMatrix.c + ((float)(x + 0.5f) / (float)_img.XRes - 0.5f) * cw * cx +
((float)(y + 0.5f)/(float)_img.YRes - 0.5f) * ch * cy;
ray.type = Ray::PRIMARY;
// shoot the primary ray
Intersection hit;
if (x > 124 && x <= 140 && y == _img.YRes - 495 ) {
// Color white = Color::WHITE;
// std::cout << "debug pixel" << std::endl;
// _img.SetPixel(x, y, white.ToInt());
// continue;
}
rayTrace.TraceRay(ray, hit);
if (n != -1) {
Color c;
c.FromInt(_img.GetPixel(x, y));
Color avg = Color::BLACK;
avg.AddScaled(c, n - 1);
avg.Add(hit.Shade);
avg.Scale(1.0f / n);
_img.SetPixel(x, y, avg.ToInt());
}
else _img.SetPixel(x, y, hit.Shade.ToInt());
}
}
}
// Calculate bounding sphere using average position of the points. Better fit but slower.
static void CalcCenteredSphere(const vector<SkinWeight> & n, const vector<Vector3>& vertices, Vector3& center, float& radius)
{
size_t nv = n.size();
Vector3 sum;
for (size_t i=0; i<nv; ++i)
sum += vertices[ n[i].index ];
center = sum / float(nv);
radius = 0.0f;
for (size_t i=0; i<nv; ++i){
Vector3 diff = vertices[ n[i].index ] - center;
float mag = diff.Magnitude();
radius = max(radius, mag);
}
}
CMatrix4 CMatrix4::RotationAxis(Vector3 axis, float angle)
{
HASENPFOTE_ASSERT_MSG(almost_equals(1.0f, axis.Magnitude(), 1), "Axis is not an unit vector.");
const float x = axis.GetX();
const float y = axis.GetY();
const float z = axis.GetZ();
const float s = std::sin(angle);
const float c = std::cos(angle);
const float vers = 1.0f - c;
return CMatrix4(
x*x*vers+c, x*y*vers-z*s, x*z*vers+y*s, 0.0f,
x*y*vers+z*s, y*y*vers+c, y*z*vers-x*s, 0.0f,
x*z*vers-y*s, y*z*vers+x*s, z*z*vers+c, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
//ANCHORED SPRING GENERATOR - OVERLOADED UpdateForce - Applies anchored spring.
void ParticleAnchoredSpring::UpdateForce(Particle* particle, marb duration){
// Calculate the vector of the spring.
Vector3 force;
//particle->GetPosition(&force); //This function seems to do the same as setPosition
particle->SetPosition(force); // This is in the place of GetPosition(&force) which is commented out, keep an eye
force -= *anchor;
// Calculate the magnitude of the force.
marb magnitude = force.Magnitude();
magnitude = marb_abs(magnitude - restLength);
magnitude *= springConstant;
// Calculate the final force and apply it.
force.Normalize();
force *= -magnitude;
particle->AddForce(force);
}
void Spring::UpdateForce(RigidBody* body, marb duration) {
// Calculate the two ends in world space
Vector3 lws = body->GetPointInWorldSpace(connectionPoint);
Vector3 ows = other->GetPointInWorldSpace(otherConnectionPoint);
// Calculate the vector of the spring
Vector3 force = lws - ows;
// Calculate the magnitude of the force
marb magnitude = force.Magnitude();
magnitude = marb_abs(magnitude - restLength);
magnitude *= springConstant;
// Calculate the final force and apply it
force.Normalize();
force *= -magnitude;
body->AddForceAtPoint(force, lws);
}
//BUNGEE GENERATOR - OVERLOADED UpdateForce - Applies bungee forces.
void ParticleBungee::UpdateForce(Particle* particle, marb duration)
{
// Calculate the vector of the spring.
Vector3 force;
//particle->GetPosition(&force); //This function seems to do the same as setPosition
particle->SetPosition(force); // This is in the place of GetPosition(&force) which is commented out, keep an eye
force -= other->GetPosition();
// Check if the bungee is compressed.
marb magnitude = force.Magnitude();
if (magnitude <= restLength) return;
// Calculate the magnitude of the force.
magnitude = springConstant * (restLength - magnitude);
// Calculate the final force and apply it.
force.Normalize();
force *= -magnitude;
particle->AddForce(force);
}
float CollapseVertex::ComputeCost(CollapseVertex * v)
{
// if we collapse edge uv by moving u to v then how
// much different will the model change, i.e. how much "error".
// Texture, vertex normal, and border vertex code was removed
// to keep this demo as simple as possible.
// The method of determining cost was designed in order
// to exploit small and coplanar regions for
// effective polygon reduction.
// Is is possible to add some checks here to see if "folds"
// would be generated. i.e. normal of a remaining face gets
// flipped. I never seemed to run into this problem and
// therefore never added code to detect this case.
assert(this->IsNeighbor(v));
float edgelength = 1.0f;
if(this->parent->Arguments.useedgelength)
{
Vector3 len = (v->Vert.Position - this->Vert.Position);
edgelength = len.Magnitude();
}
// prevent impossible movements
if(this->Neighbors.size() == v->Neighbors.size()) {
int i;
for(i = 0; i < (int)this->Neighbors.size(); i++) {
if(this->Neighbors[i] == v) {
continue;
}
if(!v->IsNeighbor(this->Neighbors[i])) {
break;
}
}
// they share the same neighbors, alert!
if(i == (int)this->Neighbors.size()) {
return 999999.9f;
}
}
float curvature = 0.001f;
// find the "sides" triangles that are on the edge uv
std::vector<CollapseTriangle *> sides;
for(int i = 0; i < (int)this->Faces.size(); i++)
{
if(v->IsFace(this->Faces[i])) {
sides.push_back(this->Faces[i]);
}
}
if(this->parent->Arguments.usecurvature) {
// use the triangle facing most away from the sides
// to determine our curvature term
for(int i = 0; i < (int)this->Faces.size(); i++)
{
float mincurv = 1; // curve for face i and closer side to it
for(int j = 0; j < (int)sides.size(); j++)
{
// use dot product of face normals. '^' defined in vector
float dotprod =
this->Faces[i]->normal.DotProduct(
sides[j]->normal);
mincurv = min(mincurv, (1.002f - dotprod) / 2.0f);
}
curvature = max(curvature, mincurv);
}
}
// check for border to interior collapses
if(this->IsBorder()) {
curvature *= WEIGHT_BORDER;
}
// LockDown border edges (i.e. try not to touch them at all)
// this is done by locking any border vertex.
// i.e. even if uv isn't a border edge we dont want
// to collapse u to any vertex
// if u is on a border
if(this->parent->Arguments.protecttexture) {
// check for texture seam ripping
if(!this->IsSameUV(v)) {
curvature = 1;
}
}
if(this->parent->Arguments.protectvc){
// adding support for 2nd pass or vert color here:
// check for vert color (or uvw2) seam ripping
if(!(*(DWORD*)this->Vert.Diffuse == *(DWORD*)v->Vert.Diffuse)) {
curvature = 1;
}
}
if(this->parent->Arguments.lockborder && this->IsBorder()) {
curvature = 9999.9f;
}
return edgelength * curvature;
}
float Vector3::Magnitude(const Vector3 &v)
{
return v.Magnitude();
}
/* AngleBetween */
float Vector3::AngleBetween(Vector3& vec) {
return acos(DotProduct(vec)/(Magnitude() * vec.Magnitude()));
}
void Contact::ApplyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], marb penetration) {
const marb angularLimit = (marb)0.2f;
marb angularMove[2];
marb linearMove[2];
marb totalInertia = 0;
marb linearInertia[2];
marb angularInertia[2];
// We need to work out the inertia of each object in the direction
// of the contact normal, due to angular inertia only.
for (unsigned i = 0; i < 2; i++) if (body[i]) {
Matrix3 inverseInertiaTensor;
body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);
// Use the same procedure as for calculating frictionless
// velocity change to work out the angular inertia.
Vector3 angularInertiaWorld =
relativeContactPosition[i] % contactNormal;
angularInertiaWorld =
inverseInertiaTensor.Transform(angularInertiaWorld);
angularInertiaWorld =
angularInertiaWorld % relativeContactPosition[i];
angularInertia[i] =
angularInertiaWorld * contactNormal;
// The linear component is simply the inverse mass
linearInertia[i] = body[i]->GetInverseMass();
// Keep track of the total inertia from all components
totalInertia += linearInertia[i] + angularInertia[i];
// We break the loop here so that the totalInertia value is
// completely calculated (by both iterations) before
// continuing.
}
// Loop through again calculating and applying the changes
for (unsigned i = 0; i < 2; i++) if (body[i]) {
// The linear and angular movements required are in proportion to
// the two inverse inertias.
marb sign = (i == 0)?1:-1;
angularMove[i] =
sign * penetration * (angularInertia[i] / totalInertia);
linearMove[i] =
sign * penetration * (linearInertia[i] / totalInertia);
// To avoid angular projections that are too great (when mass is large
// but inertia tensor is small) limit the angular move.
Vector3 projection = relativeContactPosition[i];
projection.AddScaledVector(
contactNormal,
-relativeContactPosition[i].ScalarProduct(contactNormal)
);
// Use the small angle approximation for the sine of the angle (i.e.
// the magnitude would be sine(angularLimit) * projection.magnitude
// but we approximate sine(angularLimit) to angularLimit).
marb maxMagnitude = angularLimit * projection.Magnitude();
if (angularMove[i] < -maxMagnitude) {
marb totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
} else if (angularMove[i] > maxMagnitude) {
marb totalMove = angularMove[i] + linearMove[i];
angularMove[i] = maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
// We have the linear amount of movement required by turning
// the rigid body (in angularMove[i]). We now need to
// calculate the desired rotation to achieve that.
if (angularMove[i] == 0) {
// Easy case - no angular movement means no rotation.
angularChange[i].Clear();
} else {
// Work out the direction we'd like to rotate in.
Vector3 targetAngularDirection =
relativeContactPosition[i].VectorProduct(contactNormal);
Matrix3 inverseInertiaTensor;
body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);
// Work out the direction we'd need to rotate to achieve that
angularChange[i] =
inverseInertiaTensor.Transform(targetAngularDirection) *
(angularMove[i] / angularInertia[i]);
}
// Velocity change is easier - it is just the linear movement
// along the contact normal.
linearChange[i] = contactNormal * linearMove[i];
// Now we can start to apply the values we've calculated.
// Apply the linear movement
Vector3 pos;
body[i]->GetPosition(&pos);
pos.AddScaledVector(contactNormal, linearMove[i]);
body[i]->SetPosition(pos);
// And the change in orientation
Quaternion q;
body[i]->GetOrientation(&q);
q.AddScaledVector(angularChange[i], ((marb)1.0));
body[i]->SetOrientation(q);
// We need to calculate the derived data for any body that is
// asleep, so that the changes are reflected in the object's
// data. Otherwise the resolution will not change the position
// of the object, and the next collision detection round will
// have the same penetration.
if (!body[i]->GetAwake()) body[i]->CalculateDerivedData();
}
}
double Magnitude( Vector3 const& i_vector )
{
return i_vector.Magnitude();
}
public: static Vector3 Normalize (Vector3 &A) {
return A *= float
(float (1) /
A.Magnitude());
}
int main()
{
cout << "---------------------------Matrix 3--------------------------------" << endl;
Matrix3 TranslationXY;
TranslationXY = Mat3.m_TranslationXY(2, 2);
cout << TranslationXY;
cout << endl;
cout << "--------------------------MATRIX 4 --------------------------------" << endl;
Matrix4 RotationX;
RotationX = Mat4.m_RotationX(3);
cout << RotationX;
cout << endl;
Matrix4 RotationY;
RotationY = Mat4.m_RotationY(2);
cout << RotationY;
cout << endl;
Matrix4 RotationZ;
RotationZ = Mat4.m_RotationZ(2);
cout << RotationZ;
cout << endl;
Matrix4 TranslationXYZ;
TranslationXYZ = Mat4.m_TranslationXYZ(2, 2, 2);
cout << TranslationXYZ;
cout << endl;
Matrix4 mat;
mat = Mat4.m_OrthoProjection(2,2,2,2,2,2);
cout << mat;
cout << endl;
Matrix4 Identity;
mat = Mat4.m_CreateIdentity();
cout << Identity;
cout <<endl;
cout << "---------------------------COMMON MATH--------------------------------" << endl;
cout << ComMath.Pow2(2, 2)<< endl;
cout << ComMath.m_RadianConvert(360)<< endl;
cout << ComMath.m_degreeConvert(6) << endl;
Vect3.x = 2;
Vect3.y = 2;
Vect3.z =2;
Vect33.x = 4;
Vect33.y = 4;
Vect33.z = 4;
cout << ComMath.m_Lerp(Vect3, Vect33, 2) << endl;
cout << "---------------------------VECTOR 3--------------------------------" << endl;
Vect3.x = 2;
Vect3.y =2;
Vect3.z =2;
cout << Vect3.Magnitude()<<endl;
Vect3.x = 2;
Vect3.y =2;
Vect3.z =2;
cout << Vect33.Normalise(Vect3)<<endl;
Vect3.x = 2;
Vect3.y =2;
Vect3.z = 2;
cout << Vect33.GetNormal(Vect3) << endl;
Vect3.x = 2;
Vect3.y = 2;
Vect3.z =2;
cout <<Vect33.DotProduct(Vect3) << endl;
Vect3.x = 4;
Vect3.y = 4;
Vect3.z = 4;
Vect33.x =4;
Vect33.y = 4;
Vect3.z = 4;
cout << Vect3.EulerAngle(Vect3, Vect33)<<endl;
Vect3.x = 2;
Vect3.y = 2;
Vect3.z = 2;
Vect33.x = 2;
Vect33.y =2;
Vect33.z = 2;
cout << Vect3.CrossProduct(Vect3, Vect33)<<endl;
Matrix3 Transform;
Transform = Mat3;
Vect3.x =2;
Vect3.y = 2;
Vect3.z = 2;
cout << Vect3.m_TransformVector3(Mat3)<<endl;
Matrix3 tempM;
tempM = Mat3;
Vect3.x = 2;
Vect3.y = 2;
Vect3.z = 2;
cout << Vect3.Scale(Mat3);
cout << endl;
cout << "---------------------------VECTOR 4--------------------------------" << endl;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_Magnitude()<<endl;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_GetNormal(Vect4)<<endl;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_Normalise(Vect4) <<endl;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_DotProduct(Vect4) << endl;
cout << Vect4.m_RGBconverter(0xFFFFFFFF)<<endl;
Matrix4 Transform2;
Transform2 = Mat4;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_TransformPoint(Mat4) << endl;
Matrix4 Transform3;
Transform3 = Mat4;
Vect4.x = 2;
Vect4.y =2;
Vect4.z =2;
Vect4.w = 2;
cout << Vect4.m_TransformVector4(Vect4, Mat4);
cout << endl;
Matrix4 mat4;
mat4 = Mat4;
Vect4.x = 2;
Vect4.y = 2;
Vect4.z = 2;
Vect4.w = 2;
cout << Vect4.Scale(Mat4);
cout << endl;
cout << "---------------------------VECTOR 2--------------------------------" << endl;
Vectors Point;
Point.x = 2;
Point.y =2;
cout << V2.pointSubtract(Point, 2) << endl;
Vectors Point2;
Point2.x = 2;
Point2.y =2;
cout << V2.pointAdd(Point2, 2)<<endl;
Vectors Point3;
Point3.x = 2;
Point3.y =2;
cout << V2.multiplyScalar(Point3, 2) << endl;
Vectors Point4;
Point4.x = 2;
Point4.y =2;
cout << V2.getMagnitude(Point4)<<endl;
Vectors Point5;
Point5.x = 2;
Point5.y =2;
cout<<V2.getNormal(Point5)<<endl;
getchar();
return 0;
}
Vector3 Vector3::Normalize(const Vector3& argSource)
{
return argSource / argSource.Magnitude();
};
void RigidBodyContact::ApplyPositionChange(Vector3* linearChange, Vector3* angularChange, float penetration)
{
float angularLimit = 5.0f;
float angularMove[2];
float linearMove[2];
float linearInertia[2];
float angularInteria[2];
float totalInertia = 0.0f;
// Calculate the angular inertia using the same process as for calculating frictionless velocity change
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
Vector3 angularInertiaWorld = m_relativeContactPosition[i].Cross(ContactNormal);
angularInertiaWorld = angularInertiaWorld*inverseInertiaTensor; // TODO physics: is order correct here?
angularInertiaWorld = angularInertiaWorld.Cross(m_relativeContactPosition[i]);
angularInteria[i] = angularInertiaWorld.Dot(ContactNormal);
// The linear inertia component is just the inverse mass
linearInertia[i] = Body[i]->GetInverseMass();
// Combine angular and linear to calculate total
totalInertia += linearInertia[i] + angularInteria[i];
}
}
// Calculate and apply changes
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
// The linear and angular movements required are in proportion to the two inverse inertias
float sign = (i == 0) ? 1.f : -1.f;
angularMove[i] = sign * penetration * (angularInteria[i] / totalInertia);
linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia);
// To avoid angular projections that are too great (when mass is large but inertia
// tensor is small), limit the angular move
Vector3 projection = m_relativeContactPosition[i];
projection += ContactNormal * -m_relativeContactPosition[i].Dot(ContactNormal);
// Use the small angle approximation for the sine of the angle (i.e. the magnitude would be
// sin(angularLimit) * projection.magnitude, but we approximate sin(angularLimit) to angularLimit.
float maxMagnitude = angularLimit * projection.Magnitude();
if (angularMove[i] < -maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
else if (angularMove[i] > maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
// We have the linear amount of movement required by turning the rigid body (angularMove[i]).
// We now need to calculate the desired rotation to achieve that.
if (Approximately(angularMove[i], 0.f))
{
// Easy case - no angular movement means no rotation.
angularChange[i] = Vector3::Zero;
}
else
{
// Work out the direction we'd like to rotate in
Vector3 targetAngularDirection = m_relativeContactPosition[i].Cross(ContactNormal); // TODO physics: should this get normalized?
// Work out the direction we'd need to rotate to achieve that
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
angularChange[i] = (targetAngularDirection*inverseInertiaTensor)* (angularMove[i] / angularInteria[i]); // TODO physics: is order correct here?
}
// Velocity change is just the linear movement along the contact normal
linearChange[i] = ContactNormal * linearMove[i];
// Apply the linear movement
Vector3 pos = Body[i]->GetPosition();
pos += linearMove[i] * ContactNormal;
Body[i]->SetPosition(pos);
// Apply the change in orientation
Quaternion rot = Body[i]->GetRotation();
rot.AddScaledVector(angularChange[i], 1.0);
Body[i]->SetRotation(rot);
// TODO calculate derived data for bodies that are not awake
}
}
}
float Vector3::CompProj(const Vector3& argVec1, const Vector3& argVec2)
{
return Vector3::Dot(argVec1, argVec2) / argVec1.Magnitude();
};