public Vector(Vector vec)
{
x = vec.x;
y = vec.y;
z = vec.z;
w = vec.w;
}
//keep the vehicle on the screen
private void ConstrainVehicle()
{
Vector position = vehicle.GetPosition();
Vector screenSize = new Vector(screen.Width / screenScale, screen.Height / screenScale);
while (position.X > screenSize.X / 2.0f) { position.X -= screenSize.X; }
while (position.Y > screenSize.Y / 2.0f) { position.Y -= screenSize.Y; }
while (position.X < -screenSize.X / 2.0f) { position.X += screenSize.X; }
while (position.Y < -screenSize.Y / 2.0f) { position.Y += screenSize.Y; }
}
//cross product
public static Vector operator %(Vector L, Vector R)
{
Vector temp = new Vector(L.y * R.z - L.z * R.y, L.z * R.x - L.x * R.z, L.x * R.y - L.y * R.x);
return temp;
}
//transform by matrix
public static Vector operator *(Vector L, Matrix R)
{
Vector temp = new Vector();
temp.x = L.x * R.m11 + L.y * R.m12 + L.z * R.m13 + R.m14;
temp.y = L.x * R.m21 + L.y * R.m22 + L.z * R.m23 + R.m24;
temp.z = L.x * R.m31 + L.y * R.m32 + L.z * R.m33 + R.m34;
temp.w = L.x * R.m41 + L.y * R.m42 + L.z * R.m43 + R.m44;
return temp;
}
public void AddForce(Vector worldForce, Vector worldOffset)
{
//add linar force
m_forces += worldForce;
//and it's associated torque
m_torque += worldOffset % worldForce;
}
public new void Setup(Vector halfSize, float mass, Color color)
{
//front wheels
wheels[0] = new Wheel(new Vector(halfSize.X, halfSize.Y), 0.5f);
wheels[1] = new Wheel(new Vector(-halfSize.X, halfSize.Y), 0.5f);
//rear wheels
wheels[2] = new Wheel(new Vector(halfSize.X, -halfSize.Y), 0.5f);
wheels[3] = new Wheel(new Vector(-halfSize.X, -halfSize.Y), 0.5f);
base.Setup(halfSize, mass, color);
}
//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);
}
public void Set(Vector vec)
{
x = vec.x;
y = vec.y;
z = vec.z;
w = vec.w;
}
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;
}
//operators
//subtraction
public static Vector operator -(Vector L, Vector R)
{
Vector temp = new Vector(L.x - R.x, L.y - R.y, L.z - R.z);
return temp;
}
//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);
}
public Wheel(Vector position, float radius)
{
m_Position = position;
SetSteeringAngle(0);
m_wheelSpeed = 0;
m_wheelRadius = radius;
m_wheelInertia = radius * radius; //fake value
}
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
}
//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;
}
//multiply by scalar
public static Vector operator *(Vector L, float R)
{
Vector temp = new Vector();
temp.x = L.x * R;
temp.y = L.y * R;
temp.z = L.z * R;
return temp;
}
//divide by scalar
public static Vector operator /(Vector L, float R)
{
Vector temp = new Vector();
temp.x = L.x / R;
temp.y = L.y / R;
temp.z = L.z / R;
return temp;
}
//addition
public static Vector operator +(Vector L, Vector R)
{
Vector temp = new Vector(L.x + R.x, L.y + R.y, L.z + R.z);
return temp;
}
//project this vector on to v
public Vector Project(Vector v)
{
//projected vector = (this dot v) * v;
float thisDotV = this * v;
return v * thisDotV;
}
//negative
public static Vector operator -(Vector R)
{
Vector temp = new Vector(-R.x, -R.y, -R.z);
return temp;
}
//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;
}
//component wise multiplication
public Vector CompMult(Vector v)
{
Vector temp = new Vector();
temp.x = x * v.x;
temp.y = y * v.y;
temp.z = z * v.z;
temp.w = w * v.w;
return temp;
}
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);
}
//reflect this vector over n
public Vector Reflect(Vector n)
{
float angle;
Vector nTemp = new Vector(n);
Vector vTemp = this;
angle = (vTemp * n) * 2; //v dot n * 2
nTemp *= angle;
return (vTemp - nTemp);
}
//negative
public static Vector operator -(Vector R)
{
Vector temp = new Vector(-R.X, -R.Y);
return temp;
}
//project this vector on to v, return signed magnatude
public Vector Project(Vector v, out float mag)
{
//projected vector = (this dot v) * v;
float thisDotV = this * v;
mag = thisDotV;
return v * thisDotV;
}
/// <summary>
///добавление силы
/// </summary>
/// <param name="worldForce"></param>
/// <param name="worldOffset"></param>
public void AddForce(Vector worldForce, Vector worldOffset)
{
//линейную силу
//add linar force
m_forces += worldForce;
//это связано момент
//and it's associated torque
m_torque += worldOffset % worldForce;
}
