// ------------------------------------------------------------------------------- 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); }