1

この記事で説明されている手順をコーディングしてみました。

public static double[] localize(final double[] P1, final double[] P2, final double[] P3, final double[] P4, final double r1, final double r2, final double r3, final double r4)
{

    Point3d p1 = new Point3d(P1);
    Point3d p2 = new Point3d(P2);
    Point3d p3 = new Point3d(P3);
    Point3d p4 = new Point3d(P4);

    Vector3d ex = new Vector3d();

    ex.sub(p2,p1);
    ex.normalize();
    Vector3d p3p1 = new Vector3d();
    p3p1.sub(p3,p1);
    double i = ex.dot(p3p1);
    Vector3d iex = new Vector3d();
    iex.scale(i,ex);
    Vector3d ey = new Vector3d(p3p1);
    ey.sub(iex);
    ey.normalize();
    Vector3d ez = new Vector3d();
    ez.cross(ex, ey);
    double d = p2.distance(p1);
    if(d - r1 < r2)
        System.out.println("d - r1 < r2");
    if(r2 < d + r1)
        System.out.println("r2 < d+r1");
    double j = ey.dot(p3p1);

    double x = (Math.pow(r1,2) - Math.pow(r2,2) + Math.pow(d,2))/(2*d);
    Vector3d exx = new Vector3d();
    exx.scale(x, ex);
    double y = ((Math.pow(r1,2) - Math.pow(r3,2) + Math.pow(i,2) + Math.pow(j,2)) / ((2*j)) - ((i/j)*x));
    Vector3d eyy = new Vector3d();
    eyy.scale(y, ey);

    double z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2);
    z1 = Math.sqrt(z1);
    double z2 = z1*-1;

    Vector3d ezz1 = new Vector3d();
    Vector3d ezz2 = new Vector3d();
    ezz1.scale(z1, ez);
    ezz2.scale(z2, ez);
    Point3d result1 = new Point3d();
    result1.add(p1);
    result1.add(exx);
    result1.add(eyy);
    result1.add(ezz1);

    Point3d result2 = new Point3d();
    result2.add(p1);
    result2.add(exx);
    result2.add(eyy);
    result2.add(ezz2);
    if((result1.distance(p4) - r4) <= (result2.distance(p4) - r4))
        return new double[]{round(result1.x), round(result1.y), round(result1.z)};
    else
        return new double[]{round(result2.x), round(result2.y), round(result2.z)};

}

Afterステップの後z1は常に負z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2)です。したがって、平方根を取ると になりNaNます。これは可能ですか？ローカリゼーションを行うために間違ったポイントを選択しているのかもしれません。

どこが間違っているのか教えてください。

4

1 に答える 1

1

の負の値z1は、実際の解がないことを示します。たとえば、1 番目と 2 番目の球の交点は、3 番目の球の内側または外側に完全に収まります。テスト値に実際の解が存在するセットが少なくとも 1 つ含まれていない場合、z1は常に負になります。

于 2014-05-15T08:46:00.970 に答える