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