demoOG.tcl

proc demoOGRandom {{sizeVoxel 1.0} {seed 1} {sparseTh 0.8}} {
    global pos robots

    set box [geom::new_Boxd]
    jmath::setValue [$box cget -x] "(-10, -10, -10)" 
    jmath::setValue [$box cget -size] "(20, 20, 20)"
    puts "box: [geom::print $box]"

    set denseOG [geom::new_DenseOG $box $sizeVoxel]
    $denseOG randomize $seed
    
    set sparseOG [geom::new_SparseOG $denseOG $sparseTh]

    package require Gdhe

    # create the display list
    $sparseOG glInit

    set robots(og) "$sparseOG glDraw"
    set pos(og) "0 0 0 0 0 0"

    set robots(repere) "color 0 255 0 ; repere 20"
    set pos(repere) "0 0 0 0 0 0"
    
}

proc demoOGStereo {{disparity "disparity_04.xml"} {sizeVoxel 0.05}} {
    global pos robots

    # OG
    package require geom
    set box [geom::new_Boxd]
    jmath::setValue [$box cget -x] "(0, -10, -1)" 
    jmath::setValue [$box cget -size] "(10, 20, 3)"
    puts "box: [geom::print $box]"

    set denseOG [geom::new_DenseOG $box $sizeVoxel]

    # sensor
    package require image
    package require stereo
    set sb [image::new_StereoBench]
    $sb load "stereo.cal"
    $sb applyScale 0.5
    puts "stereo: [image::print $sb]"
    set refToSensor [geom::new_T3DEuler]
    $refToSensor load "refToSensor.t3d"
    set stereoOGS [stereo::new_StereoOGSensorModel $sb 1.0]
    $stereoOGS setRefToSensor $refToSensor

    $stereoOGS setDisparity $disparity

    $denseOG update $stereoOGS

    # 3D display
    package require Gdhe

    # create the display list
    $denseOG glInit

    set robots(og) "$denseOG glDraw"
    set pos(og) "0 0 0 0 0 0"

    set robots(repere) "color 0 255 0 ; repere 2"
    set pos(repere) "0 0 0 0 0 0"

    set x [$refToSensor getX]
    set robots(sensor) "color 255 0 0 ; repere 0.5"
    set pos(sensor) "[jmath::radToDeg [$x get 3]] [jmath::radToDeg [$x get 4]] [jmath::radToDeg [$x get 5]] [$x get 0] [$x get 1] [$x get 2]"

    set robots(grille) "color 255 255 0 ; grille -20 -20 20 20 1"
    set pos(grille) "0 0 0 0 0 0"

}

proc refToSensor {} {
    set t1 [::geom::new_T3DEuler ]
    ::geom::T3DEuler_setValue $t1 0.244 0 0.946 0 0 0
    set t2 [::geom::new_T3DEuler ]
    ::geom::T3DEuler_setValue $t2 0 0 0 0 [jmath::degToRad 30] 0
    set t3 [::geom::new_T3DEuler ]
    ::geom::T3DEuler_setValue $t3 0 0.165 0.081 0 0 0
    set t4 [::geom::new_T3DEuler ]
    ::geom::T3DEuler_setValue $t4 0 0 0 [jmath::degToRad -90] 0 [jmath::degToRad -90]
    
    set refToSensor [::geom::new_T3DEuler $t1]
    ::geom::T3D_composeIncr $refToSensor $t2
    ::geom::T3D_composeIncr $refToSensor $t3
    ::geom::T3D_composeIncr $refToSensor $t4

    $refToSensor save "refToSensor.t3d"
}