public Spring( PointMass end1, PointMass end2, float stiffness, float damping )
{
this.end1 = end1;
this.end2 = end2;
this.stiffness = stiffness;
this.damping = damping;
targetLength = Vector3.Distance( end1.position, end2.position ) * 0.95f;
}
public SpringGrid( Rectangle gridSize, Vector2 spacing )
{
_gridSize = gridSize;
var springList = new List<Spring>();
// we offset the gridSize location by half-spacing so the padding is applied evenly all around
gridSize.Location -= spacing.ToPoint();
gridSize.Width += (int)spacing.X;
gridSize.Height += (int)spacing.Y;
var numColumns = (int)( gridSize.Width / spacing.X ) + 1;
var numRows = (int)( gridSize.Height / spacing.Y ) + 1;
_points = new PointMass[numColumns, numRows];
// these fixed points will be used to anchor the grid to fixed positions on the screen
var fixedPoints = new PointMass[numColumns, numRows];
// create the point masses
int column = 0, row = 0;
for( float y = gridSize.Top; y <= gridSize.Bottom; y += spacing.Y )
{
for( float x = gridSize.Left; x <= gridSize.Right; x += spacing.X )
{
_points[column, row] = new PointMass( new Vector3( x, y, 0 ), 1 );
fixedPoints[column, row] = new PointMass( new Vector3( x, y, 0 ), 0 );
column++;
}
row++;
column = 0;
}
// link the point masses with springs
for( var y = 0; y < numRows; y++ )
{
for( var x = 0; x < numColumns; x++ )
{
if( x == 0 || y == 0 || x == numColumns - 1 || y == numRows - 1 ) // anchor the border of the grid
springList.Add( new Spring( fixedPoints[x, y], _points[x, y], 0.1f, 0.1f ) );
else if( x % 3 == 0 && y % 3 == 0 ) // loosely anchor 1/9th of the point masses
springList.Add( new Spring( fixedPoints[x, y], _points[x, y], 0.002f, 0.02f ) );
const float stiffness = 0.28f;
const float damping = 0.06f;
if( x > 0 )
springList.Add( new Spring( _points[x - 1, y], _points[x, y], stiffness, damping ) );
if( y > 0 )
springList.Add( new Spring( _points[x, y - 1], _points[x, y], stiffness, damping ) );
}
}
_springs = springList.ToArray();
}
public Grid(Rectangle size, Vector2 spacing)
{
var springList = new List<Spring>();
int numColumns = (int)(size.Width / spacing.X) + 1;
int numRows = (int)(size.Height / spacing.Y) + 1;
points = new PointMass[numColumns, numRows];
// these fixed points will be used to anchor the grid to fixed positions on the screen
PointMass[,] fixedPoints = new PointMass[numColumns, numRows];
// create the point masses
int column = 0, row = 0;
for (float y = size.Top; y <= size.Bottom; y += spacing.Y)
{
for (float x = size.Left; x <= size.Right; x += spacing.X)
{
points[column, row] = new PointMass(new Vector3(x, y, 0), 1);
fixedPoints[column, row] = new PointMass(new Vector3(x, y, 0), 0);
column++;
}
row++;
column = 0;
}
// link the point masses with springs
for (int y = 0; y < numRows; y++)
for (int x = 0; x < numColumns; x++)
{
if (x == 0 || y == 0 || x == numColumns - 1 || y == numRows - 1) // anchor the border of the grid
springList.Add(new Spring(fixedPoints[x, y], points[x, y], 0.1f, 0.1f));
else if (x % 3 == 0 && y % 3 == 0) // loosely anchor 1/9th of the point masses
springList.Add(new Spring(fixedPoints[x, y], points[x, y], 0.002f, 0.02f));
const float stiffness = 0.28f;
const float damping = 0.06f;
if (x > 0)
springList.Add(new Spring(points[x - 1, y], points[x, y], stiffness, damping));
if (y > 0)
springList.Add(new Spring(points[x, y - 1], points[x, y], stiffness, damping));
}
springs = springList.ToArray();
}
public RodConstraint(PointMass A, PointMass B, double distance = 0) : base(A, B, distance)
{
}
/// <summary>Computations that depend on the observed value of vVector__288 and vdouble__864</summary>
private void Changed_vVector__288_vdouble__864()
{
if (this.Changed_vVector__288_vdouble__864_iterationsDone == 1)
{
return;
}
this.vVector__288_marginal = new PointMass <Vector[]>(this.VVector__288);
this.vdouble__864_marginal = new DistributionStructArray <Gaussian, double>(5622, delegate(int index288) {
return(Gaussian.Uniform());
});
this.vdouble__864_marginal = Distribution.SetPoint <DistributionStructArray <Gaussian, double>, double[]>(this.vdouble__864_marginal, this.Vdouble__864);
// The constant 'vVectorGaussian288'
VectorGaussian vVectorGaussian288 = VectorGaussian.FromNatural(DenseVector.FromArray(new double[3] {
0.0, 0.0, 0.0
}), new PositiveDefiniteMatrix(new double[3, 3] {
{ 1.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 }
}));
this.vVector865_marginal_F = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian288);
// Message from use of 'vdouble__865'
DistributionStructArray <Gaussian, double> vdouble__865_use_B = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__865_use' Backwards messages.
vdouble__865_use_B = new DistributionStructArray <Gaussian, double>(5622);
for (int index288 = 0; index288 < 5622; index288++)
{
vdouble__865_use_B[index288] = Gaussian.Uniform();
// Message to 'vdouble__865_use' from GaussianFromMeanAndVariance factor
vdouble__865_use_B[index288] = GaussianFromMeanAndVarianceOp.MeanAverageConditional(this.Vdouble__864[index288], 0.1);
}
DistributionRefArray <VectorGaussian, Vector> vVector865_rep_B = default(DistributionRefArray <VectorGaussian, Vector>);
// Create array for 'vVector865_rep' Backwards messages.
vVector865_rep_B = new DistributionRefArray <VectorGaussian, Vector>(5622);
for (int index288 = 0; index288 < 5622; index288++)
{
vVector865_rep_B[index288] = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian288);
// Message to 'vVector865_rep' from InnerProduct factor
vVector865_rep_B[index288] = InnerProductOp.AAverageConditional(vdouble__865_use_B[index288], this.VVector__288[index288], vVector865_rep_B[index288]);
}
// Buffer for ReplicateOp_Divide.Marginal<VectorGaussian>
VectorGaussian vVector865_rep_B_toDef = default(VectorGaussian);
// Message to 'vVector865_rep' from Replicate factor
vVector865_rep_B_toDef = ReplicateOp_Divide.ToDefInit <VectorGaussian>(vVectorGaussian288);
// Message to 'vVector865_rep' from Replicate factor
vVector865_rep_B_toDef = ReplicateOp_Divide.ToDef <VectorGaussian>(vVector865_rep_B, vVector865_rep_B_toDef);
// Message to 'vVector865_marginal' from Variable factor
this.vVector865_marginal_F = VariableOp.MarginalAverageConditional <VectorGaussian>(vVector865_rep_B_toDef, vVectorGaussian288, this.vVector865_marginal_F);
DistributionStructArray <Gaussian, double> vdouble__865_F = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__865' Forwards messages.
vdouble__865_F = new DistributionStructArray <Gaussian, double>(5622);
for (int index288 = 0; index288 < 5622; index288++)
{
vdouble__865_F[index288] = Gaussian.Uniform();
}
DistributionRefArray <VectorGaussian, Vector> vVector865_rep_F = default(DistributionRefArray <VectorGaussian, Vector>);
// Create array for 'vVector865_rep' Forwards messages.
vVector865_rep_F = new DistributionRefArray <VectorGaussian, Vector>(5622);
for (int index288 = 0; index288 < 5622; index288++)
{
vVector865_rep_F[index288] = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian288);
}
// Buffer for ReplicateOp_Divide.UsesAverageConditional<VectorGaussian>
VectorGaussian vVector865_rep_F_marginal = default(VectorGaussian);
// Message to 'vVector865_rep' from Replicate factor
vVector865_rep_F_marginal = ReplicateOp_Divide.MarginalInit <VectorGaussian>(vVectorGaussian288);
// Message to 'vVector865_rep' from Replicate factor
vVector865_rep_F_marginal = ReplicateOp_Divide.Marginal <VectorGaussian>(vVector865_rep_B_toDef, vVectorGaussian288, vVector865_rep_F_marginal);
// Buffer for InnerProductOp.InnerProductAverageConditional
// Create array for replicates of 'vVector865_rep_F_index288__AMean'
Vector[] vVector865_rep_F_index288__AMean = new Vector[5622];
for (int index288 = 0; index288 < 5622; index288++)
{
// Message to 'vdouble__865' from InnerProduct factor
vVector865_rep_F_index288__AMean[index288] = InnerProductOp.AMeanInit(vVector865_rep_F[index288]);
}
// Buffer for InnerProductOp.AMean
// Create array for replicates of 'vVector865_rep_F_index288__AVariance'
PositiveDefiniteMatrix[] vVector865_rep_F_index288__AVariance = new PositiveDefiniteMatrix[5622];
for (int index288 = 0; index288 < 5622; index288++)
{
// Message to 'vdouble__865' from InnerProduct factor
vVector865_rep_F_index288__AVariance[index288] = InnerProductOp.AVarianceInit(vVector865_rep_F[index288]);
// Message to 'vVector865_rep' from Replicate factor
vVector865_rep_F[index288] = ReplicateOp_Divide.UsesAverageConditional <VectorGaussian>(vVector865_rep_B[index288], vVector865_rep_F_marginal, index288, vVector865_rep_F[index288]);
}
// Create array for 'vdouble__865_marginal' Forwards messages.
this.vdouble__865_marginal_F = new DistributionStructArray <Gaussian, double>(5622);
for (int index288 = 0; index288 < 5622; index288++)
{
this.vdouble__865_marginal_F[index288] = Gaussian.Uniform();
// Message to 'vdouble__865' from InnerProduct factor
vVector865_rep_F_index288__AVariance[index288] = InnerProductOp.AVariance(vVector865_rep_F[index288], vVector865_rep_F_index288__AVariance[index288]);
// Message to 'vdouble__865' from InnerProduct factor
vVector865_rep_F_index288__AMean[index288] = InnerProductOp.AMean(vVector865_rep_F[index288], vVector865_rep_F_index288__AVariance[index288], vVector865_rep_F_index288__AMean[index288]);
// Message to 'vdouble__865' from InnerProduct factor
vdouble__865_F[index288] = InnerProductOp.InnerProductAverageConditional(vVector865_rep_F_index288__AMean[index288], vVector865_rep_F_index288__AVariance[index288], this.VVector__288[index288]);
// Message to 'vdouble__865_marginal' from DerivedVariable factor
this.vdouble__865_marginal_F[index288] = DerivedVariableOp.MarginalAverageConditional <Gaussian>(vdouble__865_use_B[index288], vdouble__865_F[index288], this.vdouble__865_marginal_F[index288]);
}
this.Changed_vVector__288_vdouble__864_iterationsDone = 1;
}
/// <summary>
/// sets up the SpringGrid springs and points so that it can be drawn
/// </summary>
/// <param name="gridSize"></param>
/// <param name="spacing"></param>
public void SetGridSizeAndSpacing(Rectangle gridSize, Vector2 spacing)
{
_gridSize = gridSize;
var springList = new List <Spring>();
// we offset the gridSize location by half-spacing so the padding is applied evenly all around
gridSize.Location -= spacing.ToPoint();
gridSize.Width += (int)spacing.X;
gridSize.Height += (int)spacing.Y;
var numColumns = (int)(gridSize.Width / spacing.X) + 1;
var numRows = (int)(gridSize.Height / spacing.Y) + 1;
_points = new PointMass[numColumns, numRows];
// these fixed points will be used to anchor the grid to fixed positions on the screen
var fixedPoints = new PointMass[numColumns, numRows];
// create the point masses
int column = 0, row = 0;
for (float y = gridSize.Top; y <= gridSize.Bottom; y += spacing.Y)
{
for (float x = gridSize.Left; x <= gridSize.Right; x += spacing.X)
{
_points[column, row] = new PointMass(new Vector3(x, y, 0), 1);
fixedPoints[column, row] = new PointMass(new Vector3(x, y, 0), 0);
column++;
}
row++;
column = 0;
}
// link the point masses with springs
for (var y = 0; y < numRows; y++)
{
for (var x = 0; x < numColumns; x++)
{
if (x == 0 || y == 0 || x == numColumns - 1 || y == numRows - 1) // anchor the border of the grid
{
springList.Add(new Spring(fixedPoints[x, y], _points[x, y], 0.1f, 0.1f));
}
else if (x % 3 == 0 && y % 3 == 0) // loosely anchor 1/9th of the point masses
{
springList.Add(new Spring(fixedPoints[x, y], _points[x, y], 0.002f, 0.02f));
}
const float stiffness = 0.28f;
const float damping = 0.06f;
if (x > 0)
{
springList.Add(new Spring(_points[x - 1, y], _points[x, y], stiffness, damping));
}
if (y > 0)
{
springList.Add(new Spring(_points[x, y - 1], _points[x, y], stiffness, damping));
}
}
}
_springs = springList.ToArray();
}
void Start()
{
m_Renderers = new List <SpriteRenderer>();
var springList = new List <Spring>();
int numColumns = (int)(size.width / spacing.x) + 1;
int numRows = (int)(size.height / spacing.y) + 1;
m_Points = new PointMass[numColumns, numRows];
// these fixed points will be used to anchor the grid to fixed positions on the screen
PointMass[,] fixedPoints = new PointMass[numColumns, numRows];
// create the point masses
int column = 0, row = 0;
for (float y = size.yMin; y <= size.yMax; y += spacing.y)
{
for (float x = size.xMin; x <= size.xMax; x += spacing.x)
{
m_Points[column, row] = new PointMass(new Vector3(x, y, 0), 1);
fixedPoints[column, row] = new PointMass(new Vector3(x, y, 0), 0);
column++;
}
row++;
column = 0;
}
int count = 0;
// link the point masses with springs
for (int y = 0; y < numRows; y++)
{
for (int x = 0; x < numColumns; x++)
{
if (x == 0 || y == 0 || x == numColumns - 1 || y == numRows - 1) // anchor the border of the grid
{
springList.Add(new Spring(fixedPoints[x, y], m_Points[x, y], 0.1f, 0.1f));
}
else if (x % 3 == 0 && y % 3 == 0) // loosely anchor 1/9th of the point masses
{
springList.Add(new Spring(fixedPoints[x, y], m_Points[x, y], 0.002f, 0.02f));
}
const float stiffness = 0.28f;
const float damping = 0.06f;
if (x > 0)
{
springList.Add(new Spring(m_Points[x - 1, y], m_Points[x, y], stiffness, damping));
}
if (y > 0)
{
springList.Add(new Spring(m_Points[x, y - 1], m_Points[x, y], stiffness, damping));
}
count++;
}
}
for (int i = 0; i < maxInstantiatedLines; i++)
{
m_Renderers.Add(GetNewLineRenderer());
}
m_Springs = springList.ToArray();
}
public BoundsConstraint(Rect size, PointMass point, float friction = 1) : base()
{
PointA = point;
Size = size;
Friction = friction;
}
public void PointMassOnSpring_SI()
{
// make temporary changes to the the ini-file and units-conversion
INIReader.ClearSingleton();
UnitsConversionFactory.ClearSingleton();
var ini = INIReader.Instance();
ini.Values["UnitsSystem"] = "SI";
ini.Values["UnitsSystem"] = "SI";
var gravity = 9.81; // m/s2
ini.Values["gravity"] = gravity.ToString(CultureInfo.InvariantCulture);
ini.Values["UnitLength"] = "cm";
ini.Values["UnitForce"] = "N";
ini.Values["UnitMass"] = "t";
var k3d = new Toolkit();
var uc = UnitsConversionFactory.Conv();
var c_trans = uc["N/cm"].toBase(50);
var c_rot = uc["Ncm/rad"].toBase(50);
var unitCrosec = k3d.CroSec.Spring(new double[] { c_trans, c_trans, c_trans, c_rot, c_rot, c_rot });
var elems = new List <BuilderBeam>()
{
k3d.Part.IndexToBeam(0, 1, "A", unitCrosec),
};
elems[0].bending_stiff = false;
var L = uc["cm"].toBase(10.0);
var points = new List <Point3> {
new Point3(), new Point3(L, 0, 0)
};
var supports = new List <Support>
{
k3d.Support.Support(0, k3d.Support.SupportFixedConditions),
k3d.Support.Support(1, new List <bool>()
{
false, true, true, true, true, true
})
};
var m = uc["kg"].toBase(50); // kg
var point_mass = new PointMass(1, m, 1);
var model = k3d.Model.AssembleModel(elems, supports, new List <Load>()
{
point_mass
}, out var info, out var mass, out var cog, out var msg,
out var runtimeWarning, new List <Joint>(), points);
// calculate the natural vibrations
int from_shape_ind = 1;
int shapes_num = 1;
int max_iter = 100;
double eps = 1e-8;
var disp_dummy = new List <double>();
var scaling = EigenShapesScalingType.matrix;
model = k3d.Algorithms.NaturalVibes(model, from_shape_ind, shapes_num, max_iter, eps, disp_dummy, scaling,
out var nat_frequencies, out var modal_masses, out var participation_facs,
out var participation_facs_disp, out model);
var omega0 = Math.Sqrt(c_trans / m); // rad / sec
var f0 = omega0 / 2.0 / Math.PI; // 1 / sec = Hz
Assert.AreEqual(nat_frequencies[0], f0, 1e-2);
// clear temporary changes to the the ini-file and units-conversion
INIReader.ClearSingleton();
UnitsConversionFactory.ClearSingleton();
ini = INIReader.Instance();
// switch back to SI units
ini.Values["UnitsSystem"] = "SI";
}
public void CtorTest()
{
var pointMass = new PointMass(31.0, 23.0, 11.0);
Assert.AreEqual(11.0, pointMass.Mass);
}
// creates a link between this point and another
public void AttachTo(PointMass p, float restingdist, float stiff, bool visible, Color c)
{
PointLink link = new PointLink(this, p, restingdist, stiff, visible, c);
links.Add(link);
}
public override bool isConstrained(PointMass P)
{
return(Object.ReferenceEquals(PointA, P) || Object.ReferenceEquals(PointB, P));
}
protected static double distanceBetweenSquared(PointMass A, PointMass B)
{
//Distance formula (without sqrt) (X1-X2)^2 + (Y1-Y2)^2
return((A.X - B.X) * (A.X - B.X) + (A.Y - B.Y) * (A.Y - B.Y));
}
protected static double distanceBetween(PointMass A, PointMass B)
{
return(Math.Sqrt(distanceBetweenSquared(A, B)));
}
public void Draw(Physics physics, Camera camera, bool shapes, bool outlines, bool normals, bool points, bool chains, bool tags)
{
material.Projection = camera.projection;
material.View = camera.view;
material.World = Matrix.Identity;
material.CurrentTechnique.Passes[0].Apply();
PrimitiveBatch instance = PrimitiveBatch.GetInstance(device);
instance.Begin(Primitive.Line);
for (int i = 0; i < physics.body_list.Count; i++)
{
Body body = physics.body_list[i];
if (shapes)
{
instance.SetColor(Color.Yellow);
for (int p = 1; p < body.curr_shape.count; p++)
{
instance.AddVertex(body.curr_shape.points[p - 1]);
instance.AddVertex(body.curr_shape.points[p - 0]);
}
instance.AddVertex(body.curr_shape.points[body.curr_shape.count - 1]);
instance.AddVertex(body.curr_shape.points[0]);
}
if (outlines)
{
instance.SetColor(Color.White);
for (int p = 1; p < body.pointmass_list.Length; p++)
{
instance.AddVertex(body.pointmass_list[p - 1].position);
instance.AddVertex(body.pointmass_list[p - 0].position);
}
instance.AddVertex(body.pointmass_list[body.pointmass_list.Length - 1].position);
instance.AddVertex(body.pointmass_list[0].position);
}
if (normals)
{
instance.SetColor(Color.Purple);
for (int p = 0; p < body.pointmass_list.Length; p++)
{
Vector2 pt = body.pointmass_list[p].position;
int prevPt = (p > 0) ? p - 1 : body.pointmass_list.Length - 1;
int nextPt = (p < body.pointmass_list.Length - 1) ? p + 1 : 0;
Vector2 prev = body.pointmass_list[prevPt].position;
Vector2 next = body.pointmass_list[nextPt].position;
Vector2 fromPrev = new Vector2();
fromPrev.X = pt.X - prev.X;
fromPrev.Y = pt.Y - prev.Y;
Vector2 toNext = new Vector2();
toNext.X = next.X - pt.X;
toNext.Y = next.Y - pt.Y;
Vector2 ptNorm = new Vector2();
ptNorm.X = fromPrev.X + toNext.X;
ptNorm.Y = fromPrev.Y + toNext.Y;
VectorHelper.Perpendicular(ref ptNorm);
ptNorm = Vector2.Normalize(ptNorm) * (camera.position.Z * 0.03f);
instance.AddVertex(pt);
instance.AddVertex(pt + ptNorm);
}
}
/*
* if (body is SpringBody)
* {
* SpringBody sbody = body as SpringBody;
*
* instance.SetColor(Color.Green);
* for (int s = 0; s < sbody.spring_list.Count; s++)
* {
*
* instance.AddVertex(sbody.spring_list[s].pointmass_a.position);
* instance.AddVertex(sbody.spring_list[s].pointmass_b.position);
* }
* }*/
}
if (chains)
{
for (int i = 0; i < physics.chain_list.Count; i++)
{
Chain chain = physics.chain_list[i];
instance.SetColor(Color.Green);
for (int s = 0; s < chain.spring_list.Count; s++)
{
instance.AddVertex(chain.spring_list[s].pointmass_a.position);
instance.AddVertex(chain.spring_list[s].pointmass_b.position);
}
}
}
instance.End();
if (points)
{
instance.Begin(Primitive.Quad);
instance.SetColor(Color.Red);
float size = 0.015f;
for (int i = 0; i < physics.body_list.Count; i++)
{
Body body = physics.body_list[i];
for (int p = 0; p < body.pointmass_list.Length; p++)
{
PointMass pm = body.pointmass_list[p];
instance.AddVertex(pm.position.X + size, pm.position.Y - size, 0);
instance.AddVertex(pm.position.X + size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y - size, 0);
}
}
for (int i = 0; i < physics.chain_list.Count; i++)
{
Chain chain = physics.chain_list[i];
for (int p = 0; p < chain.pointmass_list.Count; p++)
{
PointMass pm = chain.pointmass_list[p];
instance.AddVertex(pm.position.X + size, pm.position.Y - size, 0);
instance.AddVertex(pm.position.X + size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y - size, 0);
}
}
instance.End();
}
if (tags)
{
SpriteRenderer spriterenderer = SpriteRenderer.GetInstance(device);
spriterenderer.Begin(null);
for (int i = 0; i < physics.body_list.Count; i++)
{
Body body = physics.body_list[i];
Vector3 proj = camera.Project(new Vector3(body.position, 0));
spriterenderer.AddString(Resources.arial10px_font, string.Format("[id:{0} body:{1}", i, body.ToStringSimple()), proj.X, proj.Y);
}
spriterenderer.End();
}
}
public RegularConstraint(PointMass pointA, PointMass pointB) : base(pointA, pointB)
{
}
public virtual bool isConstrained(PointMass P)
{
return(Object.ReferenceEquals(PointA, P));
}
public void Update(double elapsed)
{
if (!_initialized)
{
Initialize();
}
PenetrationCount = 0;
_collisions.Clear();
for (int i = 0; i < BodyList.Count; i++)
{
BodyList[i].Update(elapsed);
UpdateBitmask(BodyList[i]);
}
for (int i = 0; i < ChainList.Count; i++)
{
ChainList[i].Update(elapsed);
}
for (int i = 0; i < BodyList.Count; i++)
{
for (int j = i + 1; j < BodyList.Count; j++)
{
if (BodyList[i].IsStatic && BodyList[j].IsStatic)
{
continue;
}
if ((BodyList[i].BitmaskX.Mask & BodyList[j].BitmaskX.Mask) == 0 && (BodyList[i].BitmaskY.Mask & BodyList[j].BitmaskY.Mask) == 0)
{
continue;
}
if (!BodyList[i].AABB.Intersects(ref BodyList[j].AABB))
{
continue;
}
_onAABBCollision?.Invoke(BodyList[i], BodyList[j]);
_collisions.AddRange(Collision.Collision.Intersects(BodyList[j], BodyList[i]));
_collisions.AddRange(Collision.Collision.Intersects(BodyList[i], BodyList[j]));
}
}
for (int i = 0; i < _collisions.Count; i++)
{
CollisionInfo info = _collisions[i];
PointMass A = info.PointMassA;
PointMass B1 = info.PointMassB;
PointMass B2 = info.PointMassC;
_onCollision?.Invoke(info.BodyA, info.BodyB, info);
Vector2 bVel = new Vector2
{
X = (B1.Velocity.X + B2.Velocity.X) * 0.5f,
Y = (B1.Velocity.Y + B2.Velocity.Y) * 0.5f
};
Vector2 relVel = new Vector2
{
X = A.Velocity.X - bVel.X,
Y = A.Velocity.Y - bVel.Y
};
float relDot = Vector2.Dot(relVel, info.Normal);
_onPenetration?.Invoke(info.Penetration, info.BodyA, info.BodyB);
if (info.Penetration > 0.3f)
{
PenetrationCount++;
continue;
}
float b1inf = 1f - info.EdgeDistance;
float b2inf = info.EdgeDistance;
float b2MassSum = float.IsPositiveInfinity(B1.Mass) || float.IsPositiveInfinity(B2.Mass) ? float.PositiveInfinity : (B1.Mass + B2.Mass);
float massSum = A.Mass + b2MassSum;
float moveA;
float moveB;
if (float.IsPositiveInfinity(A.Mass))
{
moveA = 0f;
moveB = info.Penetration + 0.001f;
}
else if (float.IsPositiveInfinity(b2MassSum))
{
moveA = info.Penetration + 0.001f;
moveB = 0f;
}
else
{
moveA = info.Penetration * (b2MassSum / massSum);
moveB = info.Penetration * (A.Mass / massSum);
}
float B1move = moveB * b1inf;
float B2move = moveB * b2inf;
float invMassA = float.IsPositiveInfinity(A.Mass) ? 0f : 1f / A.Mass;
float invMassB = float.IsPositiveInfinity(b2MassSum) ? 0f : 1f / b2MassSum;
float jDenom = invMassA + invMassB;
Vector2 numV = new Vector2();
float elasticity = Elasticity;
numV.X = relVel.X * elasticity;
numV.Y = relVel.Y * elasticity;
float jNumerator = Vector2.Dot(numV, info.Normal);
jNumerator = -jNumerator;
float j = jNumerator / jDenom;
if (!float.IsPositiveInfinity(A.Mass))
{
A.Position.X += info.Normal.X * moveA;
A.Position.Y += info.Normal.Y * moveA;
}
if (!float.IsPositiveInfinity(B1.Mass))
{
B1.Position.X -= info.Normal.X * B1move;
B1.Position.Y -= info.Normal.Y * B1move;
}
if (!float.IsPositiveInfinity(B2.Mass))
{
B2.Position.X -= info.Normal.X * B2move;
B2.Position.Y -= info.Normal.Y * B2move;
}
Vector2 tangent = info.Normal.Perpendicular();
float fNumerator = Vector2.Dot(relVel, tangent);
fNumerator *= Friction;
float f = fNumerator / jDenom;
if (relDot <= Mathf.Epsilon)
{
if (!float.IsPositiveInfinity(A.Mass))
{
A.Velocity.X += info.Normal.X * (j / A.Mass) - tangent.X * (f / A.Mass);
A.Velocity.Y += info.Normal.Y * (j / A.Mass) - tangent.Y * (f / A.Mass);
}
if (!float.IsPositiveInfinity(b2MassSum))
{
B1.Velocity.X -= info.Normal.X * (j / b2MassSum) * b1inf - tangent.X * (f / b2MassSum) * b1inf;
B1.Velocity.Y -= info.Normal.Y * (j / b2MassSum) * b1inf - tangent.Y * (f / b2MassSum) * b1inf;
}
if (!float.IsPositiveInfinity(b2MassSum))
{
B2.Velocity.X -= info.Normal.X * (j / b2MassSum) * b2inf - tangent.X * (f / b2MassSum) * b2inf;
B2.Velocity.Y -= info.Normal.Y * (j / b2MassSum) * b2inf - tangent.Y * (f / b2MassSum) * b2inf;
}
}
}
for (int i = 0; i < BodyList.Count; i++)
{
BodyList[i].UpdateBodyPositionVelocityForce();
}
}
// Update is called once per frame
void Update()
{
// ball is not moving
if (velocity.x == 0 && velocity.y == 0)
{
sphere.transform.position = pos;
// wait x seconds before deleting
alivetime -= Time.deltaTime;
if (alivetime < 0)
{
Destroy(gameObject);
}
return;
}
// Calculate a new position with velocity, airresistance, gravity, wind
TriangleStruct[] lefttriangles = mountain.lefttriangles;
TriangleStruct[] righttriangles = mountain.righttriangles;
float gravity = props.GetGravity();
float airres = props.GetAirRes();
float windspeed = props.GetWindforce();
velocity = velocity + new Vector3(0, gravity, 0);
if (velocity.x > 0)
{
velocity = velocity + new Vector3(-airres, 0, 0);
}
else
{
velocity = velocity + new Vector3(airres, 0, 0);
}
velocity = velocity + new Vector3(windspeed, 0, 0);
Vector3 newpos = pos + velocity;
// Collision Detection + Response
// Check for goats
Transform goats = goatspawner.transform;
// Get each goat instance
foreach (Transform goat in goats)
{
GoatPoints gp = (GoatPoints)goat.GetComponent("GoatPoints");
// surround each goat by a generous sphere bounding box and check that first
float r = gp.radius;
PointMass c = gp.center;
if ((c.position - (new Vector2(pos.x, pos.y))).magnitude > r)
{
continue;
}
// Check each point in the goat for a collision
ArrayList points = gp.points;
for (int i = 0; i < points.Count; i++)
{
PointMass p = (PointMass)points [i];
if (HitGoat(p))
{
// If there is a collision, delete this ball and unanchor the goat (giving it the ball's velocity)
Vector2 velocity2d = new Vector2(velocity.x, velocity.y);
gp.Unhinge();
p.GiveVelocity(velocity2d);
Destroy(gameObject);
}
}
}
// bounding boxes for mountains
if (pos.y - radius <= 2.0f)
{
// left
if (pos.x + radius >= -5.0f && pos.x - radius <= -1.0f)
{
// left mountain triangles
for (int i = 0; i < lefttriangles.Length; i++)
{
TriangleStruct lefttri = lefttriangles [i];
if (lefttri.left == lefttri.right)
{
continue;
}
// If there is a collision, change the velocity and recursively calulate a new position (up to 'counter' times)
if (lefttri.SphereCollides(newpos, radius))
{
ChangeVelocity(lefttri.getNormal());
trycounter++;
Update();
return;
}
}
}
// right
if (pos.x + radius >= 1.0f && pos.x - radius <= 5.0f)
{
// right mountain triangles
for (int i = 0; i < righttriangles.Length; i++)
{
TriangleStruct righttri = righttriangles [i];
if (righttri.left == righttri.right)
{
continue;
}
if (righttri.SphereCollides(newpos, radius))
{
ChangeVelocity(righttri.getNormal());
trycounter++;
Update();
return;
}
}
}
// middle
if (pos.x + radius >= -1.0f && pos.x - radius <= 1.0f)
{
// top of mountain
TriangleStruct toptriangle = mountain.toptriangle;
if (toptriangle.SphereCollides(newpos, radius))
{
ChangeVelocity(toptriangle.getNormal());
trycounter++;
Update();
return;
}
}
}
// Update Position
pos = newpos;
sphere.transform.position = pos;
trycounter = 0;
}
public void GivenAPointMass()
{
_pointMass = new PointMass(1 .SI().Kilogram);
}
/// <inheritdoc cref="Accumulator{T}.Add"/>
public void Add(PointMass <T> item)
{
point = item.Point;
}
public void Initialize()
{
// DO NOT make this a constructor, because it makes the test not notice complete lack of serialization as an empty object is set up exactly as the thing
// you are trying to deserialize.
this.pareto = new Pareto(1.2, 3.5);
this.poisson = new Poisson(2.3);
this.wishart = new Wishart(20, new PositiveDefiniteMatrix(new double[, ] {
{ 22, 21 }, { 21, 23 }
}));
this.vectorGaussian = new VectorGaussian(Vector.FromArray(13, 14), new PositiveDefiniteMatrix(new double[, ] {
{ 16, 15 }, { 15, 17 }
}));
this.unnormalizedDiscrete = UnnormalizedDiscrete.FromLogProbs(DenseVector.FromArray(5.1, 5.2, 5.3));
this.pointMass = PointMass <double> .Create(1.1);
this.gaussian = new Gaussian(11.0, 12.0);
this.nonconjugateGaussian = new NonconjugateGaussian(1.2, 2.3, 3.4, 4.5);
this.gamma = new Gamma(9.0, 10.0);
this.gammaPower = new GammaPower(5.6, 2.8, 3.4);
this.discrete = new Discrete(6.0, 7.0, 8.0);
this.conjugateDirichlet = new ConjugateDirichlet(1.2, 2.3, 3.4, 4.5);
this.dirichlet = new Dirichlet(3.0, 4.0, 5.0);
this.beta = new Beta(2.0, 1.0);
this.binomial = new Binomial(5, 0.8);
this.bernoulli = new Bernoulli(0.6);
this.sparseBernoulliList = SparseBernoulliList.Constant(4, new Bernoulli(0.1));
this.sparseBernoulliList[1] = new Bernoulli(0.9);
this.sparseBernoulliList[3] = new Bernoulli(0.7);
this.sparseBetaList = SparseBetaList.Constant(5, new Beta(2.0, 2.0));
this.sparseBetaList[0] = new Beta(3.0, 4.0);
this.sparseBetaList[1] = new Beta(5.0, 6.0);
this.sparseGaussianList = SparseGaussianList.Constant(6, Gaussian.FromMeanAndPrecision(0.1, 0.2));
this.sparseGaussianList[4] = Gaussian.FromMeanAndPrecision(0.3, 0.4);
this.sparseGaussianList[5] = Gaussian.FromMeanAndPrecision(0.5, 0.6);
this.sparseGammaList = SparseGammaList.Constant(1, Gamma.FromShapeAndRate(1.0, 2.0));
this.truncatedGamma = new TruncatedGamma(1.2, 2.3, 3.4, 4.5);
this.truncatedGaussian = new TruncatedGaussian(1.2, 3.4, 5.6, 7.8);
this.wrappedGaussian = new WrappedGaussian(1.2, 2.3, 3.4);
ga = Distribution <double> .Array(new[] { this.gaussian, this.gaussian });
vga = Distribution <Vector> .Array(new[] { this.vectorGaussian, this.vectorGaussian });
ga2D = Distribution <double> .Array(new[, ] {
{ this.gaussian, this.gaussian }, { this.gaussian, this.gaussian }
});
vga2D = Distribution <Vector> .Array(new[, ] {
{ this.vectorGaussian, this.vectorGaussian }, { this.vectorGaussian, this.vectorGaussian }
});
gaJ = Distribution <double> .Array(new[] { new[] { this.gaussian, this.gaussian }, new[] { this.gaussian, this.gaussian } });
vgaJ = Distribution <Vector> .Array(new[] { new[] { this.vectorGaussian, this.vectorGaussian }, new[] { this.vectorGaussian, this.vectorGaussian } });
var gp = new GaussianProcess(new ConstantFunction(0), new SquaredExponential(0));
var basis = Util.ArrayInit(2, i => Vector.FromArray(1.0 * i));
this.sparseGp = new SparseGP(new SparseGPFixed(gp, basis));
this.quantileEstimator = new QuantileEstimator(0.01);
this.quantileEstimator.Add(5);
this.stringDistribution1 = StringDistribution.String("aa").Append(StringDistribution.OneOf("b", "ccc")).Append("dddd");
this.stringDistribution2 = new StringDistribution();
this.stringDistribution2.SetToProduct(StringDistribution.OneOf("a", "b"), StringDistribution.OneOf("b", "c"));
}
/// <inheritdoc cref="Estimator{T}.GetDistribution"/>
public PointMass <T> GetDistribution(PointMass <T> result)
{
return(new PointMass <T>(point));
}
/// <summary>Computations that depend on the observed value of vVector__99</summary>
private void Changed_vVector__99()
{
if (this.Changed_vVector__99_iterationsDone == 1)
{
return;
}
this.vVector__99_marginal = new PointMass <Vector[]>(this.VVector__99);
// The constant 'vVectorGaussian99'
VectorGaussian vVectorGaussian99 = VectorGaussian.FromNatural(DenseVector.FromArray(new double[3] {
1547829870.0, 525077980.0, 200270.0
}), new PositiveDefiniteMatrix(new double[3, 3] {
{ 4254590363351.0, 1127383488860.0, 433199230.0 }, { 1127383488860.0, 482723521821.0, 146764360.0 }, { 433199230.0, 146764360.0, 56221.0 }
}));
this.vVector297_marginal_F = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian99);
// Buffer for ReplicateOp_Divide.Marginal<VectorGaussian>
VectorGaussian vVector297_rep_B_toDef = default(VectorGaussian);
// Message to 'vVector297_rep' from Replicate factor
vVector297_rep_B_toDef = ReplicateOp_Divide.ToDefInit <VectorGaussian>(vVectorGaussian99);
// Message to 'vVector297_marginal' from Variable factor
this.vVector297_marginal_F = VariableOp.MarginalAverageConditional <VectorGaussian>(vVector297_rep_B_toDef, vVectorGaussian99, this.vVector297_marginal_F);
DistributionStructArray <Gaussian, double> vdouble__297_F = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__297' Forwards messages.
vdouble__297_F = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
vdouble__297_F[index99] = Gaussian.Uniform();
}
DistributionStructArray <Gaussian, double> vdouble__298_F = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__298' Forwards messages.
vdouble__298_F = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
vdouble__298_F[index99] = Gaussian.Uniform();
}
DistributionRefArray <VectorGaussian, Vector> vVector297_rep_F = default(DistributionRefArray <VectorGaussian, Vector>);
DistributionRefArray <VectorGaussian, Vector> vVector297_rep_B = default(DistributionRefArray <VectorGaussian, Vector>);
// Create array for 'vVector297_rep' Forwards messages.
vVector297_rep_F = new DistributionRefArray <VectorGaussian, Vector>(1);
// Create array for 'vVector297_rep' Backwards messages.
vVector297_rep_B = new DistributionRefArray <VectorGaussian, Vector>(1);
for (int index99 = 0; index99 < 1; index99++)
{
vVector297_rep_B[index99] = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian99);
vVector297_rep_F[index99] = ArrayHelper.MakeUniform <VectorGaussian>(vVectorGaussian99);
}
// Buffer for ReplicateOp_Divide.UsesAverageConditional<VectorGaussian>
VectorGaussian vVector297_rep_F_marginal = default(VectorGaussian);
// Message to 'vVector297_rep' from Replicate factor
vVector297_rep_F_marginal = ReplicateOp_Divide.MarginalInit <VectorGaussian>(vVectorGaussian99);
// Message to 'vVector297_rep' from Replicate factor
vVector297_rep_F_marginal = ReplicateOp_Divide.Marginal <VectorGaussian>(vVector297_rep_B_toDef, vVectorGaussian99, vVector297_rep_F_marginal);
// Buffer for InnerProductOp.InnerProductAverageConditional
// Create array for replicates of 'vVector297_rep_F_index99__AMean'
Vector[] vVector297_rep_F_index99__AMean = new Vector[1];
for (int index99 = 0; index99 < 1; index99++)
{
// Message to 'vdouble__298' from InnerProduct factor
vVector297_rep_F_index99__AMean[index99] = InnerProductOp.AMeanInit(vVector297_rep_F[index99]);
}
// Buffer for InnerProductOp.AMean
// Create array for replicates of 'vVector297_rep_F_index99__AVariance'
PositiveDefiniteMatrix[] vVector297_rep_F_index99__AVariance = new PositiveDefiniteMatrix[1];
for (int index99 = 0; index99 < 1; index99++)
{
// Message to 'vdouble__298' from InnerProduct factor
vVector297_rep_F_index99__AVariance[index99] = InnerProductOp.AVarianceInit(vVector297_rep_F[index99]);
// Message to 'vVector297_rep' from Replicate factor
vVector297_rep_F[index99] = ReplicateOp_Divide.UsesAverageConditional <VectorGaussian>(vVector297_rep_B[index99], vVector297_rep_F_marginal, index99, vVector297_rep_F[index99]);
}
// Create array for 'vdouble__298_marginal' Forwards messages.
this.vdouble__298_marginal_F = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
this.vdouble__298_marginal_F[index99] = Gaussian.Uniform();
}
// Message from use of 'vdouble__298'
DistributionStructArray <Gaussian, double> vdouble__298_use_B = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__298_use' Backwards messages.
vdouble__298_use_B = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
vdouble__298_use_B[index99] = Gaussian.Uniform();
// Message to 'vdouble__298' from InnerProduct factor
vVector297_rep_F_index99__AVariance[index99] = InnerProductOp.AVariance(vVector297_rep_F[index99], vVector297_rep_F_index99__AVariance[index99]);
// Message to 'vdouble__298' from InnerProduct factor
vVector297_rep_F_index99__AMean[index99] = InnerProductOp.AMean(vVector297_rep_F[index99], vVector297_rep_F_index99__AVariance[index99], vVector297_rep_F_index99__AMean[index99]);
// Message to 'vdouble__298' from InnerProduct factor
vdouble__298_F[index99] = InnerProductOp.InnerProductAverageConditional(vVector297_rep_F_index99__AMean[index99], vVector297_rep_F_index99__AVariance[index99], this.VVector__99[index99]);
// Message to 'vdouble__298_marginal' from DerivedVariable factor
this.vdouble__298_marginal_F[index99] = DerivedVariableOp.MarginalAverageConditional <Gaussian>(vdouble__298_use_B[index99], vdouble__298_F[index99], this.vdouble__298_marginal_F[index99]);
}
// Create array for 'vdouble__297_marginal' Forwards messages.
this.vdouble__297_marginal_F = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
this.vdouble__297_marginal_F[index99] = Gaussian.Uniform();
}
// Message from use of 'vdouble__297'
DistributionStructArray <Gaussian, double> vdouble__297_use_B = default(DistributionStructArray <Gaussian, double>);
// Create array for 'vdouble__297_use' Backwards messages.
vdouble__297_use_B = new DistributionStructArray <Gaussian, double>(1);
for (int index99 = 0; index99 < 1; index99++)
{
vdouble__297_use_B[index99] = Gaussian.Uniform();
// Message to 'vdouble__297' from GaussianFromMeanAndVariance factor
vdouble__297_F[index99] = GaussianFromMeanAndVarianceOp.SampleAverageConditional(vdouble__298_F[index99], 0.1);
// Message to 'vdouble__297_marginal' from Variable factor
this.vdouble__297_marginal_F[index99] = VariableOp.MarginalAverageConditional <Gaussian>(vdouble__297_use_B[index99], vdouble__297_F[index99], this.vdouble__297_marginal_F[index99]);
}
this.Changed_vVector__99_iterationsDone = 1;
}
static void Main(string[] args)
{
//PhysicalContext is the core class represents set of entities and
//set of force evaluation laws binded to it.
//Type parameter represents the type of keys used to adding and retrieving entities.
var context = new PhysicalContext <string>
(
timePerTick: dt, //The time, that is considered to be as small, as force values are uniform.
//The smaller timePerTick is, the better precision we get.
//Be aware, time for evaluating state of entities after certain period of time
//is proportional to (timePerTick)^-1.
capacity: 1 //Number of entities required to be added to context.
);
//Adding entity
var freeFallEntity = new PointMass
(
x: new AxisStatus(3.4), //Position = 3.4, zero velocity
y: new AxisStatus(0, 10), //Zero position, velocity = 10
z: new AxisStatus(1.1, 2), //Position = 1.1, Velocity = 2
mass: 77.7
);
context.AddEntity
(
"freeFallEntity", //key
freeFallEntity, //entity
c => //Only force that have impact on this entity
- new Force
(
xComponent: 0,
yComponent: c["freeFallEntity"].Mass * freeFallAcceleration,
zComponent: 0
)
//This force is always vertical and equal to mass of entity multiplied
//by free fall acceleration.
);
Console.WriteLine($"Start state is \n{context["freeFallEntity"]}");
//Evaluating the state of context after 1 second.
context.Tick(timeSpan: 1);
Console.WriteLine($"\nState of entity after 1 second is \n{context["freeFallEntity"]}");
//If you want to record some data while context is updating,
//you may subscribe to OnTick event or better use class derived from ContextObserver.
var yPositionTracker = new ContextTracker <string, double>
(
context,
c => c["freeFallEntity"].Y.Position
);
context.Tick(1);
//Context tracker implements IReadonlyDictionary.
Console.WriteLine($"\nOn tick 10345 y position is {yPositionTracker[10345]}");
Console.WriteLine($"\nOn time 1.27 y position is {yPositionTracker.GetApproximately(1.27)}");
//Throws exception because tracker has started recording when time is context
//was already 1.00.
//Console.WriteLine($"On time 0.4 y position is {yPositionTracker.GetApproximately(0.4)}");
//Throws exception because tracker hasn't record data yet because time in context is 2.00.
//Console.WriteLine($"On time 2.7 y position is {yPositionTracker.GetApproximately(2.7)}");
//Don't forget to dispose tracker when you don't need it anymore.
//It will increase performance because tracker doesn't record data anymore.
yPositionTracker.Dispose();
context.Tick(1);
//Time is context is 3.0, but tracker is already disposed and doesn't record data anymore.
Console.WriteLine
(
$"\nTracker records data during time span {yPositionTracker.ObservationBeginTime} - {yPositionTracker.LastObservationTime}" +
$"\nAverage y position is {yPositionTracker.Sum(record => record.Value) / yPositionTracker.Count}"
);
//However, if you want to record only max, min, average value etc,
//better use ContextDependentValue, because it doesn't use a lot of memory
//to record value in each tick.
var maxYVelocity = new ContextDependentValue <string, double>
(
startValue: context["freeFallEntity"].Y.Velocity,
observableContext: context,
newValueFunc: (c, oldValue) =>
c["freeFallEntity"].Velocity > oldValue ? new double?(c["freeFallEntity"].Velocity) : null
//null value means that there is no reason to change value
);
context.Tick(1);
Console.WriteLine
(
$"\nMax y velocity in time span {maxYVelocity.ObservationBeginTime} - " +
$"{maxYVelocity.LastObservationTime} is {maxYVelocity.Value}"
);
//So, lets create something more complex.
//What about the spring pendulum with air resistance?
var pendulumContext = new PhysicalContext <PendulumEntities>(dt, 2);
var axis = new PointMass(0, 0, 0, 0);
var mass = new PointMass(1, 0, 0, 0);
pendulumContext.AddEntity(PendulumEntities.Axis, axis);
pendulumContext.AddEntity
(
PendulumEntities.Mass,
mass,
c => new Force(0, c[PendulumEntities.Mass].Mass * freeFallAcceleration, 0), //Gravity
HookesLaw.GetLaw //Mechanix.Laws contains amount of static classes to help force evaluating.
(
PendulumEntities.Mass,
PendulumEntities.Axis,
1, //undeformed length of spring
10 //elasticity coefficient
)
);
//Or set of elastic balls.
var ballsContext = new PhysicalContext <int>(dt, 100);
var random = new Random();
const double radius = 1;
const double elasticity = 100;
for (int i = 0; i < 100; ++i)
{
var ball = new PointMass
(
new AxisStatus(random.NextDouble(), random.NextDouble()),
new AxisStatus(random.NextDouble(), random.NextDouble()),
new AxisStatus(random.NextDouble(), random.NextDouble()),
random.NextDouble()
);
ballsContext.AddEntity
(
i,
ball,
c =>
{
var force = Force.Zero;
foreach (var pair in c)
{
if (pair.Key != i)
{
force += RadialCollisionLaw.Eval
(
c[i],
pair.Value,
radius,
elasticity
);
}
}
return(force);
}
);
}
}
public Spring(PointMass end1, PointMass end2, float stiffness, float damping)
{
End1 = end1;
End2 = end2;
Stiffness = stiffness;
Damping = damping;
TargetLength = Vector3.Distance(end1.Position, end2.Position) * 0.95f;
}
public Constraint(PointMass pointA, PointMass pointB)
{
this.pointA = pointA;
this.pointB = pointB;
this.restingDistance = Vector3.Distance(pointA.Position, pointB.Position);
}
public void Draw(Body body, Camera camera, bool shapes, bool outlines, bool normals, bool points, bool chains, bool tags)
{
material.Projection = camera.projection;
material.View = camera.view;
material.World = Matrix.Identity;
material.CurrentTechnique.Passes[0].Apply();
PrimitiveBatch instance = PrimitiveBatch.GetInstance(device);
instance.Begin(Primitive.Line);
if (shapes)
{
instance.SetColor(Color.Purple);
for (int p = 1; p < body.curr_shape.count; p++)
{
instance.AddVertex(body.curr_shape.points[p - 1]);
instance.AddVertex(body.curr_shape.points[p - 0]);
}
instance.AddVertex(body.curr_shape.points[body.curr_shape.count - 1]);
instance.AddVertex(body.curr_shape.points[0]);
}
if (outlines)
{
instance.SetColor(Color.White);
for (int p = 1; p < body.pointmass_list.Length; p++)
{
instance.AddVertex(body.pointmass_list[p - 1].position);
instance.AddVertex(body.pointmass_list[p - 0].position);
}
instance.AddVertex(body.pointmass_list[body.pointmass_list.Length - 1].position);
instance.AddVertex(body.pointmass_list[0].position);
}
if (normals)
{
instance.SetColor(Color.Purple);
for (int p = 0; p < body.pointmass_list.Length; p++)
{
Vector2 pt = body.pointmass_list[p].position;
int prevPt = (p > 0) ? p - 1 : body.pointmass_list.Length - 1;
int nextPt = (p < body.pointmass_list.Length - 1) ? p + 1 : 0;
Vector2 prev = body.pointmass_list[prevPt].position;
Vector2 next = body.pointmass_list[nextPt].position;
Vector2 fromPrev = new Vector2();
fromPrev.X = pt.X - prev.X;
fromPrev.Y = pt.Y - prev.Y;
Vector2 toNext = new Vector2();
toNext.X = next.X - pt.X;
toNext.Y = next.Y - pt.Y;
Vector2 ptNorm = new Vector2();
ptNorm.X = fromPrev.X + toNext.X;
ptNorm.Y = fromPrev.Y + toNext.Y;
VectorHelper.Perpendicular(ref ptNorm);
ptNorm = Vector2.Normalize(ptNorm) * (camera.position.Z * 0.03f);;
instance.AddVertex(pt);
instance.AddVertex(pt + ptNorm);
}
}
instance.End();
if (points)
{
instance.Begin(Primitive.Quad);
instance.SetColor(Color.Red);
float size = 0.015f;
for (int p = 0; p < body.pointmass_list.Length; p++)
{
PointMass pm = body.pointmass_list[p];
instance.AddVertex(pm.position.X + size, pm.position.Y - size, 0);
instance.AddVertex(pm.position.X + size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y + size, 0);
instance.AddVertex(pm.position.X - size, pm.position.Y - size, 0);
}
instance.End();
}
if (tags)
{
SpriteRenderer spriterenderer = SpriteRenderer.GetInstance(device);
spriterenderer.Begin(null);
Vector3 proj = camera.Project(new Vector3(body.position, 0));
spriterenderer.AddString(Resources.arial10px_font, body.ToStringSimple(), proj.X, proj.Y);
spriterenderer.End();
}
}
public DampedConstraint(PointMass pointA, PointMass pointB, float dampFactor) : base(pointA, pointB)
{
this.dampFactor = dampFactor;
//tolerance = distance * 1.5f;
}
