直線同士の交点を求める

//
// 直線の交点を求める関数（直線はp0-p1、p2-p3のペア）
//
vector CrossPointXZ(vector p0; vector p1; vector p2; vector p3)
{
    // 線分が交差していた場合、直線の交点座標を計算する
    float a1 = p0.z - p1.z;
    float b1 = p1.x - p0.x;
    float c1 = (p1.x - p0.x) * p0.z - (p1.z - p0.z) * p0.x;
     
    float a2 = p2.z - p3.z;
    float b2 = p3.x - p2.x;
    float c2 = (p3.x - p2.x) * p2.z - (p3.z - p2.z) * p2.x;
     
    float x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
    float z = (a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1);

    return set(x, 0, z);
}

//
// 直線の交点を求める関数
//
vector CrossPointXY(vector p0; vector p1; vector p2; vector p3)
{
    // 線分が交差していた場合、直線の交点座標を計算する
    float a1 = p0.y - p1.y;
    float b1 = p1.x - p0.x;
    float c1 = (p1.x - p0.x) * p0.y - (p1.y - p0.y) * p0.x;
     
    float a2 = p2.y - p3.y;
    float b2 = p3.x - p2.x;
    float c2 = (p3.x - p2.x) * p2.y - (p3.y - p2.y) * p2.x;
     
    float x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
    float y = (a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1);

    return set(x, y, 0);
}