/// <summary> /// Builds a ball socket joint. /// </summary> /// <param name="connectionA">First connection in the pair.</param> /// <param name="connectionB">Second connection in the pair.</param> /// <param name="anchor">World space anchor location used to initialize the local anchors.</param> public IKBallSocketJoint(Bone connectionA, Bone connectionB, Vector3 anchor) : base(connectionA, connectionB) { Vector3 tmp; anchor.Sub(ref ConnectionA.Position, out tmp); SetOffsetA(ref tmp); anchor.Sub(ref ConnectionB.Position, out tmp); SetOffsetB(ref tmp); }
public static int Extended(int _a, int _n) { int q = 0; if (NOD(_a, _n) == 1) { Vector3 u = new Vector3(0, 1, _n); Vector3 v = new Vector3(1, 0, _a); Vector3 t = new Vector3(); while (u.z != 1) { q = u.z / v.z; t = new Vector3(Vector3.Sub(u, Vector3.Multiply(v, q))); u = new Vector3(v); v = new Vector3(t); } return(u.x < 0 ? u.x + _n : u.x); } else { throw new Exception("Условие НОД не выполнено"); } }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate.</param> /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param> public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor); Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp ); AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp ); HingeLimit = new RevoluteLimit(connectionA, connectionB); HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis); TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0); TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp ); HingeLimit.IsActive = false; HingeMotor.IsActive = false; TwistLimit.IsActive = false; TwistMotor.IsActive = false; //Ensure that the base and test direction is perpendicular to the free axis. Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way. connectionB.position.Sub( ref anchor, out baseAxis ); baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis); } } HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix); HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix); connectionB.position.Sub( ref anchor, out baseAxis ); baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis); } } HingeLimit.TestAxis = baseAxis; HingeMotor.TestAxis = baseAxis; Add(BallSocketJoint); Add(AngularJoint); Add(HingeLimit); Add(HingeMotor); Add(TwistLimit); Add(TwistMotor); }
public void CanSubstractTwoVectors() { Vector3 actual = Vector3.Sub(v1, v2); Vector3 expected = new Vector3(0.7d, 3.5d, -79.6d); Assert.AreEqual(expected.X, actual.X, 0.00001d); Assert.AreEqual(expected.Y, actual.Y, 0.00001d); Assert.AreEqual(expected.Z, actual.Z, 0.00001d); }
public void TestSub() { var a = new Vector3(1, 2, 3); var b = new Vector3(9, -4, -5); var c = Vector3.Zero; _ = a.Sub(b, ref c); Assert.AreEqual(new Vector3(-8, 6, 8), c); }
/// <summary> /// /// </summary> /// <param name="clientSize"></param> /// <param name="here"></param> public override void MouseMove(Size clientSize, Point here) { // Normalize mouse position mouse.X = (here.X / (float)clientSize.Width) * 2 - 1; mouse.Y = -(here.Y / (float)clientSize.Height) * 2 + 1; var vector = new Vector3(mouse.X, mouse.Y, 0.5f).Unproject(camera); var raycaster = new Raycaster(camera.Position, vector.Sub(camera.Position).Normalize()); /* * if (null != SELECTED) * { * var intersects2 = raycaster.IntersectObject(plane); * SELECTED.Position.Copy(intersects2[0].Point.Sub(offset)); * * return; * } */ var intersects = raycaster.IntersectObjects(object3Ds); if (intersects.Count > 0) { if (INTERSECTED != intersects[0].Object3D) { if (null != INTERSECTED) { ((MeshLambertMaterial)INTERSECTED.Material).Color = currentHex; } INTERSECTED = intersects[0].Object3D; currentHex = ((MeshLambertMaterial)INTERSECTED.Material).Color; plane.Position.Copy(INTERSECTED.Position); plane.LookAt(camera.Position); } // container.style.cursor = 'pointer'; } else { if (INTERSECTED != null) { ((MeshLambertMaterial)INTERSECTED.Material).Color = currentHex; } INTERSECTED = null; // container.style.cursor = 'auto'; } }
public Camera(Vector3 pos, Vector3 lookAt, Vector2 frustrumSize) { this.pos = pos; var dir = lookAt.Sub(ref pos); frustrumFrontPlane.topLeft.x = -frustrumSize.x / 2; frustrumFrontPlane.topLeft.y = frustrumSize.y / 2; frustrumFrontPlane.bottomRight.x = frustrumSize.x / 2; frustrumFrontPlane.bottomRight.y = -frustrumSize.y / 2; this.frustrumFrontPlane = new Plane3( topLeft: new Vector3(-frustrumSize.x / 2, frustrumSize.y / 2, 0), topRight: new Vector3(frustrumSize.x / 2, frustrumSize.y / 2, 0), bottomLeft: new Vector3(-frustrumSize.x / 2, -frustrumSize.y / 2, 0), bottomRight: new Vector3(frustrumSize.x / 2, -frustrumSize.y / 2, 0)); }
/// <summary> /// /// </summary> public override void Render() { theta += 0.1f; camera.Position.X = radius * (float)Math.Sin(Mat.DegToRad(theta)); camera.Position.Y = radius * (float)Math.Sin(Mat.DegToRad(theta)); camera.Position.Z = radius * (float)Math.Cos(Mat.DegToRad(theta)); camera.LookAt(scene.Position); // find intersections var vector = new Vector3(mouse.X, mouse.Y, 1).Unproject(camera); raycaster = new Raycaster(camera.Position, vector.Sub(camera.Position).Normalize()); var intersects = raycaster.IntersectObjects(parentTransform.Children, true); if (intersects.Count > 0) { if (currentIntersected != null) { ((LineBasicMaterial)currentIntersected.Material).Linewidth = 1; } currentIntersected = intersects[0].Object3D; ((LineBasicMaterial)currentIntersected.Material).Linewidth = 5; sphereInter.Visible = true; sphereInter.Position.Copy(intersects[0].Point); } else { if (currentIntersected != null) { ((LineBasicMaterial)currentIntersected.Material).Linewidth = 1; } currentIntersected = null; sphereInter.Visible = false; } renderer.Render(scene, camera); }
/// <summary> /// /// </summary> public override void Render() { theta += 0.1f; camera.Position.X = radius * (float)System.Math.Sin(Mat.DegToRad(theta)); camera.Position.Y = radius * (float)System.Math.Sin(Mat.DegToRad(theta)); camera.Position.Z = radius * (float)System.Math.Cos(Mat.DegToRad(theta)); camera.LookAt(scene.Position); // find intersections var vector = new Vector3(mouse.X, mouse.Y, 1).Unproject(camera); raycaster = new Raycaster(camera.Position, vector.Sub(camera.Position).Normalize()); var intersects = raycaster.IntersectObjects(scene.Children); if (intersects.Count > 0) { if (INTERSECTED != intersects[0].Object3D) { if (INTERSECTED != null) { ((MeshLambertMaterial)INTERSECTED.Material).Emissive = currentHex; } INTERSECTED = intersects[0].Object3D; currentHex = ((MeshLambertMaterial)INTERSECTED.Material).Emissive; ((MeshLambertMaterial)INTERSECTED.Material).Emissive = Color.Red; } } else { if (INTERSECTED != null) { ((MeshLambertMaterial)INTERSECTED.Material).Emissive = currentHex; } INTERSECTED = null; } renderer.Render(scene, camera); }
/// <summary> /// /// </summary> /// <param name="clientSize"></param> /// <param name="here"></param> public override void MouseDown(Size clientSize, Point here) { var vector = new Vector3(mouse.X, mouse.Y, 0.5f).Unproject(camera); var raycaster = new Raycaster(camera.Position, vector.Sub(camera.Position).Normalize()); var intersects = raycaster.IntersectObjects(object3Ds); if (intersects.Count > 0) { //controls.enabled = false; SELECTED = intersects[0].Object3D; // take the closest to camera var intersects2 = raycaster.IntersectObject(plane); offset.Copy(intersects2[0].Point).Sub(plane.Position); //container.style.cursor = 'move'; } }
public static double getAngle(Vector3 j0, Vector3 j1, Vector3 j2) { Vector3 v1; //declaration of the two vectors used Vector3 v2; v1 = j0.Sub(j1); v2 = j2.Sub(j1); v1.Normalize(); v2.Normalize(); double dotProduct = Vector3.Dot(v1, v2); //get dot product Vector3 crossProduct; //get cross product vector and length of cross product crossProduct = Vector3.Cross(v1, v2); double crossProductLength = crossProduct.Length(); double radianAngle = Math.Atan2(crossProductLength, dotProduct); //get radian angle between vectors double degreeAngle = radianAngle * (180 / Math.PI); //get angle in degrees double threedecimal = Math.Round(degreeAngle, 3); //get degree angle in 3 decimal point accuracy return threedecimal; }
/// <summary> Subtracts (a, a, a) from this vector's components. </summary> static public Vector3 Sub(this Vector3 t, float a) => t.Sub(a, a, a);
/// <summary> /// Gets or sets the offset in world space from the center of mass of connection B to the anchor point. /// </summary> public void SetAnchorB(ref Vector3 value) { Vector3 tmp; value.Sub(ref ConnectionB.Position, out tmp); Quaternion.Transform(ref tmp, ref ConnectionB.ConjOrientation, out LocalAnchorB); }
/// <summary> /// Sets the world space location of the line anchor attached to connection A. /// </summary> public void SetLineAnchor(ref Vector3 value) { Vector3 tmp; value.Sub(ref ConnectionA.Position, out tmp); Quaternion.Transform(ref tmp, ref ConnectionA.ConjOrientation, out LocalLineAnchor); }
/// <summary> Subtracts (a.x, a.y, a.z) from this vector's components. </summary> static public Vector3 Sub(this Vector3 t, Vector3 a) => t.Sub(a.x, a.y, a.z);
private bool HitTest(Vector3 origin, Vector3 direction, float maxDistance, float stepSize, out Vector3 hitPos, out float iterations) { Vector3 step = direction.Mul(stepSize); hitPos = origin.Add(step); iterations = 0; while (hitPos.Sub(ref origin).Len() < maxDistance) { iterations = fractal.HitTest(hitPos); if (iterations >= threshold) { return true; } hitPos.AddAssign(ref step); } return false; }
public void MakePerspectiveProjection(Vector3 normal, Vector3 point, Vector3 eye) { // +- -+ // M = | Dot(N,E-P)*I - E*N^T -(Dot(N,E-P)*I - E*N^T)*E | // | -N^t Dot(N,E) | // +- -+ // // where E is the eye point, P is a point on the plane, and N is a // unit-length plane normal. double emp = normal.Dot(eye.Sub(point)); entry[ 0] = emp - eye.tuple[0] * normal.tuple[0]; entry[ 1] = -eye.tuple[0] * normal.tuple[1]; entry[ 2] = -eye.tuple[0] * normal.tuple[2]; entry[ 3] = -(entry[0] * eye.tuple[0] + entry[1] * eye.tuple[1] + entry[2] * eye.tuple[2]); entry[ 4] = -eye.tuple[1] * normal.tuple[0]; entry[ 5] = emp - eye.tuple[1] * normal.tuple[1]; entry[ 6] = -eye.tuple[1] * normal.tuple[2]; entry[ 7] = -(entry[4] * eye.tuple[0] + entry[5] * eye.tuple[1] + entry[6] * eye.tuple[2]); entry[ 8] = -eye.tuple[2] * normal.tuple[0]; entry[ 9] = -eye.tuple[2] * normal.tuple[1]; entry[10] = emp - eye.tuple[2] * normal.tuple[2]; entry[11] = -(entry[8] * eye.tuple[0] + entry[9] * eye.tuple[1] + entry[10] * eye.tuple[2]); entry[12] = -normal.tuple[0]; entry[13] = -normal.tuple[1]; entry[14] = -normal.tuple[2]; entry[15] = normal.Dot(eye); }
public void OrthoNormalise(Vector3 u, Vector3 v, Vector3 w) { // If the input vectors are v0, v1, and v2, then the Gram-Schmidt // orthonormalization produces vectors u0, u1, and u2 as follows, // // u0 = v0/|v0| // u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0| // u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1| // // where |A| indicates length of vector A and A*B indicates dot // product of vectors A and B. // compute u0 u.Normalise(); // compute u1 double dot0 = u.Dot(v); v = v.Sub(u.Mult(dot0)); v.Normalise(); // compute u2 double dot1 = v.Dot(w); dot0 = u.Dot(w); w = w.Sub(u.Mult(dot0).Add(v.Mult(dot1))); w.Normalise(); }