scenegraph.h

Go to the documentation of this file.

/* -*- mode: C; c-basic-offset: 4; -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2015, Rice University
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@rice.edu>
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of copyright holder the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef AMINO_SCENEGRAPH_H
#define AMINO_SCENEGRAPH_H

#include "amino/mat.h"
#include "rxtype.h"

typedef signed long aa_rx_frame_id;

typedef signed long aa_rx_config_id;

#define AA_RX_FRAME_ROOT ((aa_rx_frame_id)-1)

#define AA_RX_FRAME_NONE ((aa_rx_frame_id)-2)

#define AA_RX_CONFIG_NONE ((aa_rx_config_id)-1)

#define AA_RX_CONFIG_MULTI ((aa_rx_config_id)-2)

enum aa_rx_frame_type {
    AA_RX_FRAME_FIXED,       
    AA_RX_FRAME_REVOLUTE,    
    AA_RX_FRAME_PRISMATIC,   
};

AA_API struct aa_rx_sg *
aa_rx_sg_create();

AA_API void aa_rx_sg_destroy(struct aa_rx_sg *scene_graph);

AA_API int aa_rx_sg_init ( struct aa_rx_sg *scene_graph );

AA_API enum aa_rx_frame_type
aa_rx_sg_frame_type (
    const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );

AA_API const char *
aa_rx_sg_frame_name (
    const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );

AA_API const char *
aa_rx_sg_config_name (
    const struct aa_rx_sg *scene_graph, aa_rx_config_id config_id );

AA_API aa_rx_frame_id
aa_rx_sg_frame_parent (
    const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );


AA_API size_t
aa_rx_sg_frame_count (
    const struct aa_rx_sg *scene_graph );

AA_API size_t
aa_rx_sg_config_count (
    const struct aa_rx_sg *scene_graph );

AA_API aa_rx_config_id
aa_rx_sg_frame_config (
    const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame);

AA_API size_t
aa_rx_sg_config_names (
    const struct aa_rx_sg *scene_graph, size_t n_names,
    const char **names );

AA_API aa_rx_config_id aa_rx_sg_config_id(
    const struct aa_rx_sg *scene_graph, const char *config_name);

AA_API void
aa_rx_sg_config_indices(
    const struct aa_rx_sg *scene_graph, size_t n,
    const char **config_name, aa_rx_config_id *ids );


AA_API void
aa_rx_sg_config_get(
    const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset,
    const aa_rx_config_id *ids,
    const double *config_all,
    double *config_subset );

AA_API void
aa_rx_sg_config_set(
    const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset,
    const aa_rx_config_id *ids, const double *config_subset,
    double *config_all
    );


AA_API aa_rx_frame_id aa_rx_sg_frame_id (
    const struct aa_rx_sg *scene_graph, const char *frame_name);


/* /\** */
/*  *  Return the index of a configuration variable for the given frame. */
/*  *\/ */
/* AA_API aa_rx_config_id aa_rx_sg_frame_config_id( */
/*     struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id); */

AA_API void aa_rx_sg_add_frame_fixed
( struct aa_rx_sg *scene_graph,
  const char *parent, const char *name,
  const double q[4], const double v[3] );

AA_API void aa_rx_sg_add_frame_prismatic
( struct aa_rx_sg *scene_graph,
  const char *parent, const char *name,
  const double q[4], const double v[3],
  const char *config_name,
  const double axis[3], double offset );

AA_API void aa_rx_sg_add_frame_revolute
( struct aa_rx_sg *scene_graph,
  const char *parent, const char *name,
  const double q[4], const double v[3],
  const char *config_name,
  const double axis[3], double offset );

AA_API void aa_rx_sg_rm_frame
( struct aa_rx_sg *scene_graph,
  const char *name );

AA_API void
aa_rx_sg_set_limit_pos( struct aa_rx_sg *scenegraph,
                        const char *config_name,
                        double min, double max );

AA_API void
aa_rx_sg_set_limit_vel( struct aa_rx_sg *scenegraph,
                        const char *config_name,
                        double min, double max );

AA_API void
aa_rx_sg_set_limit_acc( struct aa_rx_sg *scenegraph,
                        const char *config_name,
                        double min, double max );

AA_API void
aa_rx_sg_set_limit_eff( struct aa_rx_sg *scenegraph,
                        const char *config_name,
                        double min, double max );
AA_API int
aa_rx_sg_get_limit_pos( const struct aa_rx_sg *scenegraph,
                        aa_rx_config_id config_id,
                        double *min, double *max );

AA_API int
aa_rx_sg_get_limit_vel( const struct aa_rx_sg *scenegraph,
                        aa_rx_config_id config_id,
                        double *min, double *max );

AA_API int
aa_rx_sg_get_limit_acc( const struct aa_rx_sg *scenegraph,
                        aa_rx_config_id config_id,
                        double *min, double *max );

AA_API int
aa_rx_sg_get_limit_eff( const struct aa_rx_sg *scenegraph,
                        aa_rx_config_id config_id,
                        double *min, double *max );


AA_API double
aa_rx_sg_config_center( const struct aa_rx_sg *sg, aa_rx_config_id config_id );

AA_API void
aa_rx_sg_center_configs( const struct aa_rx_sg *sg,
                             size_t n, double *q );

AA_API const double *aa_rx_sg_frame_axis
( const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame );


/* AA_API void */
/* aa_rx_sg_tf_alloc( const struct aa_rx_sg *scene_graph, */
/*                    struct aa_mem_region *reg, */
/*                    double **TF_rel, size_t *ld_rel, */
/*                    double **TF_abs, size_t *ld_abs); */

/* AA_API void */
/* aa_rx_sg_tf_pop( const struct aa_rx_sg *scene_graph, */
/*                  struct aa_mem_region *reg, */
/*                  double *TF_rel, double *TF_abs ); */


/* #define AA_RX_SG_TF_ALLOC( SCENE_GRAPH, REG, TF_REL, LD_REL, TF_ABS, LD_ABS ) \ */
/*     double *TF_REL, *TF_ABS;                                            \ */
/*     size_t LD_REL, LD_ABS;                                              \ */
/*     aa_rx_sg_tf_alloc( SCENE_GRAPH, REG, &TF_REL, &LD_REL, &TF_ABS, &LD_ABS ); */

/* #define AA_RX_SG_TF_COUNT_GET( SCENE_GRAPH, REG, N_Q, Q, N_TF, TF_REL, LD_REL, TF_ABS, LD_ABS ) \ */
/*     double *TF_REL, *TF_ABS;                                            \ */
/*     size_t LD_REL, LD_ABS;                                              \ */
/*     size_t N_TF = aa_rx_sg_frame_count(SCENE_GRAPH);                    \ */
/*     aa_rx_sg_tf_alloc( SCENE_GRAPH, REG, &TF_REL, &LD_REL, &TF_ABS, &LD_ABS ); \ */
/*     aa_rx_sg_tf(SCENE_GRAPH, N_Q, Q, N_TF,                              \ */
/*                 TF_REL, LD_REL,                                         \ */
/*                 TF_ABS, LD_ABS );       */                                 \



AA_API void aa_rx_sg_tf
( const struct aa_rx_sg *scene_graph,
  size_t n_q, const double *q,
  size_t n_tf,
  double *TF_rel, size_t ld_rel,
  double *TF_abs, size_t ld_abs );


AA_API struct aa_dmat *
aa_rx_sg_get_tf_abs( const struct aa_rx_sg *scene_graph,
                         struct aa_mem_region *reg,
                         const struct aa_dvec *q );

AA_API void
aa_rx_sg_fill_tf_abs( const struct aa_rx_sg *scene_graph,
                      const struct aa_dvec *q,
                      struct aa_dmat *tf_abs );

AA_API void aa_rx_sg_tfmat
( const struct aa_rx_sg *scene_graph,
  size_t n_q, const double *q,
  size_t n_tf,
  double *TF_rel, size_t ld_rel,
  double *TF_abs, size_t ld_abs );


AA_API void aa_rx_sg_duqu
( const struct aa_rx_sg *scene_graph,
  size_t n_q, const double *q,
  size_t n_tf,
  double *TF_rel, size_t ld_rel,
  double *TF_abs, size_t ld_abs );

AA_API void aa_rx_sg_tf_update
( const struct aa_rx_sg *scene_graph,
  size_t n_q,
  const double *q0,
  const double *q,
  size_t n_tf,
  const double *TF_rel0, size_t ld_rel0,
  const double *TF_abs0, size_t ld_abs0,
  double *TF_rel, size_t ld_rel,
  double *TF_abs, size_t ld_abs );



AA_API void aa_rx_sg_map_geom (
    const struct aa_rx_sg *scene_graph,
    void (*function)(void *context, aa_rx_frame_id frame_id, struct aa_rx_geom *geom),
    void *context );

AA_API void aa_rx_sg_rel_tf (
  const struct aa_rx_sg *scene_graph,
  const aa_rx_frame_id frame_from,
  const aa_rx_frame_id frame_to,
  const double * tf_abs,
  size_t ld_abs,
  double * from_tf_to);

AA_API void aa_rx_sg_reparent_id ( const struct aa_rx_sg *scene_graph,
                                   aa_rx_frame_id new_parent,
                                   aa_rx_frame_id frame,
                                   const double *E1);

AA_API void aa_rx_sg_reparent_name ( const struct aa_rx_sg *scene_graph,
                                     const char *new_parent,
                                     const char *frame,
                                     const double *E1);

AA_API  struct aa_rx_sg *  aa_rx_sg_copy( const struct aa_rx_sg * orig);

AA_API void aa_rx_sg_allow_collision( struct aa_rx_sg *scene_graph,
                                      aa_rx_frame_id id0, aa_rx_frame_id id1, int allowed );

AA_API void aa_rx_sg_allow_collision_name( struct aa_rx_sg *scene_graph,
                                           const char* frame0, const char* frame1, int allowed );



AA_API double *
aa_rx_sg_alloc_tf ( const struct aa_rx_sg *sg, struct aa_mem_region *region );

AA_API double *
aa_rx_sg_alloc_config ( const struct aa_rx_sg *sg, struct aa_mem_region *region );

#endif /*AMINO_SCENEGRAPH_H*/