private void HandleCollision(MoveBall b1, MoveBall b2)
{ //collision detected: the collision has happened anytime between the previous frame and this one.
//find how much time has past since the the collision
MyVector3 relVelocity = MoveBall.RelativeVelocity(b2, b1); // V° rel velocity (const)
MyVector3 relPosition = MoveBall.RelativePosition(b2, b1); // P° rel position at t=0
// relative position P(t) = P° - V° * t
// the collision happens when |P(t)| = 2 * radius
// <P(t)|P(t)> = <(P° - V° * t)|(P° - V° * t)> = 4 * radius^2
// solve the above quadratic eq. for t
float term1 = MyVector3.Dot(relVelocity, relPosition);
float term2 = MyVector3.Dot(relPosition, relPosition);
float term3 = MyVector3.Dot(relVelocity, relVelocity);
float rootSquared = term1 * term1 - term3 * (term2 - 4 * SnookerBall.radius * SnookerBall.radius);
if (rootSquared < 0)
{
rootSquared = 0;
}
float timeAfterCollision = (term1 + Mathf.Sqrt(rootSquared)) / term3;
b1.MoveByVector(b1.GetVelocity().Scale(-timeAfterCollision)); //minus sign because we are moving the ball back in time
b2.MoveByVector(b2.GetVelocity().Scale(-timeAfterCollision)); //minus sign because we are moving the ball back in time
relPosition = MoveBall.RelativePosition(b2, b1); //vector joining the centers of the 2 balls
MyVector3 b1_parallel = b1.GetLinearMomentum().ParallelComponent(relPosition);
MyVector3 b1_perpendicular = b1.GetLinearMomentum().NormalComponent(relPosition);
MyVector3 b2_parallel = b2.GetLinearMomentum().ParallelComponent(relPosition);
MyVector3 b2_perpendicular = b2.GetLinearMomentum().NormalComponent(relPosition);
//the two ball exchange the parallel components of their linear momenta
//this is only valid in the case of the masses being the same
b1.SetLinearMomentum(MyVector3.Add(b1_perpendicular, b2_parallel));
b2.SetLinearMomentum(MyVector3.Add(b2_perpendicular, b1_parallel));
}
// https://blackpawn.com/texts/pointinpoly/
bool IsSameSide(MyVector3 p1, MyVector3 p2, MyVector3 a, MyVector3 b)
{
var cp1 = MyVector3.Cross(b - a, p1 - a);
var cp2 = MyVector3.Cross(b - a, p2 - a);
return(MyVector3.Dot(cp1, cp2) >= 0);
}
/// <summary>
/// Calculates the matrix required to transfer any point from one <see cref="MyOrientedBoundingBox"/> local coordinates to another.
/// </summary>
/// <param name="A">The source OrientedBoundingBox.</param>
/// <param name="B">The target OrientedBoundingBox.</param>
/// <param name="NoMatrixScaleApplied">
/// If true, the method will use a fast algorithm which is inapplicable if a scale is applied to the transformation matrix of the OrientedBoundingBox.
/// </param>
/// <returns></returns>
public static MyMatrix GetBoxToBoxMatrix(ref MyOrientedBoundingBox A, ref MyOrientedBoundingBox B, bool NoMatrixScaleApplied = false)
{
MyMatrix AtoB_Matrix;
// Calculate B to A transformation matrix
if (NoMatrixScaleApplied)
{
var RotA = GetRows(ref A.Transformation);
var RotB = GetRows(ref B.Transformation);
AtoB_Matrix = new MyMatrix();
int i, k;
for (i = 0; i < 3; i++)
for (k = 0; k < 3; k++)
AtoB_Matrix[i, k] = MyVector3.Dot(RotB[i], RotA[k]);
var v = B.Center - A.Center;
AtoB_Matrix.M41 = MyVector3.Dot(v, RotA[0]);
AtoB_Matrix.M42 = MyVector3.Dot(v, RotA[1]);
AtoB_Matrix.M43 = MyVector3.Dot(v, RotA[2]);
AtoB_Matrix.M44 = 1;
}
else
{
MyMatrix AInvMat;
MyMatrix.Invert(ref A.Transformation, out AInvMat);
AtoB_Matrix = B.Transformation * AInvMat;
}
return AtoB_Matrix;
}
//define plane from its normal and a point in the plane
public Plane(MyVector3 normal, MyVector3 point)
{
normal = normal.UnitVector();
xCoeff = normal.getX();
yCoeff = normal.getY();
zCoeff = normal.getZ();
constTerm = -MyVector3.Dot(normal, point);
}
private static MyVector3 Get3PlanesInterPoint(ref MyPlane p1, ref MyPlane p2, ref MyPlane p3)
{
//P = -d1 * N2xN3 / N1.N2xN3 - d2 * N3xN1 / N2.N3xN1 - d3 * N1xN2 / N3.N1xN2
MyVector3 v =
-p1.D * MyVector3.Cross(p2.Normal, p3.Normal) / MyVector3.Dot(p1.Normal, MyVector3.Cross(p2.Normal, p3.Normal))
- p2.D * MyVector3.Cross(p3.Normal, p1.Normal) / MyVector3.Dot(p2.Normal, MyVector3.Cross(p3.Normal, p1.Normal))
- p3.D * MyVector3.Cross(p1.Normal, p2.Normal) / MyVector3.Dot(p3.Normal, MyVector3.Cross(p1.Normal, p2.Normal));
return(v);
}
private void FixedUpdate()
{
//simulation starts upon pressing the spacebar
if (Input.GetKeyDown(KeyCode.Space) == true)
{
startSimulation = true;
}
if (!startSimulation)
{
return;
}
float t = Time.fixedDeltaTime;
MyVector3 previousLinearMomentum = linearMomentum;
MyVector3 linearMomentumVariation = linearMomentum.UnitVector().Scale(
-SnookerBall.NormalForce * SnookerBall.frictionCoeff * t);
if (linearMomentumVariation.Magnitude() >= linearMomentum.Magnitude())
{//the decrease in linear momentum is bigger than the linear momentum itself
linearMomentum = MyVector3.Zero();
}
else
{
linearMomentum = MyVector3.Add(linearMomentum, linearMomentumVariation);
}
MyVector3 avgLinearMomentum = MyVector3.Add(linearMomentum, previousLinearMomentum).Scale(0.5f);
MyVector3 avgVelocity = avgLinearMomentum.Scale(1 / SnookerBall.mass);
MyVector3 newPosition = MyVector3.Add(position, avgVelocity.Scale(t));
foreach (Plane plane in cushions)
{
float distanceFromPlane = newPosition.DistanceFromPlane(plane) - SnookerBall.radius;
if (distanceFromPlane < 0)
{//the ball has gone beyond the boundary
//collision detected: the collision has happened anytime between the previous frame and this one.
//find how much time has past since the collision
float impactTime =
ImpactTime(
MyVector3.Dot(previousLinearMomentum, plane.Normal()),
-MyVector3.Dot(avgLinearMomentum.UnitVector(), plane.Normal()) * SnookerBall.NormalForce * SnookerBall.frictionCoeff,
position.DistanceFromPlane(plane) - SnookerBall.radius);
linearMomentumVariation = previousLinearMomentum.UnitVector().Scale(
-SnookerBall.NormalForce * SnookerBall.frictionCoeff * impactTime);
//reflect the linear momentum along the normal of the plane
linearMomentum = MyVector3.Add(previousLinearMomentum, linearMomentumVariation).Reflect(plane.Normal());
//position ball just within the boundaries
newPosition = MyVector3.Add(newPosition, plane.Normal().Scale(-distanceFromPlane));
}
}
position = newPosition;
MoveGameObject();
}
/// <summary>
/// Extracts perspective camera parameters from the frustum, doesn't work with orthographic frustums.
/// </summary>
/// <returns>Perspective camera parameters from the frustum</returns>
public MyFrustumCameraParams GetCameraParams()
{
var corners = GetCorners();
var cameraParam = new MyFrustumCameraParams();
cameraParam.Position = Get3PlanesInterPoint(ref pRight, ref pTop, ref pLeft);
cameraParam.LookAtDir = pNear.Normal;
cameraParam.UpDir = MyVector3.Normalize(MyVector3.Cross(pRight.Normal, pNear.Normal));
cameraParam.FOV = (float)((Math.PI / 2.0 - Math.Acos(MyVector3.Dot(pNear.Normal, pTop.Normal))) * 2);
cameraParam.AspectRatio = (corners[6] - corners[5]).Length() / (corners[4] - corners[5]).Length();
cameraParam.ZNear = (cameraParam.Position + (pNear.Normal * pNear.D)).Length();
cameraParam.ZFar = (cameraParam.Position + (pFar.Normal * pFar.D)).Length();
return(cameraParam);
}
void LateUpdate()
{
//CONSTRAINTS
for (int i = jointsPositions.Length - 2; i >= 0; i--)
{
if (i > 0)
{
ToParent = new MyVector3(joints[i - 1].position.x - joints[i].position.x, joints[i - 1].position.y - joints[i].position.y, joints[i - 1].position.z - joints[i].position.z).normalized();
ToChild = new MyVector3(joints[i + 1].position.x - joints[i].position.x, joints[i + 1].position.y - joints[i].position.y, joints[i + 1].position.z - joints[i].position.z).normalized();
axis = MyVector3.Cross(ToParent, ToChild).normalized();
float angle = Mathf.Acos(MyVector3.Dot(ToParent, ToChild) / (ToParent.magnitude() * ToChild.magnitude())) * Mathf.Rad2Deg;
if (angle > maxAngle || angle < ((i == 1) ? minAngle * 4 : minAngle))
{
angle = Mathf.Clamp(angle, ((i == 1) ? minAngle * 4 : minAngle), maxAngle);
joints[i].rotation = joints[i - 1].rotation;
joints[i].Rotate(new Vector3(axis.x, axis.y, axis.z), 180 + angle, Space.World);
}
}
else if (i == 0)
{
ToParent = new MyVector3(0, -1, 0);
ToChild = new MyVector3(joints[i + 1].position.x - joints[i].position.x, joints[i + 1].position.y - joints[i].position.y, joints[i + 1].position.z - joints[i].position.z).normalized();
axis = MyVector3.Cross(ToParent, ToChild).normalized();
float angle = Mathf.Acos(MyVector3.Dot(ToParent, ToChild) / (ToParent.magnitude() * ToChild.magnitude())) * Mathf.Rad2Deg;
if (angle > maxAngle + maxAngle / 4 || angle < maxAngle - maxAngle / 4)
{
angle = Mathf.Clamp(angle, maxAngle - maxAngle / 4, maxAngle + maxAngle / 4);
joints[i].rotation = Quaternion.identity;
joints[i].Rotate(new Vector3(axis.x, axis.y, axis.z), 240 + angle, Space.World);
}
}
}
}
public void Calculate()
{
try
{//parse strings into floating point numbers
float v1_x = Single.Parse(v1_inputs[0].text);
float v1_y = Single.Parse(v1_inputs[1].text);
float v1_z = Single.Parse(v1_inputs[2].text);
float v2_x = Single.Parse(v2_inputs[0].text);
float v2_y = Single.Parse(v2_inputs[1].text);
float v2_z = Single.Parse(v2_inputs[2].text);
float num = Single.Parse(scalar_input.text);
v1 = new MyVector3(v1_x, v1_y, v1_z);
v2 = new MyVector3(v2_x, v2_y, v2_z);
scalar = num;
}
catch (Exception e)
{//parsing failed
Debug.LogError(e.Message);
v1_inputs[0].text = "";
v1_inputs[1].text = "";
v1_inputs[2].text = "";
v2_inputs[0].text = "";
v2_inputs[1].text = "";
v2_inputs[2].text = "";
scalar_input.text = "";
return;
}
//print vectors in the scene
add.text = MyVector3.Add(v1, v2).Print();
subtract.text = MyVector3.Subtract(v1, v2).Print();
scale.text = v1.Scale(scalar).Print();
dot.text = MyVector3.Dot(v1, v2).ToString("0.00");
magnitude.text = v1.Magnitude().ToString("0.00");
unitVect.text = v1.UnitVector().Print();
reflectX.text = v1.ReflectX().Print();
reflectY.text = v1.ReflectY().Print();
reflectZ.text = v1.ReflectZ().Print();
zero.text = MyVector3.Zero().Print();
}
/// <summary>
/// Get the distance which when added to camera position along the lookat direction will do the effect of zoom to extents (zoom to fit) operation,
/// so all the passed points will fit in the current view.
/// if the returned value is positive, the camera will move toward the lookat direction (ZoomIn).
/// if the returned value is negative, the camera will move in the reverse direction of the lookat direction (ZoomOut).
/// </summary>
/// <param name="points">The points.</param>
/// <returns>The zoom to fit distance</returns>
public float GetZoomToExtentsShiftDistance(MyVector3[] points)
{
float vAngle = (float)((Math.PI / 2.0 - Math.Acos(MyVector3.Dot(pNear.Normal, pTop.Normal))));
float vSin = (float)Math.Sin(vAngle);
float hAngle = (float)((Math.PI / 2.0 - Math.Acos(MyVector3.Dot(pNear.Normal, pLeft.Normal))));
float hSin = (float)Math.Sin(hAngle);
float horizontalToVerticalMapping = vSin / hSin;
var ioFrustrum = GetInsideOutClone();
float maxPointDist = float.MinValue;
for (int i = 0; i < points.Length; i++)
{
float pointDist = MyCollision.DistancePlanePoint(ref ioFrustrum.pTop, ref points[i]);
pointDist = Math.Max(pointDist, MyCollision.DistancePlanePoint(ref ioFrustrum.pBottom, ref points[i]));
pointDist = Math.Max(pointDist, MyCollision.DistancePlanePoint(ref ioFrustrum.pLeft, ref points[i]) * horizontalToVerticalMapping);
pointDist = Math.Max(pointDist, MyCollision.DistancePlanePoint(ref ioFrustrum.pRight, ref points[i]) * horizontalToVerticalMapping);
maxPointDist = Math.Max(maxPointDist, pointDist);
}
return(-maxPointDist / vSin);
}
void Update()
{
// Copy the joints positions to work with
for (int i = 0; i < joints.Length; i++)
{
copy[i] = new MyVector3(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints
if (i < joints.Length - 1)
{
distances[i] = MyVector3.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances
}
}
done = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition;
if (!done)
{
float targetRootDist = MyVector3.Distance(copy[0], new MyVector3(target.position.x, target.position.y, target.position.z));
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVector3(target.position.x, target.position.y, target.position.z) - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + new MyVector3(target.position.x, target.position.y, target.position.z) * lambda;
}
}
else
{
MyVector3 b = copy[0];
float difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude;
// The target is reachable
while (difA > treshold_condition)
{
// STAGE 1: FORWARD REACHING
copy[copy.Length - 1] = new MyVector3(target.position.x, target.position.y, target.position.z);
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = copy[i + 1] * (1 - lambda) + copy[i] * lambda;
}
// STAGE 2: BACKWARD REACHING
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + copy[i + 1] * lambda;
}
difA = (copy[copy.Length - 1] - new MyVector3(target.position.x, target.position.y, target.position.z)).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
MyQuaternion parentQuat = new MyQuaternion(joints[i + 1].rotation);
MyQuaternion myQuat = new MyQuaternion(joints[i].rotation);
MyVector3 A = new MyVector3(joints[i + 1].position - joints[i].position);
MyVector3 B = copy[i + 1] - copy[i];
float cosa = MyVector3.Dot(MyVector3.Normalize(A), MyVector3.Normalize(B));
float sina = MyVector3.Cross(MyVector3.Normalize(A), MyVector3.Normalize(B)).magnitude;
float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
MyVector3 myAxis = MyVector3.Normalize(MyVector3.Cross(A, B));
//Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z);
myQuat = MyQuaternion.AngleAxis(alpha, ref myAxis);
Quaternion quat = new Quaternion(myQuat.x, myQuat.y, myQuat.z, myQuat.w);
joints[i].rotation = quat * joints[i].rotation;
//joints[i].rotation = Quaternion.AngleAxis(alpha, axis) * joints[i].rotation;
myQuat = new MyQuaternion(joints[i].rotation);
float localAngle = MyQuaternion.Angle(parentQuat, myQuat);
if (Mathf.Abs(localAngle) > maxRotation)
{
joints[i + 1].rotation = joints[i].rotation;
}
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
}
}
}
public void CylinderSnapping()
{
// Get Boundary2
if (boundaryPoints_2d == null)
{
boundaryPoints_2d = GetBoundaryPoints(mark);
}
List <MyVector2> boundary2 = ExtractOutline(edgeImage, boundaryPoints_2d);
// Project 2D edge points
//topCircle = new MyCircle(topCircle.Center, topCircle.Radius, -topCircle.Normal);
MyVector3 normal = topCircle.Normal.Cross(this.camera.target).Cross(topCircle.Normal);
MyPlane sectionPlane = new MyPlane(topCircle.Center, normal);
boundary3 = Proj2dToPlane(sectionPlane, boundary2);
topCircle = CiriFixTopCircle(topCircle, boundary3);
// UpdateCircleNormal
// foreach (var pbondary3 in pbondary3)
// {
// }
// if (topCircle.Center)
//{
//}
// Algorithm Init Params
double offset = topCircle.Radius / 50;
cur_p = topCircle.Center - offset * topCircle.Normal;
cur_dire = 1.0 * topCircle.Normal;
MyVector3 cur_dire_new = new MyVector3(cur_dire);
MyVector3 cur_p_new = new MyVector3(-1 * cur_p);
Insection1 = new MyVector3(1, 1, 1);
Insection2 = new MyVector3(0, 0, 0);
int norInsec = -1;
int notNorInsec = -1;
MyVector3 tangential1 = new MyVector3(1, 1, 1);
MyVector3 tangential2 = new MyVector3(1, 1, 1);
List <MyCircle> CircleLists = new List <MyCircle>();
CircleLists.Add(topCircle); // Fix first circle
int iter = 0;
double r = double.MaxValue;
System.Console.WriteLine(Insection1.Dot(tangential2));
System.Console.WriteLine(Math.Cos(2.0 / 3.0 * Math.PI));
int MaxInter = 1000;
GeneratedCenters = new List <MyVector3>();
List <double> radius = new List <double>();
List <double> weights = new List <double>();
List <MyVector3> dires = new List <MyVector3>();
while (--MaxInter > 0) //
{
if (Insection1 == Insection2) // 交点一直保持相同
{
System.Console.WriteLine("Warning: Insection is same!"); // 半径过小
break;
}
if (cur_dire.Dot(cur_dire_new) < 0) // 移动方向反向
{
System.Console.WriteLine("Warning: Move Direction！");
break;
}
if (cur_p + offset * cur_dire == cur_p_new) // 中心点没有移动
{
System.Console.WriteLine("Warning: Center not move!");
break;
}
RayTracein3DPlane(boundary3,
cur_p_new,
cur_dire_new.Cross(sectionPlane.Normal()),
sectionPlane.Normal(),
out norInsec,
out notNorInsec);
System.Console.WriteLine("{0} , {1}",
MyVector3.Distance(boundary3[norInsec], cur_p_new),
MyVector3.Distance(boundary3[notNorInsec], cur_p_new));
test1 = new Line3(boundary3[norInsec], cur_p_new - boundary3[norInsec]);
test2 = new Line3(boundary3[notNorInsec], cur_p_new - boundary3[notNorInsec]);
if (MyVector3.Distance(boundary3[norInsec], cur_p_new) < topCircle.Radius / 20 || // close to bottom
MyVector3.Distance(boundary3[notNorInsec], cur_p_new) < topCircle.Radius / 20)
{
System.Console.WriteLine("Warning: Close to bottom!");
break;
}
if (tangential1.Dot(tangential2) < Math.Cos(2.0 / 3.0 * Math.PI)) //切线相向
{
System.Console.WriteLine("Warning: tangential get oppsite direction!");
break;
}
if (r < 0.0001)
{
System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小
break;
}
//if (MyVector3.Distance(cur_p, cur_p_new) )
//{
// System.Console.WriteLine("Warning: Radius is too small!"); // 半径过小
// break;
//}
if (iter != 0)
{
//offset = 1 / MyVector3.Distance(cur_p, cur_p_new) * 0.000001 + 0.5 * offset;
offset = topCircle.Radius / 20;
//System.Console.WriteLine("{0}", offset);
cur_dire = cur_dire_new;
cur_p = cur_p_new + offset * cur_dire;
CircleLists.Add(new MyCircle(cur_p, r, cur_dire));
// Get Data for Fit
double weight = Math.Abs(cur_dire_new.Dot(cur_dire));
GeneratedCenters.Add(cur_p_new);
weights.Add(weight);
radius.Add(r);
dires.Add(cur_dire);
}
// Step1: Get IntersectionPoitn
RayTracein3DPlane(boundary3, cur_p, cur_dire, sectionPlane.Normal(), out norInsec, out notNorInsec);
// Step2 : Get Two Local Tangential
Insection1 = boundary3[norInsec];
Insection2 = boundary3[notNorInsec];
tangential1 = GetLocalTangential(norInsec, boundary3, cur_dire);
tangential2 = GetLocalTangential(notNorInsec, boundary3, cur_dire);
// Visualization
setdirecLine = new Line3(cur_p, cur_dire);
setLine1 = new Line3(Insection1, tangential1);
setLine2 = new Line3(Insection2, tangential2);
// Step3 : Get New Cur Direction and Cur Point
cur_dire_new = (tangential1 + tangential2) / 2;
RayTracein3DPlane(boundary3, cur_p, cur_dire_new, sectionPlane.Normal(), out norInsec, out notNorInsec);
cur_p_new = (boundary3[norInsec] + boundary3[notNorInsec]) / 2;
r = 0.5 * MyVector3.Distance(boundary3[norInsec], boundary3[notNorInsec]);
iter++;
this.view.Refresh();
}
// Fit centers and radius;
GeneratedCenters = FittingCentersCurve(GeneratedCenters, weights);
int inter = 1;
while (inter-- > 0)
{
radius = FittRadius(radius);
}
// ReBuild Object
CircleLists.Clear();
CircleLists.Add(topCircle); // Fix first circle
for (int i = 0; i < GeneratedCenters.Count; i++)
{
CircleLists.Add(new MyCircle(GeneratedCenters[i], radius[i], dires[i]));
}
CurveCyliner = new SweepMesh(CircleLists);
}
/// <summary>
/// Check the intersection between an <see cref="MyOrientedBoundingBox"/> and <see cref="MyBoundingBox"/>
/// </summary>
/// <param name="box">The BoundingBox to test.</param>
/// <returns>The type of containment the two objects have.</returns>
/// <remarks>
/// For accuracy, The transformation matrix for the <see cref="MyOrientedBoundingBox"/> must not have any scaling applied to it.
/// Anyway, scaling using Scale method will keep this method accurate.
/// </remarks>
public MyContainmentType Contains(ref MyBoundingBox box)
{
var cornersCheck = Contains(box.GetCorners());
if (cornersCheck != MyContainmentType.Disjoint)
return cornersCheck;
var boxCenter = box.Minimum + (box.Maximum - box.Minimum) / 2f;
var boxExtents = box.Maximum - boxCenter;
var SizeA = Extents;
var SizeB = boxExtents;
var RotA = GetRows(ref Transformation);
float ExtentA, ExtentB, Separation;
int i, k;
MyMatrix R; // Rotation from B to A
MyMatrix.Invert(ref Transformation, out R);
var AR = new MyMatrix(); // absolute values of R matrix, to use with box extents
for (i = 0; i < 3; i++)
for (k = 0; k < 3; k++)
{
AR[i, k] = Math.Abs(R[i, k]);
}
// Vector separating the centers of Box B and of Box A
var vSepWS = boxCenter - Center;
// Rotated into Box A's coordinates
var vSepA = new MyVector3(MyVector3.Dot(vSepWS, RotA[0]), MyVector3.Dot(vSepWS, RotA[1]), MyVector3.Dot(vSepWS, RotA[2]));
// Test if any of A's basis vectors separate the box
for (i = 0; i < 3; i++)
{
ExtentA = SizeA[i];
ExtentB = MyVector3.Dot(SizeB, new MyVector3(AR[i, 0], AR[i, 1], AR[i, 2]));
Separation = Math.Abs(vSepA[i]);
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// Test if any of B's basis vectors separate the box
for (k = 0; k < 3; k++)
{
ExtentA = MyVector3.Dot(SizeA, new MyVector3(AR[0, k], AR[1, k], AR[2, k]));
ExtentB = SizeB[k];
Separation = Math.Abs(MyVector3.Dot(vSepA, new MyVector3(R[0, k], R[1, k], R[2, k])));
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// Now test Cross Products of each basis vector combination ( A[i], B[k] )
for (i = 0; i < 3; i++)
for (k = 0; k < 3; k++)
{
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
int k1 = (k + 1) % 3, k2 = (k + 2) % 3;
ExtentA = SizeA[i1] * AR[i2, k] + SizeA[i2] * AR[i1, k];
ExtentB = SizeB[k1] * AR[i, k2] + SizeB[k2] * AR[i, k1];
Separation = Math.Abs(vSepA[i2] * R[i1, k] - vSepA[i1] * R[i2, k]);
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// No separating axis found, the boxes overlap
return MyContainmentType.Intersects;
}
/// <summary>
/// Initializes a new instance of the <see cref="T:SharpDX.Plane" /> class.
/// </summary>
/// <param name="point">Any point that lies along the plane.</param>
/// <param name="normal">The normal vector to the plane.</param>
public MyPlane(MyVector3 point, MyVector3 normal)
{
this.Normal = normal;
this.D = -MyVector3.Dot(normal, point);
}
void Update()
{
if (target.position.y > 1.25f)
{
for (int i = 0; i < joints.Length; i++)
{
jointsPositions[i] = new MyVector3(joints[i].position.x, joints[i].position.y, joints[i].position.z);
}
targetPosition = new MyVector3(target.position.x, target.position.y, target.position.z);
// Copy the joints positions to work with
// and calculate all the distances
for (int i = 0; i < copy.Length; i++)
{
copy[i] = jointsPositions[i];
if (i < distances.Length)
{
distances[i] = (jointsPositions[i + 1] - jointsPositions[i]).magnitude();
}
}
done = (targetPosition - jointsPositions[jointsPositions.Length - 1]).magnitude() < threshold_distance;
if (!done)
{
float targetRootDist = MyVector3.Distance(copy[0], targetPosition);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float dist = (targetPosition - copy[i]).magnitude();
float lam = distances[i] / dist;
copy[i + 1] = (1 - lam) * copy[i] + lam * targetPosition;
}
}
else
{
// The target is reachable
iter = 0;
while (!done /*|| iter < maxIter*/)
{
// STAGE 1: FORWARD REACHING
copy[copy.Length - 1] = targetPosition;
for (int i = copy.Length - 1; i > 0; i--)
{
MyVector3 temp = (copy[i - 1] - copy[i]).normalized();
temp = temp * distances[i - 1];
copy[i - 1] = temp + copy[i];
}
// STAGE 2: BACKWARD REACHING
copy[0] = jointsPositions[0];
for (int i = 0; i < copy.Length - 2; i++)
{
MyVector3 temp = (copy[i + 1] - copy[i]).normalized();
temp = temp * distances[i];
copy[i + 1] = temp + copy[i];
}
done = (targetPosition - copy[copy.Length - 1]).magnitude() < threshold_distance;
iter++;
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
MyVector3 a = jointsPositions[i + 1] - jointsPositions[i];
MyVector3 b = copy[i + 1] - copy[i];
MyVector3 axis = MyVector3.Cross(a, b).normalized();
float cosa = MyVector3.Dot(a, b) / (a.magnitude() * b.magnitude());
float sina = MyVector3.Cross(a.normalized(), b.normalized()).magnitude();
float angle = Mathf.Atan2(sina, cosa);
MyQuaternion q = new MyQuaternion(Mathf.Cos(angle / 2), axis.x * Mathf.Sin(angle / 2), axis.y * Mathf.Sin(angle / 2), axis.z * Mathf.Sin(angle / 2));
MyQuaternion actualRot = new MyQuaternion(joints[i].rotation.w, joints[i].rotation.x, joints[i].rotation.y, joints[i].rotation.z);
MyQuaternion newRot = MyQuaternion.multiply(q, actualRot);
jointsPositions[i] = copy[i];
joints[i].position = new Vector3(jointsPositions[i].x, jointsPositions[i].y, jointsPositions[i].z);
joints[i].rotation = new Quaternion(newRot.x, newRot.y, newRot.z, newRot.w);
for (int j = i; j < joints.Length; j++) //Actualizar posiciones de los hijos despues de rotar
{
jointsPositions[j] = new MyVector3(joints[j].position.x, joints[j].position.y, joints[j].position.z);
}
}
}
}
}
/// <summary>
/// Get the width of the frustum at specified depth.
/// </summary>
/// <param name="depth">the depth at which to calculate frustum width.</param>
/// <returns>With of the frustum at the specified depth</returns>
public float GetWidthAtDepth(float depth)
{
float hAngle = (float)((Math.PI / 2.0 - Math.Acos(MyVector3.Dot(pNear.Normal, pLeft.Normal))));
return((float)(Math.Tan(hAngle) * depth * 2));
}
/// <summary>
/// Get the height of the frustum at specified depth.
/// </summary>
/// <param name="depth">the depth at which to calculate frustum height.</param>
/// <returns>Height of the frustum at the specified depth</returns>
public float GetHeightAtDepth(float depth)
{
float vAngle = (float)((Math.PI / 2.0 - Math.Acos(MyVector3.Dot(pNear.Normal, pTop.Normal))));
return((float)(Math.Tan(vAngle) * depth * 2));
}
/// <summary>
/// Check the intersection between two <see cref="MyOrientedBoundingBox"/>
/// </summary>
/// <param name="obb">The OrientedBoundingBoxs to test.</param>
/// <returns>The type of containment the two objects have.</returns>
/// <remarks>
/// For accuracy, The transformation matrix for both <see cref="MyOrientedBoundingBox"/> must not have any scaling applied to it.
/// Anyway, scaling using Scale method will keep this method accurate.
/// </remarks>
public MyContainmentType Contains(ref MyOrientedBoundingBox obb)
{
var cornersCheck = Contains(obb.GetCorners());
if (cornersCheck != MyContainmentType.Disjoint)
return cornersCheck;
//http://www.3dkingdoms.com/weekly/bbox.cpp
var SizeA = Extents;
var SizeB = obb.Extents;
var RotA = GetRows(ref Transformation);
var RotB = GetRows(ref obb.Transformation);
var R = new MyMatrix(); // Rotation from B to A
var AR = new MyMatrix(); // absolute values of R matrix, to use with box extents
float ExtentA, ExtentB, Separation;
int i, k;
// Calculate B to A rotation matrix
for (i = 0; i < 3; i++)
for (k = 0; k < 3; k++)
{
R[i, k] = MyVector3.Dot(RotA[i], RotB[k]);
AR[i, k] = Math.Abs(R[i, k]);
}
// Vector separating the centers of Box B and of Box A
var vSepWS = obb.Center - Center;
// Rotated into Box A's coordinates
var vSepA = new MyVector3(MyVector3.Dot(vSepWS, RotA[0]), MyVector3.Dot(vSepWS, RotA[1]), MyVector3.Dot(vSepWS, RotA[2]));
// Test if any of A's basis vectors separate the box
for (i = 0; i < 3; i++)
{
ExtentA = SizeA[i];
ExtentB = MyVector3.Dot(SizeB, new MyVector3(AR[i, 0], AR[i, 1], AR[i, 2]));
Separation = Math.Abs(vSepA[i]);
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// Test if any of B's basis vectors separate the box
for (k = 0; k < 3; k++)
{
ExtentA = MyVector3.Dot(SizeA, new MyVector3(AR[0, k], AR[1, k], AR[2, k]));
ExtentB = SizeB[k];
Separation = Math.Abs(MyVector3.Dot(vSepA, new MyVector3(R[0, k], R[1, k], R[2, k])));
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// Now test Cross Products of each basis vector combination ( A[i], B[k] )
for (i = 0; i < 3; i++)
for (k = 0; k < 3; k++)
{
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
int k1 = (k + 1) % 3, k2 = (k + 2) % 3;
ExtentA = SizeA[i1] * AR[i2, k] + SizeA[i2] * AR[i1, k];
ExtentB = SizeB[k1] * AR[i, k2] + SizeB[k2] * AR[i, k1];
Separation = Math.Abs(vSepA[i2] * R[i1, k] - vSepA[i1] * R[i2, k]);
if (Separation > ExtentA + ExtentB)
return MyContainmentType.Disjoint;
}
// No separating axis found, the boxes overlap
return MyContainmentType.Intersects;
}
void Update()
{
// Copy the joints positions to work with
//TODO
copy[0] = new MyVector3(joints[0].position);
//copy[0] = joints[0].position;
for (int i = 0; i < joints.Length - 1; i++)
{
copy[i + 1] = new MyVector3(joints[i + 1].position);
distances[i] = (copy[i + 1] - copy[i]).Module();
//copy[i + 1] = joints[i + 1].position;
//distances[i] = (copy[i + 1] - copy[i]).magnitude;
}
// CALCULATE ALSO THE DISTANCE BETWEEN JOINTS
//done = TODO
done = (copy[copy.Length - 1] - new MyVector3(target.position)).Module() < tresholdCondition;
//done = (copy[copy.Length - 1] - target.position).magnitude < tresholdCondition;
if (!done)
{
float targetRootDist = (copy[0] - new MyVector3(target.position)).Module();
//float targetRootDist = Vector3.Distance(copy[0], target.position);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVector3(target.position) - copy[i]).Module();
//float r = (target.position - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * new MyVector3(target.position));
//copy[i + 1] = (1 - lambda) * copy[i] + (lambda * target.position);
}
}
else
{
MyVector3 b = copy[0];
//Vector3 b = copy[0];
// The target is reachable
//while (TODO)
float difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();
//float difference = (copy[copy.Length - 1] - target.position).magnitude;
while (difference > tresholdCondition) // treshold = tolerance
{
// numIterations++;
// STAGE 1: FORWARD REACHING
//TODO
copy[copy.Length - 1] = new MyVector3(target.position);
//copy[copy.Length - 1] = target.position;
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).Module();
//float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = (1 - lambda) * copy[i + 1] + lambda * copy[i];
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).Module();
//float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + lambda * copy[i + 1];
}
difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();
//difference = (copy[copy.Length - 1] - target.position).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i <= joints.Length - 2; i++)
{
// float originalAngle = joints[i].rotation.w;
MyQuat parentRotation = new MyQuat(joints[i + 1].rotation);
MyQuat childRotation = new MyQuat(joints[i].rotation);
//TODO
// Rotation
MyVector3 vectorA = new MyVector3(joints[i + 1].position) - new MyVector3(joints[i].position);
MyVector3 vectorB = copy[i + 1] - copy[i];
//Vector3 vectorA = joints[i + 1].position - joints[i].position;
//Vector3 vectorB = copy[i + 1] - copy[i];
// float angle = Mathf.Acos(Vector3.Dot(vectorA.normalized, vectorB.normalized)) * Mathf.Rad2Deg;
float cosA = (MyVector3.Dot(vectorA.Normalize(), vectorB.Normalize()));
float sinA = MyVector3.Cross(vectorA.Normalize(), vectorB.Normalize()).Module();
//float cosA = (Vector3.Dot(vectorA.normalized, vectorB.normalized));
//float sinA = Vector3.Cross(vectorA.normalized, vectorB.normalized).magnitude;
// Atan = Cos | Atan2 = denominador y...
float angle = Mathf.Atan2(sinA, cosA) * Mathf.Rad2Deg;
MyVector3 axis = MyVector3.Cross(vectorA, vectorB).Normalize();
//Vector3 axis = Vector3.Cross(vectorA, vectorB).normalized;
// joints[i].rotation = Quaternion.AngleAxis(angle, axis) * joints[i].rotation;
joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();
//joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();
childRotation = new MyQuat(joints[i].rotation);
float angleTest = MyQuat.Angle(parentRotation, childRotation);
if (Mathf.Abs(angleTest) > maxAngleRotation)
{
joints[i + 1].rotation = joints[i].rotation;
}
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
//joints[i + 1].position = copy[i + 1];
}
}
}
// Running the solver - all the joints are iterated through once every frame
void Update()
{
// if the target hasn't been reached
if (!done)
{
// if the Max number of tries hasn't been reached
if (tries <= Mtries)
{
// starting from the second last joint (the last being the end effector)
// going back up to the root
for (int i = joints.Length - 2; i >= 0; i--)
{
// The vector from the ith joint to the end effector
MyVector3 r1 = new MyVector3(joints[joints.Length - 1].position - joints[i].position);
// The vector from the ith joint to the target
MyVector3 r2 = tpos - new MyVector3(joints[i].position);
// to avoid dividing by tiny numbers
if (r1.magnitude * r2.magnitude <= 0.001f)
{
cos[i] = 1.0f;
sin[i] = 0.0f;
}
else
{
// find the components using dot and cross product
cos[i] = MyVector3.Dot(r1, r2) / (r1.magnitude * r2.magnitude);
sin[i] = MyVector3.Cross(r1, r2).magnitude / (r1.magnitude * r2.magnitude);
}
// The axis of rotation
MyVector3 axis = MyVector3.Cross(r1, r2);
// find the angle between r1 and r2 (and clamp values if needed avoid errors)
theta[i] = Mathf.Acos(cos[i]);
//Optional. correct angles if needed, depending on angles invert angle if sin component is negative
if (sin[i] < 0)
{
theta[i] *= -1;
}
// obtain an angle value between -pi and pi, and then convert to degrees
theta[i] *= Mathf.Rad2Deg;
// rotate the ith joint along the axis by theta degrees in the world space.
MyQuaternion quat = MyQuaternion.AngleAxis(theta[i], ref axis);
Quaternion fQuat = new Quaternion(quat.x, quat.y, quat.z, quat.w);
joints[i].rotation = fQuat * joints[i].rotation;
}
// increment tries
tries++;
}
}
// find the difference in the positions of the end effector and the target
float dif = (tpos - new MyVector3(joints[joints.Length - 1].position)).magnitude;
// if target is within reach (within epsilon) then the process is done
if (dif < epsilon)
{
done = true;
}
// if it isn't, then the process should be repeated
else
{
done = false;
}
// the target has moved, reset tries to 0 and change tpos
if (new MyVector3(targ.position) != tpos)
{
tries = 0;
tpos = new MyVector3(targ.position);
}
}
