1

で壁追従ステアリング動作を実装しようとしていますC#。そのために、点と線分があります。必要なのは、perpendicular point C線分CDが に垂直になるようにすることだけABです。http://puu.sh/1xrxQこれがシナリオです。点が動くので毎回計算する必要があります。私はC#が初めてなので、それがどのように機能するのかわかりません。

これは、私が今までそれを機能させるために試みてきたことです。

private double DotProduct(Point pointA, Point pointB, Point pointC)
{
    Point AB = new Point();
    Point BC = new Point();
    AB.X = pointB.X - pointA.X;
    AB.Y = pointB.Y - pointA.Y;
    BC.X = pointC.X - pointB.X;
    BC.Y = pointC.Y - pointB.Y;
    double dot = AB.X * BC.X + AB.Y * BC.Y;

    return dot;
}

//Compute the cross product AB x AC
private double CrossProduct(Point pointA, Point pointB, Point pointC)
{
    Point AB = new Point();
    Point AC = new Point();
    AB.X = pointB.X - pointA.X;
    AB.Y = pointB.Y - pointA.Y;
    AC.X = pointC.X - pointA.X;
    AC.Y = pointC.Y - pointA.Y;
    double cross = AB.X * AC.Y - AB.Y * AC.X;
    return cross;
}

//Compute the distance from A to B
double Distance(Point pointA, Point pointB)
{
    double d1 = pointA.X - pointB.X;
    double d2 = pointA.Y - pointB.Y;

    return Math.Sqrt(d1 * d1 + d2 * d2);
}

//Compute the distance from AB to C
//if isSegment is true, AB is a segment, not a line.
double LineToPointDistance2D(Point pointA, Point pointB, Point pointC, bool isSegment)
{
    double dist = CrossProduct(pointA, pointB, pointC) / Distance(pointA, pointB);
    if (isSegment)
    {
        double dot1 = DotProduct(pointA, pointB, pointC);
        if (dot1 > 0)
            return Distance(pointB, pointC);

        double dot2 = DotProduct(pointB, pointA, pointC);
        if (dot2 > 0)
            return Distance(pointA, pointC);

     }
     return Math.Abs(dist);
}
4

1 に答える 1

1

テストされていませんが、数学的にはこれでうまくいくはずです

Point intersectionPoint(Point A, Point B, Point C) {
    //check for slope of 0 or undefined
    if (A.Y == B.Y) return new Point (C.X, A.Y);
    if (A.X == B.X) return new Point (A.X, C.Y);
    //slope = (y1 - y2) / (x1 - x2)
    double slopeAB = (A.Y - B.Y) / (A.X - B.X);
    //perpendicular slope is the negative reciprocal
    double slopeCD = -1 / slopeAB;
    //solve for b in y = mx + b of each line
    //  b = y - mx
    double bAB = A.Y - slopeAB * A.X;
    double bCD = C.Y - slopeCD * C.X;
    double dX, dY;
    //intersection of two lines is m1x + b1 = m2x + b2
    //solve for x: x = (b2 - b1) / (m1 - m2)
    //find y by plugging x into one of the equations
    dX = (bCD - bAB) / (slopeAB - slopeCD);
    dY = slopeAB * dX + bAB;
    return new Point(dX, dY);
}
于 2012-12-05T18:57:51.290 に答える