座標の回転

[みけCAT]

なぜか座標を回転するサンプルを作ってみました。
参考になれば幸いです。
添付したサンプルの計算部分のみ抽出して掲載しておきます。
ox,oy：結果を代入するポインタ
cx,cy：回転の中心の座標
mx,my：回転される座標
angle：回転角度（ラジアン、時計回り）

void kaiten(double* ox,double* oy,
        double cx,double cy,double mx,double my,double angle) {
    double length;/*斜辺の長さ*/
    double sinmoto,cosmoto;/*元のsin,cos*/
    double sinsaki,cossaki;/*回転後のsin,cos*/
    length=sqrt((mx-cx)*(mx-cx)+(cy-my)*(cy-my));
    if(length==0) {
        *ox=mx;
        *oy=my;
        return;
    }
    sinmoto=(cy-my)/length;
    cosmoto=(mx-cx)/length;
    /*加法定理*/
    sinsaki=sinmoto*cos(angle)+cosmoto*sin(angle);
    cossaki=cosmoto*cos(angle)-sinmoto*sin(angle);
    *ox=cx+length*cossaki;
    *oy=cy-length*sinsaki;
}

/*ニュートン法*/
double sqrt(double in) {
    double x=2.0;
    double nextx=1.0;
    while((x>=nextx?1:-1)*(x-nextx)>0.0000000001) {
        x=nextx;
        nextx=x-(x*x-in)/(2*x);
    }
    return nextx;
}

/*
高校生のための マクローリン展開（１）
http://bit.ly/9Z2PNX
より
*/
double cos(double angle) {
    double result=1.0;
    double temp;
    int i,j;
    double fugou=-1;
    for(i=2;i<=30;i+=2) {
        temp=fugou;
        for(j=0;j<i;j++)temp*=angle;
        for(j=1;j<=i;j++)temp/=j;
        result+=temp;
        fugou=-fugou;
    }
    return result;
}

/*
高校生のための マクローリン展開（２）
http://bit.ly/bfVyXy
より 
*/
double sin(double angle) {
    double result;
    double temp;
    int i,j;
    double fugou=-1;
    result=angle;
    for(i=3;i<=31;i+=2) {
        temp=fugou;
        for(j=0;j<i;j++)temp*=angle;
        for(j=1;j<=i;j++)temp/=j;
        result+=temp;
        fugou=-fugou;
    }
    return result;
}