/// <summary>
/// Sets the shader uniforms that are necessary to project on screen the
/// TerrainQuad of the given TerrainNode. This method can set the uniforms
/// that are common to all the quads of the given terrain.
/// </summary>
public virtual void SetUniforms(TerrainNode node, Material mat)
{
if (mat == null || node == null)
{
return;
}
float d1 = node.SplitDist + 1.0f;
float d2 = 2.0f * node.SplitDist;
mat.SetVector(m_uniforms.blending, new Vector2(d1, d2 - d1));
m_localToCamera = node.View.WorldToCamera * node.LocalToWorld;
m_localToScreen = node.View.CameraToScreen * m_localToCamera;
Vector3d localCameraPos = node.LocalCameraPos;
Vector3d worldCamera = node.View.WorldCameraPos;
Matrix4x4d A = LocalToDeformedDifferential(localCameraPos);
Matrix4x4d B = DeformedToTangentFrame(worldCamera);
Matrix4x4d ltot = B * node.LocalToWorld * A;
m_localToTangent = new Matrix3x3d(ltot[0, 0], ltot[0, 1], ltot[0, 3],
ltot[1, 0], ltot[1, 1], ltot[1, 3],
ltot[3, 0], ltot[3, 1], ltot[3, 3]);
mat.SetMatrix(m_uniforms.localToScreen, MathConverter.ToMatrix4x4(m_localToScreen));
mat.SetMatrix(m_uniforms.localToWorld, MathConverter.ToMatrix4x4(node.LocalToWorld));
}
/// <summary>
/// Resuelve las ecuaciones:
/// u * uux + v * vvx + w * wwx = pox
/// u * uuy + v * vvy + w * wwy = poy
/// u * uuz + v * vvz + w * wwz = poz
/// Ayuda a determinar las coordenadas del punto <c>po</c> segun el plano formado por <c>uu</c> y <c>vv</c>.
/// </summary>
internal static void Resolve(Vector3d uu, Vector3d vv, Vector3d ww, Vector3d po,
out double u, out double v, out double w)
{
// Resuelve la ecuacion:
// u * uux + v * vvx + w * wwx = pox
// u * uuy + v * vvy + w * wwy = poy
// u * uuz + v * vvz + w * wwz = poz
//
// Se replantea como:
//
// / \ / \ / \
// | uux vvx wwx | | u | | pox |
// | uuy vvy wwy | * | v | = | poy |
// | uuz vvz wwz | | w | | poz |
// \ / \ / \ /
//
// Se resuelve como un cambio de coordenadas.
Matrix3x3d m = new Matrix3x3d(uu, vv, ww);
m.Inv();
Point3d ret = m * po;
u = ret.X;
v = ret.Y;
w = ret.Z;
}
/// <summary>
/// Sets the shader uniforms that are necessary to project on screen the
/// given TerrainQuad. This method can set the uniforms that are specific to
/// the given quad.
/// </summary>
public virtual void SetUniforms(TerrainNode node, TerrainQuad quad, MaterialPropertyBlock matPropertyBlock)
{
if (matPropertyBlock == null || node == null || quad == null)
{
return;
}
double ox = quad.Ox;
double oy = quad.Oy;
double l = quad.Length;
double distFactor = node.DistFactor;
int level = quad.Level;
matPropertyBlock.SetVector(m_uniforms.offset, new Vector4((float)ox, (float)oy, (float)l, (float)level));
Vector3d camera = node.LocalCameraPos;
matPropertyBlock.SetVector(m_uniforms.camera, new Vector4((float)((camera.x - ox) / l), (float)((camera.y - oy) / l),
(float)((camera.z - node.View.GroundHeight) / (l * distFactor)),
(float)camera.z));
Vector3d c = node.LocalCameraPos;
Matrix3x3d m = m_localToTangent * (new Matrix3x3d(l, 0.0, ox - c.x, 0.0, l, oy - c.y, 0.0, 0.0, 1.0));
matPropertyBlock.SetMatrix(m_uniforms.tileToTangent, MathConverter.ToMatrix4x4(m));
SetScreenUniforms(node, quad, matPropertyBlock);
}
public Deformation()
{
m_uniforms = new Uniforms();
m_localToCamera = new Matrix4x4d();
m_localToScreen = new Matrix4x4d();
m_localToTangent = new Matrix3x3d();
}
public Deformation()
{
m_uniforms = new Uniforms();
m_localToCamera = new Matrix4x4d();
m_localToScreen = new Matrix4x4d();
m_localToTangent = new Matrix3x3d();
}
/*
* Sets the shader uniforms that are necessary to project on screen the
* TerrainQuad of the given TerrainNode. This method can set the uniforms
* that are common to all the quads of the given terrain.
*/
public virtual void SetUniforms(TerrainNode node, Material mat)
{
if (mat == null || node == null)
{
return;
}
float d1 = node.GetSplitDist() + 1.0f;
float d2 = 2.0f * node.GetSplitDist();
mat.SetVector(m_uniforms.blending, new Vector2(d1, d2 - d1));
m_localToCamera = node.GetView().GetWorldToCamera() * node.GetLocalToWorld();
m_localToScreen = node.GetView().GetCameraToScreen() * m_localToCamera;
Vector3d2 localCameraPos = node.GetLocalCameraPos();
Vector3d2 worldCamera = node.GetView().GetWorldCameraPos();
Matrix4x4d A = LocalToDeformedDifferential(localCameraPos);
Matrix4x4d B = DeformedToTangentFrame(worldCamera);
Matrix4x4d ltot = B * node.GetLocalToWorld() * A;
m_localToTangent = new Matrix3x3d(ltot.m[0, 0], ltot.m[0, 1], ltot.m[0, 3],
ltot.m[1, 0], ltot.m[1, 1], ltot.m[1, 3],
ltot.m[3, 0], ltot.m[3, 1], ltot.m[3, 3]);
mat.SetMatrix(m_uniforms.localToScreen, m_localToScreen.ToMatrix4x4());
mat.SetMatrix(m_uniforms.localToWorld, node.GetLocalToWorld().ToMatrix4x4());
}
/*
* Sets the shader uniforms that are necessary to project on screen the
* given TerrainQuad. This method can set the uniforms that are specific to
* the given quad.
*/
public virtual void SetUniforms(TerrainNode node, TerrainQuad quad, MaterialPropertyBlock matPropertyBlock)
{
if (matPropertyBlock == null || node == null || quad == null)
{
return;
}
double ox = quad.GetOX();
double oy = quad.GetOY();
double l = quad.GetLength();
double distFactor = (double)node.GetDistFactor();
int level = quad.GetLevel();
matPropertyBlock.AddVector(m_uniforms.offset, new Vector4((float)ox, (float)oy, (float)l, (float)level));
Vector3d2 camera = node.GetLocalCameraPos();
matPropertyBlock.AddVector(m_uniforms.camera, new Vector4((float)((camera.x - ox) / l), (float)((camera.y - oy) / l),
(float)((camera.z - node.GetView().GetGroundHeight()) / (l * distFactor)),
(float)camera.z));
Vector3d2 c = node.GetLocalCameraPos();
Matrix3x3d m = m_localToTangent * (new Matrix3x3d(l, 0.0, ox - c.x, 0.0, l, oy - c.y, 0.0, 0.0, 1.0));
matPropertyBlock.AddMatrix(m_uniforms.tileToTangent, m.ToMatrix4x4());
SetScreenUniforms(node, quad, matPropertyBlock);
}
internal FEMTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double youngsModulus) : base(body)
{
P0 = p0;
P1 = p1;
P2 = p2;
P3 = p3;
YoungsModulus = youngsModulus;
PoissonRatio = 0.3;
Correction = new Vector3d[4];
Vector3d x0 = Body.Positions[P0];
Vector3d x1 = Body.Positions[P1];
Vector3d x2 = Body.Positions[P2];
Vector3d x3 = Body.Positions[P3];
RestVolume = Math.Abs((1.0f / 6.0) * Vector3d.Dot(x3 - x0, Vector3d.Cross(x2 - x0, x1 - x0)));
Matrix3x3d restMatrix = new Matrix3x3d();
restMatrix.SetColumn(0, x0 - x3);
restMatrix.SetColumn(1, x1 - x3);
restMatrix.SetColumn(2, x2 - x3);
InvRestMatrix = restMatrix.Inverse;
}
public static Matrixd ToMatrixd(this Matrix3x3d mat)
{
return(new Matrixd(mat.M00, mat.M10, mat.M20, 0,
mat.M01, mat.M11, mat.M21, 0,
mat.M02, mat.M12, mat.M22, 0,
0, 0, 0, 1));
}
internal StrainTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double stiffness) : base(body)
{
P0 = p0;
P1 = p1;
P2 = p2;
P3 = p3;
Stiffness = stiffness;
NormalizeStretch = false;
NormalizeShear = false;
Correction = new Vector3[4];
Vector3 x0 = body.Positions[P0];
Vector3 x1 = body.Positions[P1];
Vector3 x2 = body.Positions[P2];
Vector3 x3 = body.Positions[P3];
Matrix3x3d restMatrix = new Matrix3x3d();
restMatrix.SetColumn(0, MathConverter.ToVector3d(x1 - x0));
restMatrix.SetColumn(1, MathConverter.ToVector3d(x2 - x0));
restMatrix.SetColumn(2, MathConverter.ToVector3d(x3 - x0));
InvRestMatrix = restMatrix.Inverse;
}
public DeformationBase()
{
uniforms = new Uniforms();
localToCamera = Matrix4x4d.identity;
localToScreen = Matrix4x4d.identity;
localToTangent = Matrix3x3d.identity;
}
//public bool Inverse(ref Matrix3x3d mInv, double tolerance = 1e-06)
public bool Inverse(ref Matrix3x3d mInv, double tolerance)
{
// Invert a 3x3 using cofactors. This is about 8 times faster than
// the Numerical Recipes code which uses Gaussian elimination.
mInv.m[0, 0] = m[1, 1] * m[2, 2] - m[1, 2] * m[2, 1];
mInv.m[0, 1] = m[0, 2] * m[2, 1] - m[0, 1] * m[2, 2];
mInv.m[0, 2] = m[0, 1] * m[1, 2] - m[0, 2] * m[1, 1];
mInv.m[1, 0] = m[1, 2] * m[2, 0] - m[1, 0] * m[2, 2];
mInv.m[1, 1] = m[0, 0] * m[2, 2] - m[0, 2] * m[2, 0];
mInv.m[1, 2] = m[0, 2] * m[1, 0] - m[0, 0] * m[1, 2];
mInv.m[2, 0] = m[1, 0] * m[2, 1] - m[1, 1] * m[2, 0];
mInv.m[2, 1] = m[0, 1] * m[2, 0] - m[0, 0] * m[2, 1];
mInv.m[2, 2] = m[0, 0] * m[1, 1] - m[0, 1] * m[1, 0];
var fDet = m[0, 0] * mInv.m[0, 0] + m[0, 1] * mInv.m[1, 0] + m[0, 2] * mInv.m[2, 0];
if (System.Math.Abs(fDet) <= tolerance)
{
return(false);
}
var fInvDet = 1.0 / fDet;
for (int iRow = 0; iRow < 3; iRow++)
{
for (int iCol = 0; iCol < 3; iCol++)
{
mInv.m[iRow, iCol] *= fInvDet;
}
}
return(true);
}
public DeformationBase()
{
uniforms = new Uniforms();
localToCamera = new Matrix4x4d();
localToScreen = new Matrix4x4d();
localToTangent = new Matrix3x3d();
}
//public Matrix3x3d Inverse(double tolerance = 1e-06)
public Matrix3x3d Inverse(double tolerance)
{
Matrix3x3d kInverse = new Matrix3x3d();
Inverse(ref kInverse, tolerance);
return(kInverse);
}
private void AddBox(Matrix3x3d m)
{
// var left = m.GetRow(0);
// var forward = m.GetRow(1);
// var up = m.GetRow(2);
// var p0 = Vector3d.Multiply(forward, PoleLength);
}
public void GetColumn()
{
Matrix3x3d m = Indexed3x3();
Assert.AreEqual(new Vector3d(0, 1, 2), m.GetColumn(0));
Assert.AreEqual(new Vector3d(3, 4, 5), m.GetColumn(1));
Assert.AreEqual(new Vector3d(6, 7, 8), m.GetColumn(2));
}
public void GetRow()
{
Matrix3x3d m = Indexed3x3();
Assert.AreEqual(new Vector3d(0, 3, 6), m.GetRow(0));
Assert.AreEqual(new Vector3d(1, 4, 7), m.GetRow(1));
Assert.AreEqual(new Vector3d(2, 5, 8), m.GetRow(2));
}
public void TryInverse()
{
Matrix3x3d m = Random3x3(0);
Matrix3x3d inverse = Matrix3x3d.Identity;
m.TryInverse(ref inverse);
Assert.IsTrue((inverse * Random3x3(0)).EqualsWithError(Matrix3x3d.Identity, 1e-6));
}
public static void mult(Matrix3x3d a, Vector3d v, Vector3d result)
{
double x = a.m[0] * v.x + a.m[1] * v.y + a.m[2] * v.z;
double y = a.m[3] * v.x + a.m[4] * v.y + a.m[5] * v.z;
double z = a.m[6] * v.x + a.m[7] * v.y + a.m[8] * v.z;
result.x = x;
result.y = y;
result.z = z;
}
public Matrix3x3d ToMatrix3x3d()
{
Matrix3x3d mat = new Matrix3x3d();
mat.m[0, 0] = m[0, 0]; mat.m[0, 1] = m[0, 1]; mat.m[0, 2] = m[0, 2];
mat.m[1, 0] = m[1, 0]; mat.m[1, 1] = m[1, 1]; mat.m[1, 2] = m[1, 2];
mat.m[2, 0] = m[2, 0]; mat.m[2, 1] = m[2, 1]; mat.m[2, 2] = m[2, 2];
return(mat);
}
public static Matrix3x3d operator *(Matrix3x3d m, double s)
{
Matrix3x3d kProd = new Matrix3x3d();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kProd.m[iRow,iCol] = m.m[iRow,iCol] * s;
}
}
return kProd;
}
public static Matrix3x3d operator -(Matrix3x3d m1, Matrix3x3d m2)
{
Matrix3x3d kSum = new Matrix3x3d();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kSum.m[iRow,iCol] = m1.m[iRow,iCol] - m2.m[iRow,iCol];
}
}
return kSum;
}
private void ComputeGreenStrainAndPiolaStress(Vector3d x1, Vector3d x2, Vector3d x3, Vector3d x4, double mu, double lambda, ref Matrix3x3d epsilon, ref Matrix3x3d sigma, ref double energy)
{
// Determine \partial x/\partial m_i
Matrix3x3d F = new Matrix3x3d();
Vector3d p14 = x1 - x4;
Vector3d p24 = x2 - x4;
Vector3d p34 = x3 - x4;
F.m00 = p14.x * InvRestMatrix.m00 + p24.x * InvRestMatrix.m10 + p34.x * InvRestMatrix.m20;
F.m01 = p14.x * InvRestMatrix.m01 + p24.x * InvRestMatrix.m11 + p34.x * InvRestMatrix.m21;
F.m02 = p14.x * InvRestMatrix.m02 + p24.x * InvRestMatrix.m12 + p34.x * InvRestMatrix.m22;
F.m10 = p14.y * InvRestMatrix.m00 + p24.y * InvRestMatrix.m10 + p34.y * InvRestMatrix.m20;
F.m11 = p14.y * InvRestMatrix.m01 + p24.y * InvRestMatrix.m11 + p34.y * InvRestMatrix.m21;
F.m12 = p14.y * InvRestMatrix.m02 + p24.y * InvRestMatrix.m12 + p34.y * InvRestMatrix.m22;
F.m20 = p14.z * InvRestMatrix.m00 + p24.z * InvRestMatrix.m10 + p34.z * InvRestMatrix.m20;
F.m21 = p14.z * InvRestMatrix.m01 + p24.z * InvRestMatrix.m11 + p34.z * InvRestMatrix.m21;
F.m22 = p14.z * InvRestMatrix.m02 + p24.z * InvRestMatrix.m12 + p34.z * InvRestMatrix.m22;
// epsilon = 1/2 F^T F - I
epsilon.m00 = 0.5 * (F.m00 * F.m00 + F.m10 * F.m10 + F.m20 * F.m20 - 1.0); // xx
epsilon.m11 = 0.5 * (F.m01 * F.m01 + F.m11 * F.m11 + F.m21 * F.m21 - 1.0); // yy
epsilon.m22 = 0.5 * (F.m02 * F.m02 + F.m12 * F.m12 + F.m22 * F.m22 - 1.0); // zz
epsilon.m01 = 0.5 * (F.m00 * F.m01 + F.m10 * F.m11 + F.m20 * F.m21); // xy
epsilon.m02 = 0.5 * (F.m00 * F.m02 + F.m10 * F.m12 + F.m20 * F.m22); // xz
epsilon.m12 = 0.5 * (F.m01 * F.m02 + F.m11 * F.m12 + F.m21 * F.m22); // yz
epsilon.m10 = epsilon.m01;
epsilon.m20 = epsilon.m02;
epsilon.m21 = epsilon.m12;
// P(F) = F(2 mu E + lambda tr(E)I) => E = green strain
double trace = epsilon.m00 + epsilon.m11 + epsilon.m22;
double ltrace = lambda * trace;
sigma = epsilon * 2.0 * mu;
sigma.m00 += ltrace;
sigma.m11 += ltrace;
sigma.m22 += ltrace;
sigma = F * sigma;
double psi = 0.0;
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
psi += epsilon[j, k] * epsilon[j, k];
}
}
psi = mu * psi + 0.5 * lambda * trace * trace;
energy = RestVolume * psi;
}
public void CreatedFromArray()
{
double[] d = new double[] { 0, 1, 2, 3, 4, 5, 6, 7, 8 };
Matrix3x3d m = new Matrix3x3d(d);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
public static Matrix4x4 ToMatrix4x4(Matrix3x3d m)
{
Matrix4x4 mat = new Matrix4x4();
mat.m00 = (float)m.m00; mat.m01 = (float)m.m01; mat.m02 = (float)m.m02; mat.m03 = 0.0f;
mat.m10 = (float)m.m10; mat.m11 = (float)m.m11; mat.m12 = (float)m.m12; mat.m13 = 0.0f;
mat.m20 = (float)m.m20; mat.m21 = (float)m.m21; mat.m22 = (float)m.m22; mat.m23 = 0.0f;
mat.m30 = 0.0f; mat.m31 = 0.0f; mat.m32 = 0.0f; mat.m33 = 0.0f;
return(mat);
}
Matrix3x3d Indexed3x3()
{
Matrix3x3d m = new Matrix3x3d();
for (int i = 0; i < SIZE; i++)
{
m[i] = i;
}
return(m);
}
public void CreatedFromSingleValue()
{
float v = 1.1234f;
Matrix3x3d m = new Matrix3x3d(v);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(v, m[i]);
}
}
public void CreatedFromValues()
{
Matrix3x3d m = new Matrix3x3d(0, 3, 6,
1, 4, 7,
2, 5, 8);
for (int i = 0; i < SIZE; i++)
{
Assert.AreEqual(i, m[i]);
}
}
public void Transform()
{
Quaterniond q2 = Quaterniond.FromAxis(new Vector3d(0, 1, 0), 0.0);
Assert.IsTrue(q2 == Quaterniond.Identity);
Quaterniond q = Quaterniond.FromAxis(new Vector3d(0, 0, 1), (double)(global::System.Math.PI / 2.0));
Matrix3x3d m = q.ToMatrix();
Assert.IsTrue(Vector3d.NearEqual(m * Vector3d.AxisY, new Vector3d(-1, 0, 0)));
Assert.IsTrue(Vector3d.NearEqual(q * Vector3d.AxisY, new Vector3d(-1, 0, 0)));
}
public void SetRow()
{
Matrix3x3d m = new Matrix3x3d();
m.SetRow(0, new Vector3d(0, 3, 6));
m.SetRow(1, new Vector3d(1, 4, 7));
m.SetRow(2, new Vector3d(2, 5, 8));
Assert.AreEqual(new Vector3d(0, 3, 6), m.GetRow(0));
Assert.AreEqual(new Vector3d(1, 4, 7), m.GetRow(1));
Assert.AreEqual(new Vector3d(2, 5, 8), m.GetRow(2));
}
public void SetColumn()
{
Matrix3x3d m = new Matrix3x3d();
m.SetColumn(0, new Vector3d(0, 1, 2));
m.SetColumn(1, new Vector3d(3, 4, 5));
m.SetColumn(2, new Vector3d(6, 7, 8));
Assert.AreEqual(new Vector3d(0, 1, 2), m.GetColumn(0));
Assert.AreEqual(new Vector3d(3, 4, 5), m.GetColumn(1));
Assert.AreEqual(new Vector3d(6, 7, 8), m.GetColumn(2));
}
internal override void ConstrainPositions(double di)
{
Vector3d cm = new Vector3d(0.0, 0.0, 0.0);
double wsum = 0.0;
double mass = Body.ParticleMass;
int numParticles = Body.NumParticles;
for (int i = 0; i < numParticles; i++)
{
cm += Body.Predicted[i] * mass;
wsum += mass;
}
cm /= wsum;
Matrix3x3d A = new Matrix3x3d();
for (int i = 0; i < numParticles; i++)
{
Vector3d q = RestPositions[i];
Vector3d p = Body.Positions[i] - cm;
A[0, 0] += mass * p.x * q.x;
A[0, 1] += mass * p.x * q.y;
A[0, 2] += mass * p.x * q.z;
A[1, 0] += mass * p.y * q.x;
A[1, 1] += mass * p.y * q.y;
A[1, 2] += mass * p.y * q.z;
A[2, 0] += mass * p.z * q.x;
A[2, 1] += mass * p.z * q.y;
A[2, 2] += mass * p.z * q.z;
}
A = A * InvRestMatrix;
double eps = 1e-6;
Matrix3x3d R;
Matrix3x3dDecomposition.PolarDecompositionStable(A, eps, out R);
//Matrix3x3d R = A;
//Matrix3x3d R, U, D;
//Matrix3x3dDecomposition.PolarDecomposition(A, out R, out U, out D);
for (int i = 0; i < numParticles; i++)
{
Vector3d goal = cm + R * RestPositions[i];
Body.Predicted[i] += (goal - Body.Predicted[i]) * Stiffness * di;
}
}
public static Matrix3x3d operator *(Matrix3x3d m1, Matrix3x3d m2)
{
Matrix3x3d kProd = new Matrix3x3d();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kProd.m[iRow,iCol] = m1.m[iRow,0] * m2.m[0,iCol] +
m1.m[iRow,1] * m2.m[1,iCol] +
m1.m[iRow,2] * m2.m[2,iCol];
}
}
return kProd;
}
internal override void ConstrainPositions(double di)
{
Vector3 cm = Vector3.zero;
double wsum = 0.0;
double mass = Body.ParticleMass;
int numParticles = Body.NumParticles;
for (int i = 0; i < numParticles; i++)
{
cm += Body.Predicted[i] * (float)mass;
wsum += mass;
}
cm /= (float)wsum;
Matrix3x3d A = new Matrix3x3d();
for (int i = 0; i < numParticles; i++)
{
Vector3 q = RestPositions[i];
Vector3 p = Body.Positions[i] - cm;
A[0, 0] += mass * p.x * q.x;
A[0, 1] += mass * p.x * q.y;
A[0, 2] += mass * p.x * q.z;
A[1, 0] += mass * p.y * q.x;
A[1, 1] += mass * p.y * q.y;
A[1, 2] += mass * p.y * q.z;
A[2, 0] += mass * p.z * q.x;
A[2, 1] += mass * p.z * q.y;
A[2, 2] += mass * p.z * q.z;
}
A = A * InvRestMatrix;
double eps = 1e-6;
Matrix3x3d R;
Matrix3x3dDecomposition.PolarDecompositionStable(A, eps, out R);
//Matrix3x3d R = A;
//Matrix3x3d R, U, D;
//Matrix3x3dDecomposition.PolarDecomposition(A, out R, out U, out D);
for (int i = 0; i < numParticles; i++)
{
Vector3 goal = cm + MathConverter.ToVector3(R * new Vector3d(RestPositions[i]));
Body.Predicted[i] += (goal - Body.Predicted[i]) * (float)Stiffness * (float)di;
}
}
protected override void SetScreenUniforms(TerrainNode node, TerrainQuad quad, MaterialPropertyBlock matPropertyBlock)
{
double ox = quad.GetOX();
double oy = quad.GetOY();
double l = quad.GetLength();
Vector3d2 p0 = new Vector3d2(ox, oy, R);
Vector3d2 p1 = new Vector3d2(ox + l, oy, R);
Vector3d2 p2 = new Vector3d2(ox, oy + l, R);
Vector3d2 p3 = new Vector3d2(ox + l, oy + l, R);
Vector3d2 pc = (p0 + p3) * 0.5;
double l0 = 0.0, l1 = 0.0, l2 = 0.0, l3 = 0.0;
Vector3d2 v0 = p0.Normalized(ref l0);
Vector3d2 v1 = p1.Normalized(ref l1);
Vector3d2 v2 = p2.Normalized(ref l2);
Vector3d2 v3 = p3.Normalized(ref l3);
Matrix4x4d deformedCorners = new Matrix4x4d(v0.x * R, v1.x * R, v2.x * R, v3.x * R,
v0.y * R, v1.y * R, v2.y * R, v3.y * R,
v0.z * R, v1.z * R, v2.z * R, v3.z * R,
1.0, 1.0, 1.0, 1.0);
matPropertyBlock.AddMatrix(m_uniforms.screenQuadCorners, (m_localToScreen * deformedCorners).ToMatrix4x4());
Matrix4x4d deformedVerticals = new Matrix4x4d( v0.x, v1.x, v2.x, v3.x,
v0.y, v1.y, v2.y, v3.y,
v0.z, v1.z, v2.z, v3.z,
0.0, 0.0, 0.0, 0.0);
matPropertyBlock.AddMatrix(m_uniforms.screenQuadVerticals, (m_localToScreen * deformedVerticals).ToMatrix4x4());
matPropertyBlock.AddVector(m_uniforms.screenQuadCornerNorms, new Vector4((float)l0, (float)l1, (float)l2, (float)l3));
Vector3d2 uz = pc.Normalized();
Vector3d2 ux = (new Vector3d2(0,1,0)).Cross(uz).Normalized();
Vector3d2 uy = uz.Cross(ux);
Matrix4x4d ltow = node.GetLocalToWorld();
Matrix3x3d tangentFrameToWorld = new Matrix3x3d(ltow.m[0,0], ltow.m[0,1], ltow.m[0,2],
ltow.m[1,0], ltow.m[1,1], ltow.m[1,2],
ltow.m[2,0], ltow.m[2,1], ltow.m[2,2]);
Matrix3x3d m = new Matrix3x3d( ux.x, uy.x, uz.x,
ux.y, uy.y, uz.y,
ux.z, uy.z, uz.z);
matPropertyBlock.AddMatrix(m_uniforms.tangentFrameToWorld, (tangentFrameToWorld * m).ToMatrix4x4());
}
public Matrix3x3d ToMatrix3x3d()
{
Matrix3x3d mat = new Matrix3x3d();
mat.m[0,0] = m[0,0]; mat.m[0,1] = m[0,1]; mat.m[0,2] = m[0,2];
mat.m[1,0] = m[1,0]; mat.m[1,1] = m[1,1]; mat.m[1,2] = m[1,2];
mat.m[2,0] = m[2,0]; mat.m[2,1] = m[2,1]; mat.m[2,2] = m[2,2];
return mat;
}
public Matrix3x3d Transpose()
{
Matrix3x3d kTranspose = new Matrix3x3d();
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++) {
kTranspose.m[iRow,iCol] = m[iCol,iRow];
}
}
return kTranspose;
}
//public bool Inverse(ref Matrix3x3d mInv, double tolerance = 1e-06)
public bool Inverse(ref Matrix3x3d mInv, double tolerance)
{
// Invert a 3x3 using cofactors. This is about 8 times faster than
// the Numerical Recipes code which uses Gaussian elimination.
mInv.m[0,0] = m[1,1] * m[2,2] - m[1,2] * m[2,1];
mInv.m[0,1] = m[0,2] * m[2,1] - m[0,1] * m[2,2];
mInv.m[0,2] = m[0,1] * m[1,2] - m[0,2] * m[1,1];
mInv.m[1,0] = m[1,2] * m[2,0] - m[1,0] * m[2,2];
mInv.m[1,1] = m[0,0] * m[2,2] - m[0,2] * m[2,0];
mInv.m[1,2] = m[0,2] * m[1,0] - m[0,0] * m[1,2];
mInv.m[2,0] = m[1,0] * m[2,1] - m[1,1] * m[2,0];
mInv.m[2,1] = m[0,1] * m[2,0] - m[0,0] * m[2,1];
mInv.m[2,2] = m[0,0] * m[1,1] - m[0,1] * m[1,0];
double fDet = m[0,0] * mInv.m[0,0] + m[0,1] * mInv.m[1,0] + m[0,2] * mInv.m[2,0];
if (System.Math.Abs(fDet) <= tolerance) {
return false;
}
double fInvDet = 1.0 / fDet;
for (int iRow = 0; iRow < 3; iRow++) {
for (int iCol = 0; iCol < 3; iCol++)
mInv.m[iRow,iCol] *= fInvDet;
}
return true;
}
/**
* Sets the shader uniforms that are necessary to project on screen the
* TerrainQuad of the given TerrainNode. This method can set the uniforms
* that are common to all the quads of the given terrain.
*/
public virtual void SetUniforms(TerrainNode node, Material mat)
{
if(mat == null || node == null) return;
float d1 = node.GetSplitDist() + 1.0f;
float d2 = 2.0f * node.GetSplitDist();
mat.SetVector(m_uniforms.blending, new Vector2(d1, d2 - d1));
m_localToCamera = node.GetView().GetWorldToCamera() * node.GetLocalToWorld();
m_localToScreen = node.GetView().GetCameraToScreen() * m_localToCamera;
Vector3d2 localCameraPos = node.GetLocalCameraPos();
Vector3d2 worldCamera = node.GetView().GetWorldCameraPos();
Matrix4x4d A = LocalToDeformedDifferential(localCameraPos);
Matrix4x4d B = DeformedToTangentFrame(worldCamera);
Matrix4x4d ltot = B * node.GetLocalToWorld() * A;
m_localToTangent = new Matrix3x3d( ltot.m[0,0], ltot.m[0,1], ltot.m[0,3],
ltot.m[1,0], ltot.m[1,1], ltot.m[1,3],
ltot.m[3,0], ltot.m[3,1], ltot.m[3,3]);
mat.SetMatrix(m_uniforms.localToScreen, m_localToScreen.ToMatrix4x4());
mat.SetMatrix(m_uniforms.localToWorld, node.GetLocalToWorld().ToMatrix4x4());
}
//public Matrix3x3d Inverse(double tolerance = 1e-06)
public Matrix3x3d Inverse(double tolerance)
{
Matrix3x3d kInverse = new Matrix3x3d();
Inverse(ref kInverse, tolerance);
return kInverse;
}
public Matrix3x3d(Matrix3x3d m)
{
System.Array.Copy(m.m, this.m, 9);
}