// -------------------------------------------------------------------------------
public float Solve(Vector3 outx, float svd_tol, int svd_sweeps, float pinv_tol)
{
if (this.data.numPoints == 0)
{
// Debug.LogError("Invalid Argument");
}
this.massPoint = new Vector3(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z);
this.massPoint = VecUtils.Scale(this.massPoint, (1.0f / (float)this.data.numPoints));
this.SetAta();
this.SetAtb();
Vector3 tmpv = MatUtils.Vmul_symmetric(this.ata, this.massPoint);
atb = VecUtils.Sub(atb, tmpv);
this.x = new Vector3(0, 0, 0);
float result = Svd.SolveSymmetric(this.ata, this.atb, this.x, svd_tol, svd_sweeps, pinv_tol);
VecUtils.AddScaled(this.x, 1, this.massPoint);
this.SetAtb();
outx.Set(x.x, x.y, x.z);
this.hasSolution = true;
return(result);
}
public float solve(out Vec3 outx, float svd_tol,
int svd_sweeps, float pinv_tol)
{
if (this.data.numPoints == 0)
{
throw new UnityException("...");
}
this.massPoint.set(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z);
VecUtils.scale(out this.massPoint, 1.0f / this.data.numPoints, this.massPoint);
this.setAta();
this.setAtb();
Vec3 tmpv;
MatUtils.vmul_symmetric(out tmpv, this.ata, this.massPoint);
VecUtils.sub(out this.atb, this.atb, tmpv);
this.x.clear();
float result = Svd.solveSymmetric(this.ata, this.atb, out this.x,
svd_tol, svd_sweeps, pinv_tol);
VecUtils.addScaled(out this.x, 1.0f, this.massPoint);
this.setAtb();
outx = x;
this.hasSolution = true;
return(result);
}
private static float calcError(Mat3 A, Vec3 x, Vec3 b)
{
Vec3 vtmp;
MatUtils.vmul(out vtmp, A, x);
VecUtils.sub(out vtmp, b, vtmp);
return(VecUtils.dot(vtmp, vtmp));
}
private static void normalize(out float nx, out float ny, out float nz, float ix, float iy, float iz)
{
Vec3 tmpv = new Vec3(ix, iy, iz);
VecUtils.normalize(out tmpv, tmpv);
nx = tmpv.x;
ny = tmpv.y;
nz = tmpv.z;
}
private static float calcError(SMat3 origA, Vec3 x, Vec3 b)
{
Mat3 A = new Mat3();
Vec3 vtmp;
A.setSymmetric(origA);
MatUtils.vmul(out vtmp, A, x);
VecUtils.sub(out vtmp, b, vtmp);
return(VecUtils.dot(vtmp, vtmp));
}
// -------------------------------------------------------------------------------
public float GetError(Vector3 pos)
{
if (!this.hasSolution)
{
this.SetAta();
this.SetAtb();
}
Vector3 atax = MatUtils.Vmul_symmetric(this.ata, pos);
return(VecUtils.Dot(pos, atax) - 2 * VecUtils.Dot(pos, this.atb) + this.data.btb);
}
public float getError(Vec3 pos)
{
if (!this.hasSolution)
{
this.setAta();
this.setAtb();
}
Vec3 atax;
MatUtils.vmul_symmetric(out atax, this.ata, pos);
return(VecUtils.dot(pos, atax) - 2.0f * VecUtils.dot(pos, this.atb)
+ this.data.btb);
}
private void TryCreateRoom(float randomModifier, ICollection <Room> rooms)
{
var tryLeft = 100;
Room room = null;
while (room == null || GetCollidings(room, rooms).Count != 0 ||
room.GetLeft() < -mapWidth / 2f || room.GetRight() > mapWidth / 2f ||
room.GetTop() < -mapHeight / 2f || room.GetBottom() > mapHeight / 2f)
{
if (tryLeft <= 0)
{
return;
}
var pos = VecUtils.GetRandomPointInCircle(rand, (float)mapWidth / 2);
room = new Room((int)pos.x, (int)pos.y,
(int)(roomWidth.Random * randomModifier), (int)(roomHeight.Random * randomModifier));
tryLeft--;
}
rooms.Add(room);
}
