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); } }