//
// CheetahBullet package.
//
// 20110512: first

var VERSION = "1.0b5";

var simFile_ext = "chdata";

var targetCount = 10;

var toRAD = Math.PI/180;
var toDEG = 180/Math.PI;

function buildUI(obj) {
    
    obj.setParameter('name','CheetahBullet Constraint');
    
    //obj.addParameterSelector("constraint type", ["point constraint", "hinge constraint", "cone constraint"], true, true);
    obj.addParameterSelector("constraint type", ["point constraint", "hinge constraint"], true, true);
    
    obj.addParameterLink("target a", false);
    obj.addParameterLink("target b", false);
    
    obj.addParameterSeparator("3D View Option");
    obj.addParameterFloat("constraint radius", 0.5, 0.001, 10000, true, false);
    obj.addParameterFloat("constraint length", 3, 0.001, 10000, true, false);
    
    obj.addParameterSeparator("Hinge Constraint Option");
    obj.addParameterFloat("hinge limit min.", 0, -360, 360, true, false);
    obj.addParameterFloat("hinge limit max.", 0, -360, 360, true, false);
    
    obj.addParameterSeparator("Cone Constraint Option");
    obj.addParameterFloat("cone angle x limit", 30, 0, 180, true, false);
    obj.addParameterFloat("cone angle z limit", 30, 0, 180, true, false);
    obj.addParameterFloat("cone twist limit", 360, 0, 360, true, false);
    obj.addParameterFloat("cone twist softness", 1, 0, 1, true, false);
    obj.addParameterFloat("cone twist bias", 0.3, 0, 1, true, false);
    obj.addParameterFloat("cone twist relax", 1, 0, 1, true, false);
}

function buildObject(obj) {
    
    //print('----');
    var type = obj.getParameter("constraint type");
    var targetA = obj.getParameter("target a");
    var targetB = obj.getParameter("target b");
    
    var mat = obj.objMatrix().inverse();
    
    //draw type
    switch( type ) {
        case 0:
            drawPoint( obj );
            break;
        case 1:
            drawHinge( obj );
            break;
        case 2:
            drawCone( obj );
            break;
    }
    //
    if (targetA) {
        var tpos = mat.multiply(targetA.getParameter("position"));
        drawArrowToPos( obj, tpos );
    }
    if (targetB) {
        var tpos = mat.multiply(targetB.getParameter("position"));
        drawArrowToPos( obj, tpos );
    }
}

function drawPoint( obj ) {
    var core = obj.core();
    
    var i, j;
    var lats = 8;
    var longs = 12;
    var radius = obj.getParameter("constraint radius");
    
    var x_p, y_p;
    var z_p, zr_p;
    for(i = 0;i <= lats;i++) {
        var lat = Math.PI * (-0.5 + (i)/lats);
        var z = Math.sin( lat );
        var zr = Math.cos( lat );
        
        for (j = 0;j <= longs;j++) {
            var lng = 2 * Math.PI * (j)/longs;
            var x = Math.cos( lng );
            var y = Math.sin( lng );
            
            if (i > 0 && j > 0) {
                
                if (i == 1) {
                
                } else if (i == lats) {
                
                } else {
                    core.addPolygon( 4, true, [
                                                  (new Vec3D( x * zr, y * zr, z )).multiply( radius )
                                                , (new Vec3D( x * zr_p, y * zr_p, z_p )).multiply( radius )
                                                , (new Vec3D( x_p * zr_p, y_p * zr_p, z_p )).multiply( radius )
                                                , (new Vec3D( x_p * zr, y_p * zr, z )).multiply( radius )
                                               ] );
                }
            }
            //
            x_p = x;
            y_p = y;
        }
        //
        z_p = z;
        zr_p = zr;
    }
}

function drawHinge( obj ) {
    var core = obj.core();
    
    var i;
    var longs = 12;
    
    var size = obj.getParameter("constraint length") / 2;
    var radius = obj.getParameter("constraint radius");
    
    var hingeMin = obj.getParameter("hinge limit min.");
    var hingeMax = obj.getParameter("hinge limit max.");
    
    var minMat = new Mat4D( ROTATE_HPB, 0, 0, hingeMin );
    var maxMat = new Mat4D( ROTATE_HPB, 0, 0, hingeMax );
    
    var front = [];
    var back = [];
    for(i = 0;i <= longs;i++) {
        var lng = 2 * Math.PI * (i)/longs;
        var x = Math.cos( lng )*radius;
        var y = Math.sin( lng )*radius;
        
        if (i > 0) {
            core.addPolygon( 4, true, [  ( new Vec3D( x  , y  ,  size ))
                                        ,( new Vec3D( x  , y  , -size ))
                                        ,( new Vec3D( x_p, y_p, -size ))
                                        ,( new Vec3D( x_p, y_p,  size ))  ] );
        }
        //
        x_p = x;
        y_p = y;
        front.push( new Vec3D( x, y,  size ) );
         back.push( new Vec3D( x, y, -size ) );
    }
    
    //
    var pwidth = 0.5;
    var pheight = 0.05;
    size -= 0.1;
    for (i = 0;i < 2;i++) {
        var mat = (i == 0)? minMat : maxMat;
        
        core.addPolygon( 4, true, [  mat.multiply( new Vec3D( -pwidth, pheight,  size ))
                                    ,mat.multiply( new Vec3D( -pwidth, pheight, -size ))
                                    ,mat.multiply( new Vec3D(  0, pheight, -size ))
                                    ,mat.multiply( new Vec3D(  0, pheight,  size ))
                                    ] );
        core.addPolygon( 4, true, [  mat.multiply( new Vec3D(  0, -pheight,  size ))
                                    ,mat.multiply( new Vec3D(  0, -pheight, -size ))
                                    ,mat.multiply( new Vec3D( -pwidth, -pheight, -size ))
                                    ,mat.multiply( new Vec3D( -pwidth, -pheight,  size ))
                                    ] );
        core.addPolygon( 4, true, [  mat.multiply( new Vec3D(  0,  pheight, size ))
                                    ,mat.multiply( new Vec3D(  0, -pheight, size ))
                                    ,mat.multiply( new Vec3D( -pwidth, -pheight, size ))
                                    ,mat.multiply( new Vec3D( -pwidth,  pheight, size ))
                                    ] );
        core.addPolygon( 4, true, [  mat.multiply( new Vec3D( -pwidth,  pheight, -size ))
                                    ,mat.multiply( new Vec3D( -pwidth, -pheight, -size ))
                                    ,mat.multiply( new Vec3D(  0, -pheight, -size ))
                                    ,mat.multiply( new Vec3D(  0,  pheight, -size ))
                                    ] );
        core.addPolygon( 4, true, [  mat.multiply( new Vec3D( -pwidth, -pheight, -size ))
                                    ,mat.multiply( new Vec3D( -pwidth,  pheight, -size ))
                                    ,mat.multiply( new Vec3D( -pwidth,  pheight,  size ))
                                    ,mat.multiply( new Vec3D( -pwidth, -pheight,  size ))
                                    ] );
        pwidth  += 0.1;
        pheight -= 0.02;
        size += 0.09;
    }
    core.addPolygon( front.length, true, front.reverse() );
    core.addPolygon(  back.length, true, back );
}

function drawCone( obj ) {
    var core = obj.core();
    
    var i;
    
    var size = obj.getParameter("constraint length");
    
    var angleX = obj.getParameter("cone angle x limit") * toRAD;
    var angleY = obj.getParameter("cone angle z limit") * toRAD;
    
    var v0 = core.addVertex( false, new Vec3D(0, 0, 0) );
    
    var v1 = core.addVertex( false, new Vec3D(Math.sin(angleX)*size, -Math.cos(angleX)*size, Math.sin(angleY)*size));
    var v2 = core.addVertex( false, new Vec3D(-Math.sin(angleX)*size, -Math.cos(angleX)*size, Math.sin(angleY)*size));
    var v3 = core.addVertex( false, new Vec3D(Math.sin(angleX)*size, -Math.cos(angleX)*size, -Math.sin(angleY)*size));
    var v4 = core.addVertex( false, new Vec3D(-Math.sin(angleX)*size,-Math.cos(angleX)*size, -Math.sin(angleY)*size));
    
    var v5 = core.addVertex( false, new Vec3D(Math.sin(angleX)*size, Math.cos(angleX)*size, Math.sin(angleY)*size));
    var v6 = core.addVertex( false, new Vec3D(-Math.sin(angleX)*size, Math.cos(angleX)*size, Math.sin(angleY)*size));
    var v7 = core.addVertex( false, new Vec3D(Math.sin(angleX)*size, Math.cos(angleX)*size, -Math.sin(angleY)*size));
    var v8 = core.addVertex( false, new Vec3D(-Math.sin(angleX)*size,Math.cos(angleX)*size, -Math.sin(angleY)*size));
    
    core.addIndexPolygon( 3, [v0, v1, v2] );
    core.addIndexPolygon( 3, [v4, v3, v0] );
    core.addIndexPolygon( 3, [v3, v1, v0] );
    core.addIndexPolygon( 3, [v0, v2, v4] );
    
    core.addIndexPolygon( 3, [v6, v5, v0] );
    core.addIndexPolygon( 3, [v0, v7, v8] );
    core.addIndexPolygon( 3, [v0, v5, v7] );
    core.addIndexPolygon( 3, [v8, v6, v0] );
}

function drawArrowToPos( obj, pos ) {
    var core = obj.core();
    
    var norm = pos.norm();
    
    var posVec = (norm > 0)?  pos.multiply( 1/norm ) : new Vec3D();
    
    var theta = Math.acos( posVec.y )*toDEG;
    var phi = Math.atan2( posVec.x, posVec.z)*toDEG;
    var mat = new Mat4D( ROTATE_HPB, phi, theta, 0 );
    
    var dist = Math.sqrt( pos.x*pos.x + pos.y*pos.y + pos.z*pos.z );
    
    var scale = 0.5 * obj.getParameter("constraint radius");
    var lats = 6;
    for (var i = 0;i < lats;i++) {
        var lat0 = 2 * Math.PI * (-0.5+(i-1)/lats);
        var lat1 = 2 * Math.PI * (-0.5+(i)/lats);
        core.addPolygon( 3, true,  [ mat.multiply( (new Vec3D( Math.cos( lat0 ), 0.5, Math.sin( lat0 )).multiply( scale ) )),
                                     mat.multiply( (new Vec3D( Math.cos( lat1 ), 0.5, Math.sin( lat1 )).multiply( scale ) )),
                                     mat.multiply( new Vec3D( 0, dist, 0) ) ]);
    }
    
}

function Vec3D_normalize( vec ) {
    if (vec.norm() == 0) return (new Vec3D());
    return vec.multiply( 1/vec.norm() );
}

function Vec3D_toString( vec ) {
	return vec.x.toFixed(3) + ', ' + vec.y.toFixed(3) + ', ' + vec.z.toFixed(3);
}

function Mat4D_toString( mat ) {
    var str = '(' + mat.m00.toFixed(3) + ', ' + mat.m01.toFixed(3) + ', ' + mat.m02.toFixed(3) + ', ' + mat.m03.toFixed(3) + "\n";
    str += ' ' + mat.m10.toFixed(3) + ', ' + mat.m11.toFixed(3) + ', ' + mat.m12.toFixed(3) + ', ' + mat.m13.toFixed(3) + "\n";
    str += ' ' + mat.m20.toFixed(3) + ', ' + mat.m21.toFixed(3) + ', ' + mat.m22.toFixed(3) + ', ' + mat.m23.toFixed(3) + "\n";
    str += ' ' + mat.m30.toFixed(3) + ', ' + mat.m31.toFixed(3) + ', ' + mat.m32.toFixed(3) + ', ' + mat.m33.toFixed(3) + ")\n";
    return str; 
}

function printElapsedTime(label, time) {
	if (time/1000 > 60) {
		print( label + ':' + (time/1000).toFixed(2) + 'sec(s) ( ' + Math.floor(time/1000/60) + '.' + Math.floor((time/1000)%60) + ' )');
	} else {
		print( label + ':' + (time/1000).toFixed(2) + 'sec(s)' );
	}
}
