public static Vector3 operator *(Quaternion xform, Vector3 vec)
{
/* From nVidia SDK */
Vector3 uv, uuv;
Vector3 qvec = new Vector3(xform.X, xform.Y, xform.Z);
uv = qvec.Cross(vec);
uuv = qvec.Cross(uv);
uv *= (2.0f * xform.W);
uuv *= 2.0f;
return vec + uv + uuv;
}
public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
ref Vector3 cameraUpVector, Vector3 cameraForwardVector, out Matrix4 result)
{
Vector3 look = cameraPosition - objectPosition;
look = look.Normalize();
Vector3 right = cameraUpVector.Cross(look).Normalize();
Vector3 up = look.Cross(right).Normalize();
//Matrix4 mat = Matrix4.LookAt(cameraPosition, cameraForwardVector, cameraUpVector);
//Vector3 right = new Vector3(mat.M11, mat.M21, mat.M31);
//right = right.Normalize();
//Vector3 up = new Vector3(mat.M12, mat.M22, mat.M32);
//up = up.Normalize();
result.M11 = right.X;
result.M12 = right.Y;
result.M13 = right.Z;
result.M14 = 0;
result.M21 = up.X;
result.M22 = up.Y;
result.M23 = up.Z;
result.M24 = 0;
result.M31 = look.X;
result.M32 = look.Y;
result.M33 = look.Z;
result.M34 = 0;
result.M41 = objectPosition.X;
result.M42 = objectPosition.Y;
result.M43 = objectPosition.Z;
result.M44 = 1;
}
public static void Test_Vector3_Generic_Cross_Product_Against_Unity3D(float a, float b, float c, float d, float e, float f)
{
//arrange
Vector3<float> genericVec3One = new Vector3<float>(a, b, c);
Vector3<float> genericVec3Two = new Vector3<float>(d, e, f);
UnityEngine.Vector3 unityVec3One = new UnityEngine.Vector3(a, b, c);
UnityEngine.Vector3 unityVec3Two = new UnityEngine.Vector3(d, e, f);
//act
Vector3<float> genericVec3Cross = genericVec3One.Cross(genericVec3Two);
UnityEngine.Vector3 unityVec3Cross = UnityEngine.Vector3.Cross(unityVec3One, unityVec3Two);
Vector3<float> manualCross = new Vector3<float>(genericVec3One.y * genericVec3Two.z - genericVec3One.z * genericVec3Two.y,
genericVec3One.z * genericVec3Two.x - genericVec3One.x * genericVec3Two.z, genericVec3One.x * genericVec3Two.y - genericVec3One.y * genericVec3Two.x);
double termOne = Operator<double>.Multiply((double)genericVec3One.y, (double)genericVec3Two.z);
double termTwo = Operator<double>.Multiply((double)genericVec3One.z, (double)genericVec3Two.y);
double termThree = Operator<double>.Multiply((double)genericVec3One.z, (double)genericVec3Two.x);
double termFour = Operator<double>.Multiply((double)genericVec3One.x, (double)genericVec3Two.z);
double termFive = Operator<double>.Multiply((double)genericVec3One.x, (double)genericVec3Two.y);
double termSix = Operator<double>.Multiply((double)genericVec3One.y, (double)genericVec3Two.x);
double newX = Operator<double>.Subtract((double)termOne, (double)termTwo);
double newY = Operator<double>.Subtract((double)termThree, (double)termFour);
double newZ = Operator<double>.Subtract((double)termFive, (double)termSix);
Vector3<float> operatorGenericCross = new Vector3<float>((float)newX, (float)newY, (float)newZ);
//assert
for (int i = 0; i < 3; i++) //we have to accept a higher delta of error since cross product has so much float math.
{
Assert.AreEqual(operatorGenericCross[i], manualCross[i]);
Assert.AreEqual(manualCross[i], unityVec3Cross[i]);
Assert.AreEqual(unityVec3Cross[i], genericVec3Cross[i], UnityEngine.Vector3.kEpsilon * 5, "Index: {0} failed. Failed to compute cross product.", i);
}
}
public bool RayIntersect(Vector3 ray, Vector3 rayStart)
{
if (Math.Abs(Normal.Normal.Dot(ray)) < 0.02) return false;
if (-Normal.Distance(rayStart) <= 0) return false;
var T = rayStart - Vertices[0];
var edge = Vertices[1] - Vertices[0];
for (var i = 2; i < Vertices.Count; i++)
{
var prev = edge;
edge = Vertices[i] - Vertices[0];
var P = ray.Cross(edge);
var Q = T.Cross(prev);
var t = P.Dot(prev);
var u = P.Dot(T) / t;
var v = Q.Dot(ray) / t;
t = 1.0f - u - v;
if (u.IsBetween(0.0f, 1.0f) && v.IsBetween(0.0f, 1.0f) && t.IsBetween(0.0f, 1.0f))
return true;
}
return false;
}
public void Rotate( ref Vector3 _Axis, double _Angle, out Vector3 _Out )
{
double cos_ang = Math.Cos( _Angle );
double sin_ang = Math.Sin( _Angle );
_Out.x = x * cos_ang;
_Out.y = y * cos_ang;
_Out.z = z * cos_ang;
double temp = Dot( ref _Axis );
temp *= 1.0-cos_ang;
_Out.x += _Axis.x * temp;
_Out.y += _Axis.y * temp;
_Out.z += _Axis.z * temp;
_Axis.Cross( ref this, out TempCross );
_Out.x += TempCross.x * sin_ang;
_Out.y += TempCross.y * sin_ang;
_Out.z += TempCross.z * sin_ang;
}
public bool RayIntersect(Vector3 rayDir, Vector3 dot, ref float lambda)
{
var u = Plane.Normal.Dot(rayDir);
if(Math.Abs(u) < 0.001)
{
return false; // plane is parallel to the ray - no intersection
}
lambda = -Plane.Distance(dot) / u;
var vp = 0; // current polygon index
var T = dot - Vertices[0].Position;
var E2 = Vertices[1].Position - Vertices[0].Position;
for (var i = 0; i < Vertices.Count - 2; i++, vp++)
{
var E1 = E2; // PREV
E2 = Vertices[vp + 2].Position - Vertices[0].Position; // NEXT
var P = rayDir.Cross(E2);
var Q = T.Cross(E1);
var tt = P.Dot(E1);
u = P.Dot(T);
u /= tt;
var v = Q.Dot(rayDir);
v /= tt;
tt = 1.0f - u - v;
if(u.IsBetween(0, 1) && v.IsBetween(0, 1) && tt.IsBetween(0, 1))
{
return true;
}
}
return false;
}
public void SetCameraDef(Vector3 p, Vector3 l, Vector3 u)
{
if (pl[0] == null) { ConstructPlanes(); }
Vector3 nc, fc, X, Y, Z;
// compute the Z axis of camera
// this axis points in the opposite direction from
// the looking direction
Z = p - l;
Z.Normalize();
// X axis of camera with given "up" vector and Z axis
//X = u * Z;
X = u.Cross(Z);
X.Normalize();
// the real "up" vector is the cross product of Z and X
//Y = Z * X;
Y = Z.Cross(X);
// compute the centers of the near and far planes
nc = p - Z * NearD;
fc = p - Z * FarD;
// compute the 4 corners of the frustum on the near plane
ntl = nc + Y * nh - X * nw;
ntr = nc + Y * nh + X * nw;
nbl = nc - Y * nh - X * nw;
nbr = nc - Y * nh + X * nw;
// compute the 4 corners of the frustum on the far plane
ftl = fc + Y * fh - X * fw;
ftr = fc + Y * fh + X * fw;
fbl = fc - Y * fh - X * fw;
fbr = fc - Y * fh + X * fw;
// compute the six planes
// the function set3Points assumes that the points
// are given in counter clockwise order
pl[(int)FrustumPlanes.TOP].Set3Points(ntr, ntl, ftl);
pl[(int)FrustumPlanes.BOTTOM].Set3Points(nbl, nbr, fbr);
pl[(int)FrustumPlanes.LEFT].Set3Points(ntl, nbl, fbl);
pl[(int)FrustumPlanes.RIGHT].Set3Points(nbr, ntr, fbr);
pl[(int)FrustumPlanes.NEARP].Set3Points(ntl, ntr, nbr);
pl[(int)FrustumPlanes.FARP].Set3Points(ftr, ftl, fbl);
//Now let's set what is needed for the frustum sphere
Center = p + (-Z * (((FarD - NearD) * 0.5f) + NearD));
//And some more stuff for a frustum cone
ViewAxis = -Z;
ViewPosition = p;
}
public override void Move()
{
if (m_gridFinder != null)
{
//m_logger.debugLog("updating grid finder", "Move()");
m_gridFinder.Update();
// if grid finder picks a new entity, have to set OrbitEntity to null first or altitude will be incorrect
if (m_gridFinder.Grid == null || OrbitEntity != m_gridFinder.Grid)
{
m_logger.debugLog("no grid found");
OrbitEntity = null;
m_mover.StopMove();
return;
}
if (OrbitEntity == null)
{
m_logger.debugLog("found grid: " + m_gridFinder.Grid.Entity.DisplayName);
OrbitEntity = m_gridFinder.Grid.Entity;
CalcFakeOrbitSpeedForce();
}
}
Vector3D targetCentre = OrbitEntity.GetCentre() + m_targetPositionOffset;
m_faceDirection = Vector3.Reject(targetCentre - m_navBlock.WorldPosition, m_orbitAxis);
float alt = m_faceDirection.Normalize();
Vector3 orbitDirection = m_faceDirection.Cross(m_orbitAxis);
Vector3D destination = targetCentre - m_faceDirection * Altitude;
float speed = alt > Altitude ?
Math.Max(1f, OrbitSpeed - alt + Altitude) :
OrbitSpeed;
m_mover.CalcMove(m_navBlock, destination, OrbitEntity.GetLinearVelocity() + orbitDirection * speed);
}
public void Assign(Vector3 v1, Vector3 v2, Vector3 pos)
{
Normal = v1.Cross(v2);
Normal = Normal.SafeNormalize();
Dot = Normal.Dot(pos);
}
public static Matrix3x3 GetRotationMatrix(Vector3 a, Vector3 b)
{
a = a.Normalize ();
b = b.Normalize ();
if (a != b &&
a.Length () > 0 &&
b.Length () > 0)
{
Vector3 c = a.Cross (b);
return new Matrix3x3 (
a.x, b.x, c.x,
a.y, b.y, c.y,
a.z, b.z, c.z);
}
return new Matrix3x3 ();
}
public void cross() {
var a = new Vector3(3, -3, 1);
var b = new Vector3(4, 9, 2);
Assert.Equal(
new Vector3(-15, -2, 39),
a.Cross(b)
);
Assert.Equal(
new Vector3(15, 2, -39),
b.Cross(a)
);
}
private static JacobianContact[] addFriction(
SimulationObject objectA,
SimulationObject objectB,
SimulationParameters simulationParameters,
int indexA,
int indexB,
Vector3 normal,
Vector3 relativeVelocity,
Vector3 ra,
Vector3 rb,
List<StartImpulseProperties> startImpulseProperties)
{
JacobianContact[] friction = new JacobianContact[2];
var tx = new Vector3 ();
var ty = new Vector3 ();
GeometryUtilities.ComputeBasis(
normal,
ref tx,
ref ty);
double constraintLimit = 0.0;
Vector3 tangentialVelocity = relativeVelocity -
(normal.Dot(relativeVelocity)) *
normal;
#region Get start friction direction
if (Vector3.Length (tangentialVelocity) >
simulationParameters.ShiftToStaticFrictionTolerance)
constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
else
constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
#endregion
#region Tangential Direction 1
var linearComponentA = tx;
var linearComponentB = -1.0 * linearComponentA;
var angularComponentA = ra.Cross (linearComponentA);
var angularComponentB = -1.0 * rb.Cross (linearComponentA);
friction [0] = JacobianCommon.GetDOF (
indexA,
indexB,
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
objectA,
objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null,
startImpulseProperties[1]);
#endregion
#region Tangential Direction 2
linearComponentA = ty;
linearComponentB = -1.0 * linearComponentA;
angularComponentA = ra.Cross (linearComponentA);
angularComponentB = -1.0 * rb.Cross (linearComponentA);
friction [1] = JacobianCommon.GetDOF (
indexA,
indexB,
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
objectA,
objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null,
startImpulseProperties[2]);
#endregion
return friction;
}