三维空间内两相交直线内切圆

 voidMoveDataCircle::initWithStraight(MoveDataBase* straight1,MoveDataBase* straight2)

{

    

    C3DVector3 v1 = ((MoveDataStraight*)straight1)->m_startPosition;

    C3DVector3  v2 = ((MoveDataStraight*)straight1)->m_endPosition;

    C3DVector3  v3 = ((MoveDataStraight*)straight2)->m_startPosition;

    C3DVector3  v4 = ((MoveDataStraight*)straight2)->m_endPosition;

    float distance = 24;//假设平滑过渡得距离

    

    Vec3 dirL1 =  Vec3((v2.x-v1.x),(v2.y-v1.y),(v2.z-v1.z));

    dirL1.normalize();

    //切点坐标1

    Vec3  P1 = Vec3(v2.x,v2.y,v2.z) + distance*dirL1*(-1);

    

    

    Vec3 dirL2 =  Vec3((v4.x-v3.x),(v4.y-v3.y),(v4.z-v3.z));

    dirL2.normalize();

    //切点坐标2

    Vec3  P2 = Vec3(v3.x,v3.y,v3.z) + distance*dirL2;

    

    //把切点付给圆弧起始点

    v2 = C3DVector3(P1.x,P1.y,P1.z);

    ((MoveDataStraight*)straight1)->m_endPosition = v2;

    v3 = C3DVector3(P2.x,P2.y,P2.z);

    ((MoveDataStraight*)straight2)->m_startPosition = v3;

    ((MoveDataStraight*)straight2)->m_initPos = ((MoveDataStraight*)straight2)->m_startPosition;

    //计算圆心等参数

    initWithPoint4(v1,v2,v3,v4);

}

voidMoveDataCircle::initWithPoint4(C3DVector3 v1,C3DVector3  v2,C3DVector3  v3,C3DVector3  v4)

{

    

    //直线方程 L1;直线1  v1,v2

    //向量

    Vec3 dirL1 =  Vec3((v2.x-v1.x),(v2.y-v1.y),(v2.z-v1.z));

    dirL1.normalize();

    //垂直与此直线的平面方程

    //    x2*dirL1.x+y2*dirL1.y+z2*dirL1.z+d=0;

    float A1,B1,C1,D1;

    D1 = (v2.x*dirL1.x+v2.y*dirL1.y+v2.z*dirL1.z);

    A1 = dirL1.x;

    B1 = dirL1.y;

    C1= dirL1.z;

    //      x*dirL1.x+y*dirL1.y+z*dirL1.z+d=0;

    

    //直线方程 L2;直线2 v3,v4

    //向量

    Vec3 dirL2 =  Vec3((v4.x-v3.x),(v4.y-v3.y),(v4.z-v3.z));

    dirL2.normalize();

    //垂直与此直线的平面方程

     float A2,B2,C2,D2;

//    x3*dirL2.x+y3*dirL2.y+z3*dirL2.z+d=0;

     D2 = (v3.x*dirL2.x+v3.y*dirL2.y+v3.z*dirL2.z);

     A2 = dirL2.x;

     B2 = dirL2.y;

     C2 = dirL2.z;

//    x*dirL2.x+y*dirL2.y+z*dirL2.z+d=0;

    float A3,B3,C3,D3;

    //平面方向

    Vec3 dirL3;

    Vec3 dir1 = dirL1 - dirL2;

    Vec3 dir2 = dirL1 + dirL2;

    dirL3 = Vec3(dir1.y*dir2.z-dir1.z*dir2.y,dir2.x*dir1.z-dir1.x*dir2.z,dir1.x*dir2.y-dir1.y*dir2.x);

    

//    向量AB×向量CD＝（y1z2-z1y2，x2z1-x1z2，x1y2-y1x2）

    

    

    dirL3.normalize();

    A3 = dirL3.x;

    B3 = dirL3.y;

    C3 = dirL3.z;

    //平面方程  (点法式)

    D3 = (A3*v1.x+B3*v1.y+C3*v1.z);

    Vec3 direction = Vec3(A3,B3,C3);

    

    

    //联立平面方程

    

    //解方程得圆心  行列式算法

   

//    Vec3 center = getXYZ(Vec3(A1,B1,C1),Vec3(A2,B2,C2),Vec3(A3,B3,C3),Vec3 (D1,D2,D3));

    

    

    Vec3 center ;

    float m=fx(A1,B1,C1,A2,B2,C2,A3,B3,C3);

    if(m==0)

    {

        center = Vec3(0, 0, 0);

    }else

    {

        center.x=fx(D1,B1,C1,D2,B2,C2,D3,B3,C3)/(float)m;

        center.y=fx(A1,D1,C1,A2,D2,C2,A3,D3,C3)/(float)m;

        center.z=fx(A1,B1,D1,A2,B2,D2,A3,B3,D3)/(float)m;

    }

    

    //半径

    float radius = center.distance(Vec3(v2.x,v2.y,v2.z));

    

    // 两直线夹角

    float cosθ=judge((A1*A2+B1*B2+C1*C2),(sqrt(A1*A1+B1*B1+C1*C1)*sqrt(A2*A2+B2*B2+C2*C2)));

    float θ= CC_RADIANS_TO_DEGREES (acos(cosθ));

    //圆的弧度

    

    //计算初始角度

    if (direction.x==0&&direction.y==0) {

        direction.x = 0.001; //杜绝x、y同时为0 在路径时设定0.01的误差

    }

    float a3 = radius*sqrt(direction.x*direction.x + direction.y*direction.y)/sqrt(direction.x*direction.x + direction.y*direction.y+direction.z*direction.z);

    //a3*sin = center.z - v2.z;

    float sin= (center.z - v2.z)/a3;

    if (sin>1) {

        sin = 1.0;

    }

    if (sin<-1) {

        sin = -1.0;

    }

    float cos= sqrt(1-sin*sin);

    float beginAngle = atan2(sin,cos);

    

    Vec3 begin =  getaAnglePoint(center,direction,beginAngle,radius);

    C3DVector3 begin_ = C3DVector3(begin.x,begin.y,begin.z);

    if (begin_.distance(v2)>2.0) {

        beginAngle = atan2(sin,-cos);

        begin =  getaAnglePoint(center,direction,beginAngle,radius);

        begin_ = C3DVector3(begin.x,begin.y,begin.z);

        if (begin_.distance(v2)>2.0) {

        

            CCLOG("fuck! begin\n");

        }

    }

    

     //判断顺时针逆时针

    bool clockWise = false;  //假设是顺时针

    float factor = 1.0f;

    float usingAngle = θ * M_PI*2 /360.0f;

    float angle = (beginAngle+factor*usingAngle);

 

    //得到终点坐标

    C3DVector3 end;

    

    Vec3 end_= getaAnglePoint(center,direction,angle,radius);

    end = C3DVector3(end_.x,end_.y,end_.z);

    if (end.distance(v3)>2.0) {

        clockWise = true; //假设不成立

    }

  direction.z *= -1;

    

    

    //赋值

    m_centerPosition = C3DVector3(center.x,center.y,center.z);

    m_nDirection = C3DVector3(direction.x,direction.y,direction.z);

    m_radius = radius;

    m_time = 1.0;

    m_moveStyle = 0;

    m_totalAngle = θ;

    m_beginAngle = beginAngle;

    m_clockWise = clockWise;

    m_bLoop = false;

    m_initPos = v2;

}

Vec3  MoveDataCircle::getaAnglePoint(Vec3 center ,Vec3 direction, float angle,float radius)

{

    float cosAngle = cosf (angle);

    float sinAngle = sinf (angle);

    Vec3 begin;

    

    begin.x = center.x + radius*direction.y/sqrt(direction.x*direction.x + direction.y*direction.y)*cosAngle+radius*direction.x*direction.z/(sqrt(direction.x*direction.x+direction.y*direction.y)*sqrt(direction.x*direction.x+direction.y*direction.y+direction.z*direction.z))*sinAngle;

    begin.y = center.y - radius*direction.x/sqrt(direction.x*direction.x + direction.y*direction.y)*cosAngle+radius*direction.y*direction.z/(sqrt(direction.x*direction.x+direction.y*direction.y)*sqrt(direction.x*direction.x+direction.y*direction.y+direction.z*direction.z))*sinAngle;

    begin.z = center.z - radius*sqrt(direction.x*direction.x + direction.y*direction.y)/sqrt(direction.x*direction.x + direction.y*direction.y+direction.z*direction.z)*sinAngle;

    return begin;

}

floatMoveDataCircle::fx(float a,float b,float c,

       float d,float e,float f,

       float g,float h,float i)

{float result;

    result=a*e*i+b*f*g+c*d*h-a*f*h-b*d*i-c*e*g;

    return result;

}

floatMoveDataCircle::judge(float numerator ,float denominator)

{

    if (denominator==0) {

        return0;

    }

    return numerator/denominator;

}