function [L,U] = lucrout(A) [~,n] = size(A); L = zeros(n,n); U = eye(n,n); L(1,1) = A(1,1); for j=2:n L(j,1) = A(j,1); U(1,j) = A(1,j) / L(1,1); end for j=2:n-1 for i=j:n L(i,j) = A(i,j); for k=1:j-1 L(i,j) = L(i,j) - L(i,k)*U(k,j); end end for k=j+1:n U(j,k) = A(j,k); for i=1:j-1 U(j,k) = U(j,k) - L(j,i)*U(i,k); end U(j,k) = U(j,k) / L(j,j); end end L(n,n) = A(n,n); for k=1:n-1 L(n,n) = L(n,n) - L(n,k)*U(k,n); end end
Matrix insectToWorldTransform = ballToWorldTransform * insectToBallTransform;
class Transform extends Component { // the parent transform of this transform // if it is null then the parent transform // is the world coordinate system private Transform parent; // all of the transforms that have this // transform set as their parent private Transform[] children; // the position relative to the parent transform private Vector2 localPosition = new Vector2(0.0f, 0.0f); // rotation relative to the parent private float localRotation = 0.0f; // scale relative to the parent private Vector2 localScale = new Vector2(1.0f, 1.0f); // specifies if the localToWorldTransform // needs to be recalulated private bool isDirty = false; // the transform that converts local coordinates // to world coordinates private Matrix localToWorldMatrix = Matrix.identity; // specifies if the worldToLocalMatrix // needs to be recalculated private bool isInverseDirty = false; // the transform that converts world cooridnates // to local coordinates private Matrix worldToLocalMatrix = Matrix.identity; /* * Whenever any change happens that changes the localToWorldMatrix * this should be called. That way the next time localToWorldMatrix * is requested it will be recalculated */ private void setDirty() { // only update dirty boolean if it isn't already dirty if (!isDirty) { isDirty = true; isInverseDirty = true; // set all children to be dirty since any modification // of a parent transform also effects its children's // localToWorldTransform foreach (Transform child in children) { child.setDirty(); } } } // change the parent transform. // setting it to null makes the // transform a child of world coordinates public void setParent(Transform value) { // remove this from the previous parent if (parent != null) { parent.children.remove(this); } // assign new parent parent = value; // add this to new parent if (parent) { parent.children.add(this); } // changes parents effects // the world position setDirty(); } public Transform getParent() { return parent; } // calculates the transform matrix that converts // from local coordinates to the coordinate space // of the parent transform public Matrix calculateLocalToParentMatrix() { // Matrix.translate creates a translation matrix // that shifts by (localPosition.x, localPosition.y) // Matrix.rotate rotates by localRotation radians // Matrix.scale scales by a factor of (localScale.x, localScale.y) // These are the basic transforms that are described previously // in this article return Matrix.translate(localPosition) * Matrix.rotate(localRotation) * Matrix.scale(localScale); } // gets the matrix that converts from local // coordinates to world coordinates public Matrix getLocalToWorldMatrix() { // if the dirty flag is set, the the // localToWorldMatrix is out of date // and needs to be reclaculated if (isDirty) { if (parent == null) { // if the parent is null then the parent is // the world so the localToWorldMatrix // is the same as local to parent matrix localToWorldMatrix = calculateLocalToParentMatrix(); } else { // if there is a parent, then the localToWorldMatrix // is calcualted recursively using the parent's localToWorldMatrix // concatenated with the local to parent matrix localToWorldMatrix = parent.getLocalToWorldMatrix() * calculateLocalToParentMatrix(); } // clear the dirty flag since the // matrix is now up to date isDirty = false; } return localToWorldMatrix; } public Matrix getWorldToLocalMatrix() { if (isInverseDirty) { // the inverse is out of date // so it needs to be updated // the worldToLocalMatrix is the inverse of // the localToWorldMatrix worldToLocalMatrix = getLocalToWorldMatrix().inverse(); // clear the dirty flag since the // matrix is now up to date isInverseDirty = false; } return worldToLocalMatrix; } // transforms a point from local coordinates to world coordinates public Vector2 transformPoint(Vector2 point) { // matrix multiply padding the extra element with a 1 Matrix1x3 transformResult = getLocalToWorldMatrix() * Matrix1x3(point.x, point.y, 1); return new Vector2(transformResult[1,1], transformResult[1,2], transformResult[1,3]); } // transforms a direction from local coordinates to world coordinates public Vector2 transformDirection(Vector2 point) { // matrix multiply padding the extra element with a 0 // notice that the worldToLocalMatrix is used here // and the point is multiplied as a row matrix before the // transform matrix. This is the proper way to transform // directions as described before in this article Matrix3x1 transformResult = Matrix3x1(point.x, point.y, 0) * getWorldToLocalMatrix(); return new Vector2(transformResult[1,1], transformResult[2,1], transformResult[3,1]); } // transforms a point from world coordinates to local coordinates public Vector2 inverseTransformPoint(Vector2 point) { // same logic as transformPoint only with the inverse matrix Matrix1x3 transformResult = getWorldToLocalMatrix() * Matrix1x3(point.x, point.y, 1); return new Vector2(transformResult[1,1], transformResult[1,2], transformResult[1,3]); } // transforms a direction from world coordinates to local coordinates public Vector2 inverseTransformDirection(Vector2 point) { // same logic as transformDirection only with the inverse of the // inverse localToWorldMatrix which is just the localToWorldMatrix Matrix3x1 transformResult = Matrix3x1(point.x, point.y, 0) * getLocalToWorldMatrix(); return new Vector2(transformResult[1,1], transformResult[2,1], transformResult[3,1]); } public Vector2 getLocalPosition() { return localPosition; } // sets the position relative to the parent // and marks the transform as dirty public void setLocalPosition(Vector2 value) { localPosition = value; // set the dirty flag since the localToWorldMatrix needs to be updated setDirty(); } /* localRoation and localScale should also have getters and setters * like the local position does. Be sure to call setDirty in the * setters for each of them */ }
class Renderer extends Component { void render(graphics) { graphics.setTransformMatrix(this.gameObject.transform.getLocalToWorldMatrix()); graphics.drawSprite(this.frame); } // whatever else the renderer does // would go here ... }
graphics.setTransformMatrix( camera.transform.getWorldToLocalMatrix() * gameObject.transform.getLocalToWorldMatrix());
class Mine extends Behavoir { void update(float deltaTime, InputState input) { // transforming the zero vector gets that // transform's origin in world coordinates Vector2 playerWorldPosition = player.transform.transformPoint(new Vector2(0.0, 0.0)); // take the players world position and convert it the mine's // local coordinates Vector2 playerLocalPosition = this.transform.inverseTransformPoint(playerWorldPosition); // since playerLocalPosition is the players position relative to the mine // the magnitude of the position is the distance from the mine to the player if (playerLocalPosition.magnitudeSquared() < sensorRange * sensorRange) { this.detonate(); } } }
bool pointOnLine = fabs(dot(L,P)) < epsilon
Vector P = cross(new Vector(a1,b1,c1), new Vector(a2,b2,c2)); // check to see if the lines are parallel if(P[2] != 0) { // this is to transform back to (x,y) from (X,Y,W) x = P[0] / P[2]; y = P[1] / P[2]; }
A = c*c+d*d; B = 2*(c(x0-a)+d(y0-b)); C = (x0-a)*(x0-a)+(y0-b)*(y0-b)-r*r; disc = B*B-4*A*C; if(disc == 0) { t = -B/(2*A); x1 = x0 + c*t; y1 = y0 + d*t; return [Point(x1,y1)]; } else if(disc > 0) { t1 = (-B+sqrt(disc))/(2*A); t2 = (-B-sqrt(disc))/(2*A); x1 = x0 + c*t1; y1 = y0 + d*t1; x2 = x0 + c*t2; y2 = y0 + d*t2; return [Point(x1,y1), Point(x2,y2)]; } else return [];
Vector4 PL = new Vector4(a,b,c,d); Vector4 P = new Vector4(x0,y0,z0,1); Vector4 D = new Vector4(u,v,w,0); den = dot(PL,D); if(den != 0) // check for div by zero { t = -dot(PL,P) / den; x = x0 + u*t; y = y0 + v*t; z = z0 + w*t; }
public double[] GetBasisFunctions(int n, double t) { double[] F = new double[n+1]; F[0] = 1; double u = 1.0 - t; for(int j = 1; j <= n; j++) { for(int i = j; i >= 0; i--) { double lp = (i > n ? 0 : u * F[i]); double rp = (i - 1 < 0 ? 0 : t * F[i-1]); F[i] = lp + rp; } } return F; }
function evaluate(double t) { x = 0, y = 0, z = 0; nChooseI = 1; for(int i=0;i<controlpoints.length;i++) { u = pow(t,i); uComp = pow(1-t,i); p = u * uComp; if(i != 0) { nChooseI *= (n-i+1)/i; } x += nChooseI * controlpoints[i].x * p; y += nChooseI * controlpoints[i].y * p; z += nChooseI * controlpoints[i].z * p; } return x,y,z; }
function eval_horner(t) { u = 1 - t; bc = 1; tn = 1; tmp = controlpoints[0] * u; for(int i = 1; i < n; i++) { tn *= t; bc *= (n-i+1)/i; tmp = (tmp + tn * bc * controlpoints[i]) * u; } return (tmp + tn * t * controlpoints[n]); }
//The array contains 3 vectors - one for each tree: D3DXVECTOR2 v_treePositions[3] = {D3DXVECTOR2(0, 1), D3DXVECTOR2(1, 0), D3DXVECTOR2(-1, 0)};
//Each tree is 1m in radius: float f_treeRadius = 1.0f;
//This is a global vector holding the camera's location. It starts at the centre of the world (0, 0, 0): D3DXVECTOR3 v_camPos(0, 0, 0);
D3DXVECTOR2 handle_Collisions_Radial() { D3DXVECTOR2 v_deltaPos = D3DXVECTOR2(0, 0); for (int i = 0; i < 3, i++) { D3DXVECTOR2 v_vectorToCentre(v_camPos.x - v_treePositions[i].x, v_camPos.z - v_treePositions[i].z); if (D3DXVec2Length(&v_vectorToCentre) < f_treeRadius) { v_deltaPos += ((f_treeRadius / D3DXVec2Length(&v_vectorToCentre)) - 1) * v_vectorToCentre; } } return v_deltaPos; }
//Call the function to assess collisions: D3DXVECTOR2 v_camPosCollisionDelta = handle_Collisions_Radial(); //Since this function returns a D3DXVECTOR2 and our camera position is a D3DXVECTOR3, we //will need to create a dummy D3DXVECTOR3 with no y-component to add to v_camPos: D3DXVECTOR3 v_camPosCollisionDelta3D(v_camPosCollisionDelta.x, 0, v_camPosCollisionDelta.y); //Finally, we will move the camera according to our collison delta vector: v_camPos += v_camPosCollisionDelta3D; //Now that we have our new camera position, we can continue to render the frame as normal...
x = cos(angle) * magnitude y = sin(angle) * magnitude
// to get the magnitude, use the pathagorean theorem magnitude = sqrt(x * x + y * y) angle = atan2(y, x)
function normalize() { float magnitude = sqrt(x * x + y * y) x = x / magnitude y = y / magnitude }
function normalize() { magnitude = sqrt(x * x + y * y) if (magnitude > 0.0) { x = x / magnitude y = y / magnitude } }
// subtract the player position from the turret position // to get the direction direction = player.position - turret.position // the above code is really just shorthand for direction.x = player.position.x - turret.position.x direction.y = player.position.y - turret.position.y // when using vectors you should use a vector class that lets you write out // operations like a - b, or a.sub(b) instead of having to subtract // each component individually
// make the length of direction one direction.normalize() // set the velocity of the bullet bullet.velocity = speed * direction // remember, the above code is the same as bullet.velocity.x = speed * direction.x bullet.velocity.y = speed * direction.y // since speed is just a number value, not a vector, it // scales both components equalily
// dot product dot(a, b) = a.magnitude * b.magnitude * cos(angleBetween(a,b)) // also equal to dot(a, b) = a.x * b.x + a.y * b.y
angleBetween(a,b) = acos((a.x * b.x + a.y * b.y) / (a.magnitude * b.magnitude))
angleBetweenUnitVectors(a,b) = acos(a.x * b.x + a.y * b.y) // also the same as angleBetweenUnitVectors(a,b) = acos(dot(a,b))
(a + bi) * (c + di) = (a*c - b*d) + (b*c + a*d)i // or using x and y function rotateVector(Vector2 a, Vector2 b) { float x = a.x * b.x - a.y * b.y; float y = a.x * b.y + a.y * b.x; return new Vector2(x, y); }
function unitVector(float angle) { // this returns a unit vector in the direction of angle // it will always be of length 1 since, in this case // magnitude = cos(angle)^2 + sin(angle)^2 = 1 return new Vector2(cos(angle), sin(angle)); } // rotate the velocity by 10 degress velocity = rotateVector(velocity, unitVector(degToRads(10)))
// a and b should be unit vectors // t is a value in the range [0, 1] // when t = 0, slerp returns a // when t = 1, slerp returns b // any value between 1 and 0 will be // vector between a and b function slerp(Vector2 vectorA, Vector2 vectorB, float t) { float cosAngle = dot(vectorA, vectorB); float angle = acos(cosAngle); return (vectorA * sin((1 - t) * angle) + vectorB * sin(t * angle)) / sin(angle); }
// rotates rotateFrom towards rotateTo by maxDeltaTheta radians and returns // the result. If the angle between them is less than maxDeltaTheta then it // simply returns rotateTo // rotateFrom and rotateTo should both be unit vectors function rotateTowards(Vector2 rotateFrom, Vector2 rotateTo, float maxDeltaTheta) { float angleBetween = acos(dot(rotateFrom, rotateTo)); if (angleBetween <= maxDeltaTheta) { return rotateTo; } else { return slerp(rotateFrom, rotateTo, angleBetween / maxDeltaTheta); } } // and an example of how it can be used Vector2 playerOffset = player.position - turret.position // the input to rotates towards needs to be a unit vector playerOffset.normalize() // turretDirection is a unit vector specifying where the turret is pointing // delta time is a float holding the number of seconds between this frame and the last // turretTurnRate is the turn rate of the turret in radians/second turretDirection = rotateTowards(turretDirection, playerOffset, turretTurnRate * deltaTime)
class Ray { public: float4 origin; float4 direction; <<constructor>> <<methods>> };
class Plane { public: float4 point; float4 normal; <<constructor>> <<methods>> };
class Sphere { public: float4 center; float radius; <<constructor>> <<methods>> };
class AABB { public: float4 min_p; float4 max_p; <<constructor>> <<methods>> };
class Triangle { public: float4 p[3]; <<constructor>> <<methods>> };
bool IntrRayPlane(const Ray* r, const Plane* p, float* t) { float denom = dot(p->normal, r->direction); // Test whether ray and plane aren't parallel if(fabsf(dotND) < EPSILON) return false; *t = -(dot(p->normal, r->origin - p->point) / denom); return ((*t) > 0.0f); }
bool IntrRaySphere(const Ray* r, const Sphere* s, float* t) { float rad2 = s->radius * s->radius; float4 L = s->center - r->origin; float tPX = dot(L, r->direction); if(tPX < 0.0) return false; float dsq = dot(L, L) - tPX * tPX; if(dsq > rad2) return false; float thit = sqrt(rad2 - dsq); *t = tPX - thit; if(*t < 0.0f) *t = tPX + thit; return ((*t) < 0.0f); }
bool IntrRayAABB(const Ray* r, const AABB* b, float* enter, float* exit) { float4 tmin = (b->minimum - r->origin) / r->direction; float4 tmax = (b->maximum - r->origin) / r->direction; float4 tnear = f4min(tmin, tmax); float4 tfar = f4min(tmin, tmax); *enter = max(max(tnear.x, 0.0f), max(tnear.y, tnear.z)); *exit = min(tfar.x, min(tfar.y, tfar.z)); return (*exit > 0.0f && *enter < *exit); }
bool IntrRayTriangle( const Ray* r, const Triangle* t, float* thit, float* u, float* v) { float4 e1 = t->v[1] - t->v[0]; float4 e2 = t->v[2] - t->v[0]; float4 pvec = cross(r->direction, e2); float det = dot(e1, pvec); if(det > -EPSILON && det < EPSILON) return false; float inv_det = 1.0f / det; float4 tvec = r->origin - t->v[0]; *u = dot(tvec, pvec) * inv_det; if(*u < 0.0f || *u > 1.0f) return false; float4 qvec = cross(tvec, e1); *v = dot(r->direction, qvec) * inv_det; if(*v < 0.0f || *u + *v > 1.0f) return false; *thit = dot(e2, qvec) * inv_det; return (*thit > 0.0f); }
tree_position = (10, 10, 0) my_position = (3, 3, 0) # distance and direction you would need to move # to get exactly where the tree is vector_to_tree = tree_position - my_position
position = (0, 10, 10) # position: 10 units in Y and Z direction velocity = (500, 0, 0) # initial movement is 500 units in X direction over the next second
position += velocity # add velocity to position and update position
position += velocity * delta
# increase velocity of every object -2 down per second gravity_vector = (0, -2, 0)
velocity += gravity_vector * delta # apply gravity effect position += velocity * delta # update position
velocity += gravity_vector * delta # apply gravity effect velocity *= 0.5 * delta # apply 0.5 slowdown per second position += velocity * delta # update position
# modify kinetic energy / velocity velocity += gravity_vector * delta # apply gravity effect velocity *= 0.5 * delta # apply 0.5 slowdown per second # add all forces final_change_per_second = velocity + wind_force_per_second # update position position += final_change_per_second * delta
final_change = (B - A).normalize() * 3
result = A dot B
angle = acos(A dot B)
distance_vec = light_pos - point_pos
light_direction = distance_vec.normalize()
brightness = surface_normal dot light_direction
varying vec3 surface_normal; varying vec3 vertex_to_light_vector; void main(void) { vec4 diffuse_color = vec3(1.0, 1.0, 1.0); // the color of surface - white float diffuse_term = dot(surface_normal, normalize(vertex_to_light_vector)); gl_FragColor = diffuse_color * diffuse_term; }
distance_to_a_plane = (point - plane_point) dot plane_normal;
dot1 = SN dot (SP - P1) dot2 = SN dot (P2 - P1)
u = (SN dot (SP - P1)) / (SN dot (P2 - P1))
intersection point = (P2 - P1) * u
temp_vector = change cross plane_normal
new_direction = temp_vector cross plane_normal
new_direction = (change cross plane_normal) cross plane_normal
struct Point { Vec2 Position; Vec2 OldPosition; Vec2 Acceleration; }; class Physics { int PointCount; Point* Points[ MAX_VERTICES ]; float Timestep; public: void UpdateVerlet(); //Constructors, getters/setters etc. omitted }; void Physics::UpdateVerlet() { for( int I = 0; I < PointCount; I++ ) { Point& P = *Points[ I ]; Vec2 Temp = P.Position; P.Position += P.Position - P.OldPosition + P.Acceleration*Timestep*Timestep; P.OldPosition = Temp; } }Now we have a working physics code that will calculate the trajectories of arbitrary points just fine. But points alone are not very useful, except when you're programming a particle simulation. Since that's normally not the case if you're into game programming, we have to extend the points in some way so we can simulate rigid body behaviour as well. If we look at rigid bodies in nature, we see that they are actually a huge amount of points (=atoms) held together by various forces.
struct Edge { Vertex* V1; Vertex* V2; float OriginalLength; //The length of the edge when it was created //Constructors etc. omitted }; void Physics::UpdateEdges() { for( int I = 0; I < EdgeCount; I++ ) { Edge& E = *Edges[ I ]; //Calculate the vector mentioned above Vec2 V1V2 = E.V2->Position - E.V1->Position; //Calculate the current distance float V1V2Length = V1V2.Length(); //Calculate the difference from the original length float Diff = V1V2Length - E.OriginalLength; V1V2.Normalize(); //Push both vertices apart by half of the difference respectively //so the distance between them equals the original length E.V1->Position += V1V2*Diff*0.5f; E.V2->Position -= V1V2*Diff*0.5f; } }That's it - if we created a few points and connected them with our newly created edge struct, the resulting body would show very nice rigid body-behaviour, including rotational effects when it hits the floor. But why does that work? The code isn't much different from before, we only added a few lines to satisfy the distance constraint and suddenly we have rigid bodies. The reason behind this lies within our integration method. If we recall that the Verlet integration doesn't work with velocity but rather with the difference between the current position and the position before the last integration step, it should become clear that the speed of the point will change if we change its position. Therefore, since we change its position in the UpdateEdges method, its velocity will also change. The overall change in velocity looks exactly like we would expect it from a vertex of a rigid body; it is not totally correct, but good enough for games.
struct PhysicsBody { int VertexCount; int EdgeCount; Vertex* Vertices[ MAX_BODY_VERTICES ]; Edge* Edges [ MAX_BODY_EDGES ]; void ProjectToAxis( Vec2& Axis, float& Min, float& Max ); //Again, constructors etc. omitted };The ProjectToAxis method will project the body onto the passed axis and change the Min and Max variables to the result of the projection. Since a projection of a 2D-shape onto 1D results in a mere interval of a line, the result of the projection can be stored in two floats that denote the beginning and the end of the interval. The projection method is quite simple:
void PhysicsBody::ProjectToAxis( Vec2& Axis, float& Min, float& Max ) { float DotP = Axis*Vertices[ 0 ]->Position; //Set the minimum and maximum values to the projection of the first vertex Min = Max = DotP; for( int I = 1; I < VertexCount; I++ ) { //Project the rest of the vertices onto the axis and extend //the interval to the left/right if necessary DotP = Axis*Vertices[ I ]->Position; Min = MIN( DotP, Min ); Max = MAX( DotP, Max ); } }As you can see, projection in 2D is simply a dot product of the projection axis and the point we want to project. The collision detection may look like this:
bool Physics::DetectCollision( PhysicsBody* B1, PhysicsBody* B2 ) { //Just a fancy way of iterating through all of the edges of both bodies at once for( int I = 0; I < B1->EdgeCount + B2->EdgeCount; I++ ) { Edge* E; if( I < B1->EdgeCount ) E = B1->Edges[ I ]; else E = B2->Edges[ I - B1->EdgeCount ]; //Calculate the axis perpendicular to this edge and normalize it Vec2 Axis( E->V1->Position.Y - E->V2->Position.Y, E->V2->Position.X - E->V1->Position.X ); Axis.Normalize(); float MinA, MinB, MaxA, MaxB; //Project both bodies onto the perpendicular axis B1->ProjectToAxis( Axis, MinA, MaxA ); B2->ProjectToAxis( Axis, MinB, MaxB ); //Calculate the distance between the two intervals - see below float Distance = IntervalDistance( MinA, MaxA, MinB, MaxB ); if( Distance > 0.0f ) //If the intervals don't overlap, return, since there is no collision return false; } return true; //There is no separating axis. Report a collision! }The algorithm works just like described above; if something isn't clear, I would suggest rereading the explanations step by step. The IntervalDistance method that is mentioned in the code is actually quite simple:
float Physics::IntervalDistance( float MinA, float MaxA, float MinB, float MaxB ) { if( MinA < MinB ) return MinB - MaxA; else return MinA - MaxB; }Since we don't know if body A will lie on the left and body B on the right or vice-versa, we have to check which interval begins sooner. We then subtract the end of the left interval from the beginning of the right interval to get the distance between the two - if this value is smaller than zero, they overlap.
class Physics { struct { float Depth; Vec2 Normal; } CollisionInfo; //Everything else omitted }The 'Depth' is the length of the vector, the 'Normal' is the direction of the vector discussed above.
bool Physics::DetectCollision( PhysicsBody* B1, PhysicsBody* B2 ) { float MinLength = 10000.0f; //Initialize the length of the collision vector to a relatively large value for( int I = 0; I < B1->EdgeCount + B2->EdgeCount; I++ ) { Edge* E; if( I < B1->EdgeCount ) E = B1->Edges[ I ]; else E = B2->Edges[ I - B1->EdgeCount ]; Vec2 Axis( E->V1->Position.Y - E->V2->Position.Y, E->V2->Position.X - E->V1->Position.X ); Axis.Normalize(); float MinA, MinB, MaxA, MaxB; B1->ProjectToAxis( Axis, MinA, MaxA ); B2->ProjectToAxis( Axis, MinB, MaxB ); float Distance = IntervalDistance( MinA, MaxA, MinB, MaxB ); if( Distance > 0.0f ) return false; //If the intervals overlap, check, whether the vector length on this //edge is smaller than the smallest length that has been reported so far else if( abs( Distance ) < MinDistance ) { MinDistance = abs( Distance ); CollisionInfo.Normal = Axis; //Save collision information for later } } CollisionInfo.Depth = MinDistance; return true; //There is no separating axis. Report a collision! }Once we have this, we'd be already able to write a very simple collision response. Since the collision vector we calculated pushes the two bodies apart so they don't collide anymore, we could just move all vertices of both bodies back by half the vector and we'd be done. This would work, since interpenetrations are resolved, but it wouldn't look right. The bodies would simply glide off each other, meaning they don't start to spin like a real object when hit.
struct { float Depth; Vec2 Normal; Edge* E; Vertex* V; } CollisionInfo;Now we can rewrite our DetectCollision method to detect the additional information:
bool Physics::DetectCollision( PhysicsBody* B1, PhysicsBody* B2 ) { float MinDistance = 10000.0f; for( int I = 0; I < B1->EdgeCount + B2->EdgeCount; I++ ) { //Same old Edge* E; if( I < B1->EdgeCount ) E = B1->Edges[ I ]; else E = B2->Edges[ I - B1->EdgeCount ]; Vec2 Axis( E->V1->Position.Y - E->V2->Position.Y, E->V2->Position.X - E->V1->Position.X ); Axis.Normalize(); float MinA, MinB, MaxA, MaxB; B1->ProjectToAxis( Axis, MinA, MaxA ); B2->ProjectToAxis( Axis, MinB, MaxB ); float Distance = IntervalDistance( MinA, MaxA, MinB, MaxB ); if( Distance > 0.0f ) return false; else if( abs( Distance ) < MinDistance ) { MinDistance = abs( Distance ); CollisionInfo.Normal = Axis; CollisionInfo.E = E; //Store the edge, as it is the collision edge } } CollisionInfo.Depth = MinDistance; //Ensure that the body containing the collision edge lies in //B2 and the one containing the collision vertex in B1 if( CollisionInfo.E->Parent != B2 ) { PhysicsBody* Temp = B2; B2 = B1; B1 = Temp; } //This is needed to make sure that the collision normal is pointing at B1 int Sign = SGN( CollisionInfo.Normal*( B1->Center - B2->Center ) ); //Remember that the line equation is N*( R - R0 ). We choose B2->Center //as R0; the normal N is given by the collision normal if( Sign != 1 ) CollisionInfo.Normal = -CollisionInfo.Normal; //Revert the collision normal if it points away from B1 float SmallestD = 10000.0f; //Initialize the smallest distance to a high value for( int I = 0; I < B1->VertexCount; I++ ) { //Measure the distance of the vertex from the line using the line equation float Distance = CollisionInfo.Normal*( B1->Vertices[ I ]->Position - B2->Center ); //If the measured distance is smaller than the smallest distance reported //so far, set the smallest distance and the collision vertex if( Distance < SmallestD ) { SmallestD = Distance; CollisionInfo.V = B1->Vertices[ I ]; } } return true; }In the above code, we introduced a new variable in the PhysicsBody struct, the center. It will be recalculated before the collision step and is simply the average of all vertices of the body.
void Physics::ProcessCollision() { Vec2 CollisionVector = CollisionInfo.Normal*CollisionInfo.Depth; CollisionInfo.V->Position += CollisionVector*0.5f; }For the edge case, this will become a bit more complicated. The edge consists of two vertices that will move differently, depending on where the collision vertex lies. The closer it lies to the one end of the edge, the more this end will move and vice-versa. This means that we first have to calculate where on the edge the collision vertex lies. This is done using the following equation:
Vertex* E1 = CollisionInfo.E->V1; Vertex* E2 = CollisionInfo.E->V2; float T = ( CollisionInfo.V->Position.X - CollisionVector.X - E1->Position.X )/( E2->Position.X - E1->Position.X );But be careful! If E2 lies directly above E1, the program would divide by zero. Therefore, we should build in a small check to avoid this:
float T; if( abs( E1->Position.X - E2->Position.X ) > abs( E1->Position.Y - E2->Position.Y ) ) T = ( CollisionInfo.V->Position.X - CollisionVector.X - E1->Position.X )/( E2->Position.X - E1->Position.X ); else T = ( CollisionInfo.V->Position.Y - CollisionVector.Y - E1->Position.Y )/( E2->Position.Y - E1->Position.Y );This basically divides by the X denominator if it is bigger than the Y denominator and vice-versa.
void Physics::ProcessCollision() { Vec2 CollisionVector = CollisionInfo.Normal*CollisionInfo.Depth; Vertex* E1 = CollisionInfo.E->V1; Vertex* E2 = CollisionInfo.E->V2; float T; if( abs( E1->Position.X - E2->Position.X ) > abs( E1->Position.Y - E2->Position.Y ) ) T = ( CollisionInfo.V->Position.X - CollisionVector.X - E1->Position.X )/( E2->Position.X - E1->Position.X); else T = ( CollisionInfo.V->Position.Y - CollisionVector.Y - E1->Position.Y )/( E2->Position.Y - E1->Position.Y); float Lambda = 1.0f/( T*T + ( 1 - T )*( 1 - T ) ); E1->Position -= CollisionVector*( 1 - T )*0.5f*Lambda; E2->Position -= CollisionVector* T *0.5f*Lambda; CollisionInfo.V->Position += CollisionVector*0.5f; }That's it. We're done with collision response - easy, wasn't it?
void Physics::Update() { UpdateForces(); UpdateVerlet(); IterateCollisions(); }IterateCollisions is a method that does multiple things. It iterates over all bodies, calls the respective UpdateEdges method, recalculates the body center and then does the collision detection (and the collision response, if necessary). Of course, it doesn't just do this once, but repeats those steps a few times. The more repetitions are made, the more realistic the physics will look. The reason was explained above (if you've forgotten, better read it again ;) ).
Someone asked me in the #gamedev IRC channel about how to make a 2d vehicle simulator. Instead of spending all day trying to explain the concepts to him, I decided just to write this tutorial.
Please bear with me, this is my first tutorial.
So as I mentioned we’re going to be learning how to make a basic 2d vehicle simulator. We’re going to do it in C# and try do use as few hacks as possible. I’ve broken the process
down into three steps. First, we will learn how to setup a basic game application in C#.NET and how to draw some basic graphics (emphasis on basic.) Next, we will learn how to create a rigid body
simulator using a simple Euler integrator with a variable time step. And last but not least, we will calculate vehicle forces simulating the tire patch contacting the road. And that’s all there
is to it! Let’s get started.
Phase one, as I mentioned, is to create the renderer; something graphical so we can actually see what our simulation is doing. This will make it a lot easier to debug. Create a windows form
project in C# and place a picturebox control on it (name it "screen"). This control is where we will display our simulation. We could just start drawing to this screen but we’re going to be
using double buffering as well to avoid flicker, so we need to create the back buffer now. That bit of code looks like this.
Graphics graphics; //gdi+ Bitmap backbuffer; Size buffersize; //intialize rendering private void Init(Size size) { //setup rendering device buffersize = size; backbuffer = new Bitmap(buffersize.Width, buffersize.Height); graphics = Graphics.FromImage(backbuffer); }
Init
function must be called with the size of the “screen” control that you created on the form. This will create a bitmap “backbuffer” to which we can do our//main rendering function private void Render(Graphics g) { //clear back buffer graphics.Clear(Color.Black); //draw to back buffer graphics.DrawLine(new Pen(Color.Yellow), 1, 0, 1, 5); //present back buffer g.DrawImage(backbuffer, new Rectangle(0, 0, buffersize.Width, buffersize.Height), 0, 0, buffersize.Width, buffersize.Height, GraphicsUnit.Pixel); }
on_paint
method of the "screen" control placed on our form. The on_paint
method has a parameter “e” that contains a graphicsNow by default, the graphics of a picturebox control has the origin in the top-left corner, and extends downward for +y and to the right for +x. This is highly unnatural for most cases. In
addition to that, it has extremely large units. Since we will be simulating in the metric system, I recommend introducing a scale factor to scale up the simulation and make it much more visible. The
transformation looks like this and takes place after Graphics.Clear()
is called.
graphics.ResetTransform(); graphics.ScaleTransform(screenScale, -screenScale); graphics.TranslateTransform(buffersize.Width / 2.0f / screenScale, -buffersize.Height / 2.0f / screenScale);
Now the line should draw starting right at the center of the screen.
Up until now, I havn’t explained how to connect all the functions. The first thing you’ll need to do is call the Render
function from your on_paint
event.
Next, you’ll need to create a function that gets called continously to update the simulation. It is preferred to call this function on the Application_Idle
event. So create an
event handler for Application_Idle
and have it call your DoFrame
function. Inside this function you’ll need to
On_Paint
gets triggerd and the simulation gets drawn. You’ll also want to wire up some “key_down” and “key_up” events to keepSince we don’t know how often our DoFrame
function will be getting called, we need to code everything to handle a variable time step. To utilize this we must measure the time
between DoFrame
calls. So I’ll introduce the timer which, very simply, queries the number of milliseconds that have passed since the computer was turned on. So we store this number
every frame and on a subsequent frame we compute the difference, which gives us the amount of time that has passed since the last frame. Here is my very simple timer object. Note: you will
need to call GetETime
in your intialize function in order to clear the timer, otherwise the first call to it will return the amount of time that has passed since the computer was turned
on.
class Timer { //store last time sample private int lastTime = Environment.TickCount; private float etime; //calculate and return elapsed time since last call public float GetETime() { etime = (Environment.TickCount - lastTime) / 1000.0f; lastTime = Environment.TickCount; return etime; } }
So up until now we’ve covered: setting up a rendering surface using GDI, wiring a form to process a game loop and draw it to the screen, and computing the time that has passed since the last
frame. Our application looks like this:
using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Drawing.Drawing2D; using System.Text; using System.Windows.Forms; namespace racing_simulation_2d { //our main application form public partial class frmMain : Form { //graphics Graphics graphics; //gdi+ Bitmap backbuffer; Size buffersize; const float screenScale = 3.0f; Timer timer = new Timer(); //keyboard controls bool leftHeld = false, rightHeld = false; bool upHeld = false, downHeld = false; //vehicle controls float steering = 0; //-1 is left, 0 is center, 1 is right float throttle = 0; //0 is coasting, 1 is full throttle float brakes = 0; //0 is no brakes, 1 is full brakes public frmMain() { InitializeComponent(); Application.Idle += new EventHandler(ApplicationIdle); screen.Paint += new PaintEventHandler(screen_Paint); this.KeyDown += new KeyEventHandler(onKeyDown); this.KeyUp += new KeyEventHandler(onKeyUp); Init(screen.Size); } //intialize rendering private void Init(Size size) { //setup rendering device buffersize = size; backbuffer = new Bitmap(buffersize.Width, buffersize.Height); graphics = Graphics.FromImage(backbuffer); timer.GetETime(); //reset timer } //main rendering function private void Render(Graphics g) { //clear back buffer graphics.Clear(Color.Black); graphics.ResetTransform(); graphics.ScaleTransform(screenScale, -screenScale); graphics.TranslateTransform(buffersize.Width / 2.0f / screenScale, -buffersize.Height / 2.0f / screenScale); //draw to back buffer DrawScreen(); //present back buffer g.DrawImage(backbuffer, new Rectangle(0, 0, buffersize.Width, buffersize.Height), 0, 0, buffersize.Width, buffersize.Height, GraphicsUnit.Pixel); } //draw the screen private void DrawScreen() { //draw our simulation here } //process game logic private void DoFrame() { //get elapsed time since last frame float etime = timer.GetETime(); //process input ProcessInput(); //////////////////////////////// //integrate our simulation here //////////////////////////////// //redraw our screen screen.Invalidate(); } //process keyboard input private void ProcessInput() { if (leftHeld) steering = -1; else if (rightHeld) steering = 1; else steering = 0; if (upHeld) throttle = 1; else throttle = 0; if (downHeld) brakes = 1; else brakes = 0; } private void onKeyDown(object sender, KeyEventArgs e) { switch (e.KeyCode) { case Keys.Left: leftHeld = true; break; case Keys.Right: rightHeld = true; break; case Keys.Up: upHeld = true; break; case Keys.Down: downHeld = true; break; default: //no match found return; //return so handled dosnt get set } //match found e.Handled = true; } private void onKeyUp(object sender, KeyEventArgs e) { switch (e.KeyCode) { case Keys.Left: leftHeld = false; break; case Keys.Right: rightHeld = false; break; case Keys.Up: upHeld = false; break; case Keys.Down: downHeld = false; break; default: //no match found return; //return so handled dosnt get set } //match found e.Handled = true; } //rendering - only when screen is invalidated private void screen_Paint(object sender, PaintEventArgs e) { Render(e.Graphics); } //when the os gives us time, run the game private void ApplicationIdle(object sender, EventArgs e) { // While the application is still idle, run frame routine. DoFrame(); } private void MenuExit_Click(object sender, EventArgs e) { this.Close(); } } //keep track of time between frames class Timer { //store last time sample private int lastTime = Environment.TickCount; private float etime; //calculate and return elapsed time since last call public float GetETime() { etime = (Environment.TickCount - lastTime) / 1000.0f; lastTime = Environment.TickCount; return etime; } } }
Ok, now we’re getting into some good stuff here. Let's put everything we just covered on the back burner now and talk about some physics. We’re going to be using a very simple Euler
integration method. Basically, each frame we accumulate a bunch of forces (in our case from each wheel of the vehicle) and calculate the resultant acceleration, which is in the form of A=F/M (the
same as F=MA, Newton’s second law of motion). We use this to modify Newton’s first law of motion, “an object in motion stays in motion…” So we calculate our A, and we
integrate it into our V. Without an A, V would be constant, hence staying in motion, if no forces should act on it. Newton's third law gets applied in the form that any potential force the vehicle is
applying to the ground, gets applied in the opposite direction to the vehicle (I'll explain this in the vehicle section). This topic is much easier to explain with symbols. So, P is our vehicle
position, V is its linear velocity, F is the net force acting on it, M is its mass, A is the resultant acceleration, and T is the time step (the value our timer gave us from the last frame).
A = F / M V = V + A * T P = P + V * T
This is a basic linear rigid body simulator. Each frame, we total up some F, integrate it, and then zero out F to restart the accumulation the next frame. Now let’s talk about rotation. The
angular case is nearly identical to the linear case (especially in 2D). Instead of P we have an Angle, instead of V we have an Angular Velocity, instead of F we have a torque, and instead of M we
have inertia. So the angular model looks like this
AngA = Torque / Inertia AngV = AngV + AngA * T Angle = Angle + AngV * T
AddForce
functionThis is what that bit of code looks like. % is the cross product operator for my vector class.
public void AddForce(Vector worldForce, Vector worldOffset) { //add linar force m_forces += worldForce; //and it's associated torque m_torque += worldOffset % worldForce; }
Here is my rigid body object. You’ll notice all the properties I mentioned above. It has a Draw
function which will draw its rectangle to the provided graphics object. It has an
AddForce
function, a space conversion method, to and from world space (very handy), and a function that returns the velocity of a point on the body (in world space). This point velocity
is a combination of the linear velocity and the angular velocity. But the angular velocity is multiplied by the distance the point is from the center of rotation and perpendicular to its offset
direction. So to kill two birds with one stone, I simply find the orthogonal vector to the point offset and multiply it by the angular velocity (then add the linear velocity.)
One thing you may be curious about is how I calculate the inertia value. That is a generalized formula I found at this
link.
//our simulation object class RigidBody { //linear properties private Vector m_position = new Vector(); private Vector m_velocity = new Vector(); private Vector m_forces = new Vector(); private float m_mass; //angular properties private float m_angle; private float m_angularVelocity; private float m_torque; private float m_inertia; //graphical properties private Vector m_halfSize = new Vector(); Rectangle rect = new Rectangle(); private Color m_color; public RigidBody() { //set these defaults so we dont get divide by zeros m_mass = 1.0f; m_inertia = 1.0f; } //intialize out parameters public void Setup(Vector halfSize, float mass, Color color) { //store physical parameters m_halfSize = halfSize; m_mass = mass; m_color = color; m_inertia = (1.0f / 12.0f) * (halfSize.X * halfSize.X) * (halfSize.Y * halfSize.Y) * mass; //generate our viewable rectangle rect.X = (int)-m_halfSize.X; rect.Y = (int)-m_halfSize.Y; rect.Width = (int)(m_halfSize.X * 2.0f); rect.Height = (int)(m_halfSize.Y * 2.0f); } public void SetLocation(Vector position, float angle) { m_position = position; m_angle = angle; } public Vector GetPosition() { return m_position; } public void Update(float timeStep) { //integrate physics //linear Vector acceleration = m_forces / m_mass; m_velocity += acceleration * timeStep; m_position += m_velocity * timeStep; m_forces = new Vector(0,0); //clear forces //angular float angAcc = m_torque / m_inertia; m_angularVelocity += angAcc * timeStep; m_angle += m_angularVelocity * timeStep; m_torque = 0; //clear torque } public void Draw(Graphics graphics, Size buffersize) { //store transform, (like opengl's glPushMatrix()) Matrix mat1 = graphics.Transform; //transform into position graphics.TranslateTransform(m_position.X, m_position.Y); graphics.RotateTransform(m_angle/(float)Math.PI * 180.0f); try { //draw body graphics.DrawRectangle(new Pen(m_color), rect); //draw line in the "forward direction" graphics.DrawLine(new Pen(Color.Yellow), 1, 0, 1, 5); } catch(OverflowException exc) { //physics overflow :( } //restore transform graphics.Transform = mat1; } //take a relative vector and make it a world vector public Vector RelativeToWorld(Vector relative) { Matrix mat = new Matrix(); PointF[] vectors = new PointF[1]; vectors[0].X = relative.X; vectors[0].Y = relative.Y; mat.Rotate(m_angle / (float)Math.PI * 180.0f); mat.TransformVectors(vectors); return new Vector(vectors[0].X, vectors[0].Y); } //take a world vector and make it a relative vector public Vector WorldToRelative(Vector world) { Matrix mat = new Matrix(); PointF[] vectors = new PointF[1]; vectors[0].X = world.X; vectors[0].Y = world.Y; mat.Rotate(-m_angle / (float)Math.PI * 180.0f); mat.TransformVectors(vectors); return new Vector(vectors[0].X, vectors[0].Y); } //velocity of a point on body public Vector PointVel(Vector worldOffset) { Vector tangent = new Vector(-worldOffset.Y, worldOffset.X); return tangent * m_angularVelocity + m_velocity; } public void AddForce(Vector worldForce, Vector worldOffset) { //add linar force m_forces += worldForce; //and it's associated torque m_torque += worldOffset % worldForce; } }
To make sure your rigid body works, instantiate one in your Init()
function and apply a force with some offset in the DoFrame
function. If you apply a constant
worldOffset, the body will continue to accelerate its angular velocity. If you take your offset and run it through the RelativeToWorld
function, the body will angularly accelerate in one
direction and then come back the other way, like a pendulum as the point the force is applied to changes. Play around with this for a while, this has to work and make sense in order for the next
section to work.
Assuming everything has gone well above, you should have a rigid body actor in your scene that you can apply forces to and watch move around. Now all that’s left is to calculate these forces
in a way that will simulate a vehicle. For that we are going to need a vehicle object. I recommend deriving directly from you rigid body object since the chassis is essentially a rigid body. In
addition to that we will need to construct a “wheel" object. This wheel will handle the steering direction of each wheel, the velocity the wheel is spinning, and calculate the forces that that
particular wheel applies to the chassis (all in vehicle space). Since our wheel is known to be constrained to the vehicle, we don’t need to simulate it as another rigid body (though you could,
but not in the 2D case.) We will simply duplicate the angular properties of the rigid body in the wheel object.
So we’ll need: Wheel Velocity, Wheel Inertia, and and Wheel Torque. We’ll also need the relative offset of the wheel in the vehicle space, and the angle the wheel is facing (this is
constant for the back wheels, unless you want 4 wheel steering.) Just like the rigid body, the wheel's torque function acts as an accumulator, we add torques to it and after it gets integrated the
torque is zeroed out. The AddTorque
function is where you will apply a wheel torque from either the transmission (to make you go) or from the brakes (to make you stop). Internally the
wheel will generate a torque caused by the friction on the road.
The wheel object also needs a SetSteering
function. This function calculates two vectors: an effective Side Direction, and an effective Forward Direction (both in vehicle space) that
the tire patch will act on. The force applied on the tire by the ground acting in the side direction will directly translate into the chassis. Meanwhile the force acting in the forward direction will
not only act on the chassis, but it will induce a rotation of the tire. Here is the SetSteering
function; you will see I used the Drawing2D.Matrix
to transform the initial
forward and side vectors by the steering angle (I had to convert the vectors to “points" in order to transform them by the matrix.)
public void SetSteeringAngle(float newAngle) { Matrix mat = new Matrix(); PointF[] vectors = new PointF[2]; //foward vector vectors[0].X = 0; vectors[0].Y = 1; //side vector vectors[1].X = -1; vectors[1].Y = 0; mat.Rotate(newAngle / (float)Math.PI * 180.0f); mat.TransformVectors(vectors); m_forwardAxis = new Vector(vectors[0].X, vectors[0].Y); m_sideAxis = new Vector(vectors[1].X, vectors[1].Y); }
So, if the vehicle is sitting there not moving with its front wheels turned, and you push it, a force will be generated in the opposite direction you push. This force gets projected onto these two
directions. If the wheels were straight there would be no side force. So the vehicle would simply roll forward. But since the wheels are turned, there is a bit of the force that acts in the
“effective side direction” so we apply an opposite force to the chassis. This is what causes you to turn when you steer the wheels. To get this force that gets projected onto the two
directions, we need to first determine the velocity difference between the tire patch and the road. If the wheel is spinning at the same speed the ground is wizzing by, then there is effectively no
force acting on the vehicle. But as soon as you slam on the brakes and stop the wheel, there is a huge velocity difference and this is what causes the force that stops your car.
So here is the process broken down into 6 steps, for each wheel.
Step 1, calculate the effective direction vectors (with steering function).
Step 2, calculate velocity difference. The ground speed is determined via the “PointVel” function on the rigidbody, given the current wheel’s world offset.
Step 3, project this velocity onto the two effective directions.
Step 4, generate an equal and opposite force for the two direction and call this the “response force”. This is what gets added to the chassis for each wheel.
Step 5, calculate the torque that the forward response force created on the wheel, and add this to the wheel torque.
Step 6, integrate the wheel torques into the wheel velocity.
That bit of code looks like this:
public Vector CalculateForce(Vector relativeGroundSpeed, float timeStep) { //calculate speed of tire patch at ground Vector patchSpeed = -m_forwardAxis * m_wheelSpeed * m_wheelRadius; //get velocity difference between ground and patch Vector velDifference = relativeGroundSpeed + patchSpeed; //project ground speed onto side axis float forwardMag = 0; Vector sideVel = velDifference.Project(m_sideAxis); Vector forwardVel = velDifference.Project(m_forwardAxis, out forwardMag); //calculate super fake friction forces //calculate response force Vector responseForce = -sideVel * 2.0f; responseForce -= forwardVel; //calculate torque on wheel m_wheelTorque += forwardMag * m_wheelRadius; //integrate total torque into wheel m_wheelSpeed += m_wheelTorque / m_wheelInertia * timeStep; //clear our transmission torque accumulator m_wheelTorque = 0; //return force acting on body return responseForce; }
We’re in the home stretch here now. Now we have a way to calculate the force each wheel generates on the chassis. Every frame, all we have to do is set our transmission and brake torques,
our steering angle, calculate each wheel force, add these to the chassis, and integrate the rigid body. Badaboom badabing, vehicle done! :)
Here is the entire source code for the project. If you have any questions or comments please
feel free to post them here and either I can make things more clear or maybe someone else could offer some better expertise. If you’d like you can email me at Kincaid05 on google's fine
emailing service.
Thanks for reading and I hope this was informative.
-Matt Kincaid