public void Add(Vector2 p, Vector2 n)
{
Intersections.Add(p);
Normals.Add(n);
/*ata.M11 += n.X * n.X;
ata.M12 += n.X * n.Y;
ata.M13 += 0; //x * z
ata.M21 += n.Y * n.Y;
ata.M22 += 0; //y * z
ata.M23 += 0; //z * z
float dot = n.X * p.X + n.Y * p.Y;
atb.X += dot * n.X;
atb.Y += dot * n.Y;
btb += dot * dot;*/
mass_point += new Vector3(p, 0);
}