/// <summary>
/// This function will pick an arbitrary orthogonal to the vector passed in. This will only be usefull if you are going
/// to rotate 180
/// </summary>
public static MyVector GetArbitraryOrhonganal(MyVector vector)
{
// Clone the vector passed in
MyVector retVal = vector.Clone();
// Make sure that none of the values are equal to zero.
if (retVal.X == 0)
{
retVal.X = 0.000000001d;
}
if (retVal.Y == 0)
{
retVal.Y = 0.000000001d;
}
if (retVal.Z == 0)
{
retVal.Z = 0.000000001d;
}
// Figure out the orthogonal X and Y slopes
double orthM = (retVal.X * -1) / retVal.Y;
double orthN = (retVal.Y * -1) / retVal.Z;
// When calculating the new coords, I will default Y to 1, and find an X and Z that satisfy that. I will go ahead and reuse the retVal
retVal.Y = 1;
retVal.X = 1 / orthM;
retVal.Z = orthN;
// Exit Function
return(retVal);
}
/// <summary>
/// This function returns a vector that is rotated by the opposite of me
/// </summary>
public MyVector GetRotatedVectorReverse(MyVector vector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return(myUnitClone.GetRotatedVectorReverse(vector, true));
}
MyVector qvec = new MyVector(this.X, this.Y, this.Z);
//Vector uv = qvec.Cross(vector);
MyVector uv = MyVector.Cross(qvec, vector);
//Vector uuv = qvec.Cross(uv);
MyVector uuv = MyVector.Cross(qvec, uv);
//uv *= (2.0f * quat.w);
uv.Multiply(this.W * -2d);
//uuv *= 2.0f;
uuv.Multiply(2d);
//return vector + uv + uuv;
MyVector retVal = vector.Clone();
retVal.Add(uv);
retVal.Add(uuv);
return(retVal);
}
/// <summary>
/// This creates a new vector that is the vector with a length of one
/// </summary>
public static MyVector BecomeUnitVector(MyVector vector)
{
MyVector retVal = vector.Clone();
retVal.BecomeUnitVector();
return(retVal);
}
/// <summary>
/// This function will calculate the new velocity based on the current acceleration and elapsed time. I will store the result back into
/// my velocity property.
/// </summary>
private void TimerSprtVel(double elapsedTime)
{
// Clone the current accel
MyVector changeInVel = _acceleration.Clone();
// Now make the ChangeInVel a velocity instead of an acceleration by multiplying it by time
changeInVel.Multiply(elapsedTime);
// I have the change in velocity, so I will add it to the current velocity in order to make a new velocity
_velocity.Add(changeInVel);
}
/// <summary>
/// This function will compute the new acceleration based on the force pushing against the ball. The result will overwrite the
/// current acceleration
/// </summary>
private void TimerSprtAccel(MyVector force)
{
// Make the PartialAccel into a clone of the force passed in
MyVector partialAccel = force.Clone();
// I will divide by mass, and it will no longer be force, it will be change in accel (the property guarantees that mass
// isn't zero)
partialAccel.Divide(_mass);
// Add the partial accel to this ball's accel
_acceleration.Add(partialAccel);
}
private static void SplitForceIntoTranslationAndTorque(out MyVector translationForce, out MyVector torque, MyVector centerOfMass, MyVector offset, MyVector force)
{
// The offset passed in is relative to position. I need it to be relative to the center of mass
MyVector trueOffset = offset - centerOfMass;
// Torque is how much of the force is applied perpendicular to the radius
torque = MyVector.Cross(trueOffset, force);
// I'm still not convinced this is totally right, but none of the articles I've read seem to do anything
// different
translationForce = force.Clone();
}
public override void TimerTestPosition(double elapsedTime)
{
base.TimerTestPosition(elapsedTime);
// Either remember the orig rotation, or restore to that orig rotation (as of PrepareForNew)
if (_savedRotation == null)
{
_savedRotation = this.Rotation.Clone();
}
else
{
this.Rotation.StoreNewValues(_savedRotation);
}
// Remember the elapsed time that was passed in
_elapsedTime = elapsedTime;
// Figure out the new rotation (the body has been rotating at some velocity during the previous tick)
if (!_angularVelocity.IsZero)
{
// Figure out what the angle will be
double timedAngle = _angularVelocity.GetMagnitude();
timedAngle *= elapsedTime;
if (!_centerOfMass.IsZero)
{
#region Rotate around center of mass
// Remember where the center of mass is in world coords
MyVector cmRotated = this.Rotation.GetRotatedVector(_centerOfMass, true);
MyVector cmWorld = this.Position + cmRotated;
// Get the opposite of the cm
MyVector posRelativeToCM = cmRotated.Clone();
posRelativeToCM.Multiply(-1d);
// Rotate the center of position around the center of mass
posRelativeToCM.RotateAroundAxis(_angularVelocity, timedAngle);
// Now figure out the new center of position
this.Position.X = cmWorld.X + posRelativeToCM.X;
this.Position.Y = cmWorld.Y + posRelativeToCM.Y;
this.Position.Z = cmWorld.Z + posRelativeToCM.Z;
#endregion
}
// Rotate myself
this.RotateAroundAxis(_angularVelocity, timedAngle);
}
}
private void button1_Click(object sender, EventArgs e)
{
MyVector v1 = new MyVector(3, 4, 5);
v1.Add(1, 2, 3);
v1.BecomeUnitVector();
MyVector v2 = v1.Clone();
v2.Multiply(3);
v1.Divide(3);
}
private void ApplyTorque(double elapsedTime)
{
// Calculate the new angular momentum (current + (torque * time))
MyVector newMomentum = _internalTorque.Clone();
newMomentum.Multiply(elapsedTime);
_angularMomentum.Add(newMomentum);
// Figure out the inverse of the world frame's inertia tensor
// (Rotation * bodyInertialTensorInverse * Transposed Rotation)
MyMatrix3 curRotation = base.RotationMatrix.Clone();
MyMatrix3 inverseWorldInertiaTensor = MyMatrix3.Multiply(MyMatrix3.Multiply(curRotation, _inertialTensorBodyInverse), MyMatrix3.Transpose(curRotation));
// Now all that's left is to figure out the new angular velocity
_angularVelocity.StoreNewValues(MyMatrix3.Multiply(inverseWorldInertiaTensor, _angularMomentum));
}
/// <summary>
/// This function returns the addition of internal force and external force. Before I do that, I need to rotate a copy of internal
/// force so that it is based off of the same orientation as the external force.
/// </summary>
private MyVector TimerSprtCombineForce()
{
if (_internalForce.IsZero)
{
// Internal force is zero, so there is nothing to rotate (or add)
return(_externalForce.Clone());
}
// The sphere's rotation quaternion already knows how to rotate from internal to external. So make a copy of
// internal line up with external.
MyVector retVal = base.Rotation.GetRotatedVector(_internalForce, true);
// retVal holds the internal force in the same orientation as the external force. I will add external force to it, and retVal
// will hold the net force placed on the ball
retVal.Add(_externalForce);
// Exit Function
return(retVal);
}
private void trackBar1_Scroll(object sender, EventArgs e)
{
MyVector thrusterSeed = new MyVector(0, _ship.Radius, 0);
MyVector zAxis = new MyVector(0, 0, 1);
double angle = trackBar1.Value;
// Bottom Thrusters
_shipThrusterOffset_BottomRight = thrusterSeed.Clone();
_shipThrusterOffset_BottomRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(angle * -1));
_shipThrusterOffset_BottomLeft = thrusterSeed.Clone();
_shipThrusterOffset_BottomLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(angle));
// Top Thrusters
thrusterSeed = new MyVector(0, _ship.Radius * -1, 0);
_shipThrusterOffset_TopRight = thrusterSeed.Clone();
_shipThrusterOffset_TopRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(angle));
_shipThrusterOffset_TopLeft = thrusterSeed.Clone();
_shipThrusterOffset_TopLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(angle * -1));
}
private void PropsChangedSprtThrusters()
{
if (_type != ShipTypeQual.SolidBall)
{
return; // the ball just has the thruster in the center
}
MyVector thrusterSeed = new MyVector(0, _ship.Ball.Radius, 0);
MyVector zAxis = new MyVector(0, 0, 1);
// Bottom Thrusters
_thrusterOffset_BottomRight = thrusterSeed.Clone();
_thrusterOffset_BottomRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(_thrusterAngle * -1));
_thrusterOffset_BottomLeft = thrusterSeed.Clone();
_thrusterOffset_BottomLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(_thrusterAngle));
// Top Thrusters
thrusterSeed = new MyVector(0, _ship.Ball.Radius * -1, 0);
_thrusterOffset_TopRight = thrusterSeed.Clone();
_thrusterOffset_TopRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(_thrusterAngle));
_thrusterOffset_TopLeft = thrusterSeed.Clone();
_thrusterOffset_TopLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(_thrusterAngle * -1));
}
void picturebox_MouseDown(object sender, MouseEventArgs e)
{
if (_mode == AddingMode.Inactive)
{
return;
}
if (e.Button == MouseButtons.Left)
{
_isMouseDown = true;
_mouseDownPoint = _picturebox.GetPositionViewToWorld(new MyVector(e.X, e.Y, 0));
_curMousePoint = _mouseDownPoint.Clone();
_lastCreateTime = Environment.TickCount;
_createdBallDuringMouseDrag = false;
//_diminishPercent = 1d;
if (_newBallProps.SizeMode == BallProps.SizeModes.Draw)
{
// I need to create an object now (but don't commit it to the map), so that the user can see
// it while they drag the size
_drawingBall = BuildObject();
}
}
}
/// <summary>
/// This function will pick an arbitrary orthogonal to the vector passed in. This will only be usefull if you are going
/// to rotate 180
/// </summary>
public static MyVector GetArbitraryOrhonganal(MyVector vector)
{
// Clone the vector passed in
MyVector retVal = vector.Clone();
// Make sure that none of the values are equal to zero.
if (retVal.X == 0) retVal.X = 0.000000001d;
if (retVal.Y == 0) retVal.Y = 0.000000001d;
if (retVal.Z == 0) retVal.Z = 0.000000001d;
// Figure out the orthogonal X and Y slopes
double orthM = (retVal.X * -1) / retVal.Y;
double orthN = (retVal.Y * -1) / retVal.Z;
// When calculating the new coords, I will default Y to 1, and find an X and Z that satisfy that. I will go ahead and reuse the retVal
retVal.Y = 1;
retVal.X = 1 / orthM;
retVal.Z = orthN;
// Exit Function
return retVal;
}
/// <summary>
/// This creates a new vector that is the vector with a length of one
/// </summary>
public static MyVector BecomeUnitVector(MyVector vector)
{
MyVector retVal = vector.Clone();
retVal.BecomeUnitVector();
return retVal;
}
private void chkIncludeShip_CheckedChanged(object sender, EventArgs e)
{
const double THRUSTERANGLE = 75;
if (chkIncludeShip.Checked)
{
if (_ship == null)
{
#region Create Ship
// Set up the ship
double radius = MINRADIUSMASS + (_rand.NextDouble() * (MAXRADIUSMASS - MINRADIUSMASS));
SolidBall ship = new SolidBall(Utility3D.GetRandomVector(_boundryLower, _boundryUpper), new DoubleVector(0, 1, 0, 1, 0, 0), radius, GetMass(radius), GetElasticity(), 1, 1, _boundryLower, _boundryUpper);
// Set up the thrusters
MyVector thrusterSeed = new MyVector(0, ship.Radius, 0);
MyVector zAxis = new MyVector(0, 0, 1);
// Bottom Thrusters
_shipThrusterOffset_BottomRight = thrusterSeed.Clone();
_shipThrusterOffset_BottomRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(THRUSTERANGLE * -1));
_shipThrusterOffset_BottomLeft = thrusterSeed.Clone();
_shipThrusterOffset_BottomLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(THRUSTERANGLE));
// Top Thrusters
thrusterSeed = new MyVector(0, ship.Radius * -1, 0);
_shipThrusterOffset_TopRight = thrusterSeed.Clone();
_shipThrusterOffset_TopRight.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(THRUSTERANGLE));
_shipThrusterOffset_TopLeft = thrusterSeed.Clone();
_shipThrusterOffset_TopLeft.RotateAroundAxis(zAxis, Utility3D.GetDegreesToRadians(THRUSTERANGLE * -1));
// Add to the map
_ship = new BallBlip(ship, CollisionStyle.Standard, RadarBlipQual.BallUserDefined03, TokenGenerator.NextToken());
_map.Add(_ship);
#endregion
}
}
else
{
if (_ship != null)
{
_map.Remove(_ship.Token);
_ship = null;
}
}
}
private static void SplitForceIntoTranslationAndTorque(out MyVector translationForce, out MyVector torque, MyVector centerOfMass, MyVector offset, MyVector force)
{
// The offset passed in is relative to position. I need it to be relative to the center of mass
MyVector trueOffset = offset - centerOfMass;
// Torque is how much of the force is applied perpendicular to the radius
torque = MyVector.Cross(trueOffset, force);
// I'm still not convinced this is totally right, but none of the articles I've read seem to do anything
// different
translationForce = force.Clone();
}
public virtual Sphere Clone()
{
return(new Sphere(_position.Clone(), _origDirFacing.Clone(), _rotation.Clone(), _radius));
}
/// <summary>
/// This function creates a vector that is the vector passed in rotated by my definition
/// </summary>
/// <param name="vector">The vector to rotate (I don't touch this vector, I create a new one that is rotated)</param>
/// <param name="isQuatNormalized">Whether this class is already normalized or not (if you don't know, pass false)</param>
public MyVector GetRotatedVector(MyVector vector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return myUnitClone.GetRotatedVector(vector, true);
}
MyVector qvec = new MyVector(this.X, this.Y, this.Z);
//Vector uv = qvec.Cross(vector);
MyVector uv = MyVector.Cross(qvec, vector);
//Vector uuv = qvec.Cross(uv);
MyVector uuv = MyVector.Cross(qvec, uv);
//uv *= (2.0f * quat.w);
uv.Multiply(this.W * 2d);
//uuv *= 2.0f;
uuv.Multiply(2d);
//return vector + uv + uuv;
MyVector retVal = vector.Clone();
retVal.Add(uv);
retVal.Add(uuv);
return retVal;
}
private MyVector GetSpinVelocityAtPoint(ref AngularVelocityInfo angularInfo, out MyVector dirToCenterLine, MyVector dirFacingWorld, MyVector lineBetween, MyVector blipPosition)
{
// Get a line that's orthogonal to lineBetween, and always points toward the dirFacingWorld vector
dirToCenterLine = MyVector.Cross(MyVector.Cross(lineBetween, dirFacingWorld), lineBetween);
dirToCenterLine.BecomeUnitVector();
if (angularInfo == null)
{
#region Cache Angular Velocity
angularInfo = new AngularVelocityInfo();
if (_ship.TorqueBall != null)
{
angularInfo.AngularVelocity = _ship.TorqueBall.AngularVelocity.GetMagnitude();
angularInfo.SpinDirection = MyVector.Cross(_ship.TorqueBall.AngularVelocity, _ship.TorqueBall.DirectionFacing.Standard);
angularInfo.SpinDirection.BecomeUnitVector();
angularInfo.CenterMass = _ship.TorqueBall.Rotation.GetRotatedVector(_ship.TorqueBall.CenterOfMass, true);
angularInfo.CenterMass.Add(_ship.TorqueBall.Position);
}
else
{
angularInfo.SpinDirection = dirToCenterLine.Clone();
angularInfo.AngularVelocity = 0d;
angularInfo.CenterMass = _ship.Ball.Position.Clone();
}
#endregion
}
// Get the line between the blip and the center of mass
MyVector lineBetweenCM = blipPosition - angularInfo.CenterMass;
// Figure out my velocity of spin where the blip is
return angularInfo.SpinDirection * (angularInfo.AngularVelocity * lineBetweenCM.GetMagnitude());
}
/// <summary>
/// This function tells me to look at the point passed in, but keep looking at where it is now, not actively track it.
/// I will store the clone of it, so you don't have to make a clone before you pass me the pointer to a position
/// </summary>
public void StaticPoint(MyVector point)
{
_centerPoint = point.Clone();
}
void picturebox_MouseDown(object sender, MouseEventArgs e)
{
if (!_active)
{
return;
}
if (e.Button == MouseButtons.Left || e.Button == MouseButtons.Right)
{
_mouseDownPoint = _picturebox.GetPositionViewToWorld(new MyVector(e.X, e.Y, 0));
_curMousePoint = _mouseDownPoint.Clone();
_prevMousePositions.Clear();
_tempStationaryObjects.Clear(); // this should be cleared by now anyway
}
if (e.Button == MouseButtons.Left)
{
#region Left
_isMouseDown = MouseButtonDown.Left;
// Get all the blips
List<RadarBlip> remainingBlips = new List<RadarBlip>(_map.GetAllBlips());
bool selectedPrevious = false;
#region See if they selected one of the previously selected objects
foreach (long token in _selectedObjects)
{
RadarBlip blip = FindAndRemove(token, remainingBlips);
if (blip == null)
{
continue;
}
if (selectedPrevious)
{
// I just wanted to remove this blip from the total list
continue;
}
if (SelectionTest(blip, _curMousePoint))
{
selectedPrevious = true;
}
}
#endregion
// Check for ctrl or shift key being pressed (if they are, don't clear the previous)
if (!selectedPrevious && !(_isShiftPressed || _isCtrlPressed))
{
_selectedObjects.Clear();
}
bool selectedNew = false;
#region See if they clicked on any other objects
foreach (RadarBlip blip in remainingBlips)
{
if (SelectionTest(blip, _curMousePoint))
{
_selectedObjects.Add(blip.Token);
selectedNew = true;
}
}
#endregion
// Set my mode
if (selectedPrevious || selectedNew)
{
_mode = SelectionMode.Selected;
#region Rebuild the offsets list (and temp stationary)
_draggingPositionOffsets.Clear();
foreach (RadarBlip blip in _map.GetAllBlips())
{
if (_selectedObjects.Contains(blip.Token))
{
_draggingPositionOffsets.Add(blip.Token, blip.Sphere.Position - _curMousePoint);
if (blip.CollisionStyle == CollisionStyle.Standard)
{
_tempStationaryObjects.Add(blip.Token);
blip.CollisionStyle = CollisionStyle.Stationary;
}
}
}
#endregion
}
else
{
_mode = SelectionMode.Rectangle;
}
#endregion
}
else if (e.Button == MouseButtons.Right)
{
#region Right
if (_mode == SelectionMode.Selected && _selectedObjects.Count > 0)
{
_isMouseDown = MouseButtonDown.Right;
}
else
{
// If nothing is selected, then there is nothing for me to do
_isMouseDown = MouseButtonDown.None;
}
#endregion
}
}
protected void GetCollisionNormalAndPointsOfContact_SphereSphere(out MyVector normal, out double normalMagnitude, out MyVector pointOfContact1, out MyVector pointOfContact2, BallBlip ball1, BallBlip ball2)
{
// Vector that is perpendicular to the tangent of the collision, and it points in the direction of object 1. Real
// easy when dealing with spheres :)
normal = ball2.Ball.Position - ball1.Ball.Position;
// Remember this length
normalMagnitude = normal.GetMagnitude();
// This needs to be returned as a unit vector
normal.Divide(normalMagnitude);
// Start them off as unit vectors
pointOfContact1 = normal.Clone();
pointOfContact2 = normal.Clone();
// Finish (use the ratio of their radii)
pointOfContact1.Multiply((ball1.Ball.Radius / (ball1.Ball.Radius + ball2.Ball.Radius)) * normalMagnitude);
pointOfContact2.Multiply((ball2.Ball.Radius / (ball1.Ball.Radius + ball2.Ball.Radius)) * normalMagnitude * -1); // I want this one pointing the other direction
// Now that I have the points of contact relative to the centers of position, I need to make them
// relative to the centers of mass
if (ball1.TorqueBall != null)
{
pointOfContact1.Subtract(ball1.TorqueBall.CenterOfMass);
}
if (ball2.TorqueBall != null)
{
pointOfContact2.Subtract(ball2.TorqueBall.CenterOfMass);
}
}
/// <summary>
/// If intersecting, this will return the point of intersection. Otherwise null
/// </summary>
/// <param name="lineStartPoint">Starting point of the line</param>
/// <param name="lineDirection">direction and length of the line</param>
/// <param name="triangle">This represents the plane (3 arbitrary points on the plane)</param>
/// <param name="limitToLineSegment">If false, the line passed in is thought of as infinitely long</param>
/// <returns>point of intersection or null</returns>
public static MyVector IsIntersecting_LinePlane(MyVector lineStartPoint, MyVector lineDirection, Triangle triangle, bool limitToLineSegment)
{
MyVector normal = triangle.Normal;
double denominator = MyVector.Dot(normal, lineDirection);
if (Utility3D.IsNearZero(denominator)) // parallel to the triangle's plane
{
return null;
}
double percentAlongLine = (triangle.DistanceFromOriginAlongNormal - MyVector.Dot(normal, lineStartPoint)) / denominator;
if (limitToLineSegment && (percentAlongLine < 0d || percentAlongLine > 1d))
{
// The ray is intersecting, but they want the line segment only
return null;
}
// Calculate the point along the line
MyVector retVal = lineDirection.Clone();
double length = retVal.GetMagnitude();
retVal.Divide(length);
retVal.Multiply(percentAlongLine * length);
retVal.Add(lineStartPoint);
// Exit Function
return retVal;
}
/// <summary>
/// If the user clicks on a picture box, you need to run those coords through this function to figure out where they
/// clicked in world coords
/// </summary>
/// <param name="position">A point in PictureBox coords</param>
/// <returns>The point in World Coords</returns>
public MyVector GetPositionViewToWorld(MyVector position)
{
MyVector retVal = position.Clone();
// Figure out the world coords that the top left of the picturebox represents
double picLeft = _centerPoint.X - ((this.Width / 2d) / _zoom);
double picTop = _centerPoint.Y - ((this.Height / 2d) / _zoom);
// The point passed in is a distance from the top left of the picture box to where they clicked. Turn this view
// coords into world coords
retVal.Divide(_zoom);
// Add these world coords to the top left of the picture box
retVal.X += picLeft;
retVal.Y += picTop;
// It's now what it needs to be
return retVal;
}
/// <summary>
/// This function will compute the new acceleration based on the force pushing against the ball. The result will overwrite the
/// current acceleration
/// </summary>
private void TimerSprtAccel(MyVector force)
{
// Make the PartialAccel into a clone of the force passed in
MyVector partialAccel = force.Clone();
// I will divide by mass, and it will no longer be force, it will be change in accel (the property guarantees that mass
// isn't zero)
partialAccel.Divide(_mass);
// Add the partial accel to this ball's accel
_acceleration.Add(partialAccel);
}