Commit fc9b2288 authored by Sébastien Timmermans's avatar Sébastien Timmermans
Browse files

[ErrMsg] in mbs_set add some return errors

parent 85e4940d
/**
* @file mbs_set.c
*
*
* Creation date: 11/06/2015
* @author Olivier Lantsoght (based on the work of other from CEREM)
*
*
* (c) Universite catholique de Louvain
*/
#include "mbs_set.h"
#include <stdio.h>
#include <stdlib.h>
#include "useful_functions.h"
#include "mbs_message.h"
void mbs_set_nb_userc(MbsData *mbs_data, int Nuserc)
{
int i;
mbs_data->Nuserc = Nuserc;
mbs_data->Ncons = mbs_data->Nloopc + mbs_data->Nuserc;
mbs_data->nhu = mbs_data->Ncons;
// re-initialize hu vector
if (mbs_data->hu != NULL)
{
free(mbs_data->hu);
}
mbs_data->hu = (int*)malloc((mbs_data->nhu + 1) * sizeof(int));
mbs_data->hu[0] = mbs_data->nhu;
for (i = 1; i <= mbs_data->nhu; i++)
{
mbs_data->hu[i] = i;
}
}
void mbs_set_qu(MbsData *mbs_data, int qu)
{
if (qu > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qu: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qu=%d > mbs_data->njoint=%d\n", qu, mbs_data->njoint);
return;
}
// add element
mbs_data->nqu = add_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qu);
// remove element
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qu);
mbs_data->nqc = remove_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qu);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qu);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qu);
}
void mbs_set_qv(MbsData *mbs_data, int qv)
{
if (qv > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qv: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qv=%d > mbs_data->njoint=%d\n", qv, mbs_data->njoint);
return;
}
// add element
mbs_data->nqv = add_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qv);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qv);
mbs_data->nqc = remove_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qv);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qv);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qv);
}
void mbs_set_qdriven(MbsData *mbs_data, int qdriven)
{
if (qdriven > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qdriven: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qdriven=%d > mbs_data->njoint=%d\n", qdriven, mbs_data->njoint);
return;
}
// add element
mbs_data->nqc = add_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qdriven);
mbs_data->nqdriven = add_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qdriven);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qdriven);
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qdriven);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qdriven);
}
void mbs_set_qlocked(MbsData *mbs_data, int qlocked)
{
if (qlocked > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qlocked: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qlocked=%d > mbs_data->njoint=%d\n", qlocked, mbs_data->njoint);
return;
}
// add element
mbs_data->nqc = add_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qlocked);
mbs_data->nqlocked = add_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qlocked);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qlocked);
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qlocked);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qlocked);
}
void mbs_set_qa(MbsData *mbs_data, int qa)
{
if (qa > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qa: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qa=%d > mbs_data->njoint=%d\n", qa, mbs_data->njoint);
return;
}
mbs_data->nqa = add_mbs_q_elem(mbs_data->qa, mbs_data->nqa, qa);
}
void mbs_unset_qa(MbsData *mbs_data, int qa)
{
mbs_data->nqa = remove_mbs_q_elem(mbs_data->qa, mbs_data->nqa, qa);
}
void mbs_empty_qa(MbsData *mbs_data)
{
int i;
for (i = 1; i < mbs_data->njoint; i++) {
mbs_data->qa[i] = 0;
}
mbs_data->nqa = 0;
}
void udpate_nq(MbsData *mbs_data, int new_nqu, int new_nqv, int new_nqc, int new_nqdriven, int new_nqlocked)
{
mbs_data->nqu = new_nqu;
mbs_data->nqv = new_nqv;
mbs_data->nqc = new_nqc;
mbs_data->nqdriven = new_nqdriven;
mbs_data->nqlocked = new_nqlocked;
// check
if (mbs_data->nqdriven + mbs_data->nqlocked != mbs_data->nqc)
{
mbs_error_msg("in mbs_set::udpate_nq(): nqdriven (%d) + nqlocked (%d) != nqc (%d) !\n", mbs_data->nqdriven, mbs_data->nqlocked, mbs_data->nqc);
exit(1);
}
if (mbs_data->nqu + mbs_data->nqv + mbs_data->nqc != mbs_data->njoint)
{
mbs_error_msg("in mbs_set::udpate_nq(): nqu (%d) + nqv (%d) + nqc (%d) != njoint (%d) !\n", mbs_data->nqu, mbs_data->nqv, mbs_data->nqc, mbs_data->njoint);
exit(1);
}
}
int add_mbs_q_elem(int *q_vec, int nq, int new_q)
{
// safety vector should already been allocated
if (q_vec == NULL)
{
mbs_error_msg("in mbs_set::add_mbs_q_elem(): Vector not initialized, should be already initialized.\n");
exit(1);
}
// nothing yet in this vector
if (!nq)
{
q_vec[1] = new_q;
return 1;
}
// Look if new_q belongs to q_vec
if (find_ivec_1(q_vec, nq, new_q) == -1) {
// Case it was not found, we add it
q_vec[nq + 1] = new_q;
nq = nq + 1;
}
// We sort vector
sort_inplace_ivec_1(q_vec, nq);
return nq;
}
int remove_mbs_q_elem(int *q_vec, int nq, int old_q)
{
int index;
// safety vector should already been allocated
if (q_vec == NULL)
{
mbs_error_msg("in mbs_set::remove_mbs_q_elem(): Vector not initialized, should be already initialized.\n");
exit(1);
}
// nothing in the vector
if (!nq)
{
return nq;
}
// Look if old_q belongs to q_vec
index = find_ivec_1(q_vec, nq, old_q);
if (index != -1) {
// Case it was found, we remove it
q_vec[index] = q_vec[nq];
q_vec[nq] = 0;
nq = nq - 1;
}
// We sort vector
sort_inplace_ivec_1(q_vec, nq);
return nq;
}
void print_mbs_q_all(MbsData *mbs_data)
{
mbs_msg("qu -> ");
print_mbs_q_vec(mbs_data->qu, mbs_data->nqu);
mbs_msg("qv -> ");
print_mbs_q_vec(mbs_data->qv, mbs_data->nqv);
mbs_msg("qc -> ");
print_mbs_q_vec(mbs_data->qc, mbs_data->nqc);
mbs_msg("qdriven -> ");
print_mbs_q_vec(mbs_data->qdriven, mbs_data->nqdriven);
mbs_msg("qlocked -> ");
print_mbs_q_vec(mbs_data->qlocked, mbs_data->nqlocked);
}
void print_mbs_q_vec(int *q_vec, int nq)
{
int i;
if (nq)
{
mbs_msg("nb: %d - q: [%d]", nq, q_vec[0]);
for (i = 1; i <= nq; i++)
{
mbs_msg(" ; %d", q_vec[i]);
}
mbs_msg("\n");
}
else
{
mbs_msg("NULL\n");
}
}
/**
* @file mbs_set.c
*
*
* Creation date: 11/06/2015
* @author Olivier Lantsoght (based on the work of other from CEREM)
*
*
* (c) Universite catholique de Louvain
*/
#include "mbs_set.h"
#include <stdio.h>
#include <stdlib.h>
#include "useful_functions.h"
#include "mbs_message.h"
#include "mbs_errors_names.h"
void mbs_set_nb_userc(MbsData *mbs_data, int Nuserc)
{
int i;
mbs_data->Nuserc = Nuserc;
mbs_data->Ncons = mbs_data->Nloopc + mbs_data->Nuserc;
mbs_data->nhu = mbs_data->Ncons;
// re-initialize hu vector
if (mbs_data->hu != NULL)
{
free(mbs_data->hu);
}
mbs_data->hu = (int*)malloc((mbs_data->nhu + 1) * sizeof(int));
mbs_data->hu[0] = mbs_data->nhu;
for (i = 1; i <= mbs_data->nhu; i++)
{
mbs_data->hu[i] = i;
}
}
void mbs_set_qu(MbsData *mbs_data, int qu)
{
if (qu > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qu: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qu=%d > mbs_data->njoint=%d\n", qu, mbs_data->njoint);
return;
}
// add element
mbs_data->nqu = add_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qu);
// remove element
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qu);
mbs_data->nqc = remove_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qu);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qu);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qu);
if (mbs_data->nqu<0 || mbs_data->nqv < 0 || mbs_data->nqc < 0 || mbs_data->nqdriven < 0 || mbs_data->nqlocked < 0)
{
mbs_error_msg(" error during mbs_set_qu \n");
}
}
void mbs_set_qv(MbsData *mbs_data, int qv)
{
if (qv > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qv: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qv=%d > mbs_data->njoint=%d\n", qv, mbs_data->njoint);
return;
}
// add element
mbs_data->nqv = add_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qv);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qv);
mbs_data->nqc = remove_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qv);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qv);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qv);
if (mbs_data->nqu < 0 || mbs_data->nqv < 0 || mbs_data->nqc < 0 || mbs_data->nqdriven < 0 || mbs_data->nqlocked < 0)
{
mbs_error_msg(" error during mbs_set_qv \n");
}
}
void mbs_set_qdriven(MbsData *mbs_data, int qdriven)
{
if (qdriven > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qdriven: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qdriven=%d > mbs_data->njoint=%d\n", qdriven, mbs_data->njoint);
return;
}
// add element
mbs_data->nqc = add_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qdriven);
mbs_data->nqdriven = add_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qdriven);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qdriven);
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qdriven);
mbs_data->nqlocked = remove_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qdriven);
if (mbs_data->nqu < 0 || mbs_data->nqv < 0 || mbs_data->nqc < 0 || mbs_data->nqdriven < 0 || mbs_data->nqlocked < 0)
{
mbs_error_msg(" error during mbs_set_qdriven \n");
}
}
void mbs_set_qlocked(MbsData *mbs_data, int qlocked)
{
if (qlocked > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qlocked: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qlocked=%d > mbs_data->njoint=%d\n", qlocked, mbs_data->njoint);
return;
}
// add element
mbs_data->nqc = add_mbs_q_elem(mbs_data->qc, mbs_data->nqc, qlocked);
mbs_data->nqlocked = add_mbs_q_elem(mbs_data->qlocked, mbs_data->nqlocked, qlocked);
// remove element
mbs_data->nqu = remove_mbs_q_elem(mbs_data->qu, mbs_data->nqu, qlocked);
mbs_data->nqv = remove_mbs_q_elem(mbs_data->qv, mbs_data->nqv, qlocked);
mbs_data->nqdriven = remove_mbs_q_elem(mbs_data->qdriven, mbs_data->nqdriven, qlocked);
if (mbs_data->nqu < 0 || mbs_data->nqv < 0 || mbs_data->nqc < 0 || mbs_data->nqdriven < 0 || mbs_data->nqlocked < 0)
{
mbs_error_msg(" error during mbs_set_qlocked \n");
}
}
void mbs_set_qa(MbsData *mbs_data, int qa)
{
if (qa > mbs_data->njoint) {
mbs_warning_msg("mbs_set_qa: specified joint index is not in project dimension.\n");
mbs_warning_msg(" qa=%d > mbs_data->njoint=%d\n", qa, mbs_data->njoint);
return;
}
mbs_data->nqa = add_mbs_q_elem(mbs_data->qa, mbs_data->nqa, qa);
if (mbs_data->nqa < 0 )
{
mbs_error_msg(" error during mbs_set_qa \n");
}
}
void mbs_unset_qa(MbsData *mbs_data, int qa)
{
mbs_data->nqa = remove_mbs_q_elem(mbs_data->qa, mbs_data->nqa, qa);
}
void mbs_empty_qa(MbsData *mbs_data)
{
int i;
for (i = 1; i < mbs_data->njoint; i++) {
mbs_data->qa[i] = 0;
}
mbs_data->nqa = 0;
}
void udpate_nq(MbsData *mbs_data, int new_nqu, int new_nqv, int new_nqc, int new_nqdriven, int new_nqlocked)
{
mbs_data->nqu = new_nqu;
mbs_data->nqv = new_nqv;
mbs_data->nqc = new_nqc;
mbs_data->nqdriven = new_nqdriven;
mbs_data->nqlocked = new_nqlocked;
// check
if (mbs_data->nqdriven + mbs_data->nqlocked != mbs_data->nqc)
{
mbs_error_msg("in mbs_set::udpate_nq(): nqdriven (%d) + nqlocked (%d) != nqc (%d) !\n", mbs_data->nqdriven, mbs_data->nqlocked, mbs_data->nqc);
exit(EXIT_FAILURE);
}
if (mbs_data->nqu + mbs_data->nqv + mbs_data->nqc != mbs_data->njoint)
{
mbs_error_msg("in mbs_set::udpate_nq(): nqu (%d) + nqv (%d) + nqc (%d) != njoint (%d) !\n", mbs_data->nqu, mbs_data->nqv, mbs_data->nqc, mbs_data->njoint);
exit(EXIT_FAILURE);
}
}
int add_mbs_q_elem(int *q_vec, int nq, int new_q)
{
// safety vector should already been allocated
if (q_vec == NULL)
{
mbs_msg("in mbs_set::add_mbs_q_elem(): Vector not initialized, should be already initialized.\n");
return _MBS_ERR_LOW_FILES;
}
// nothing yet in this vector
if (!nq)
{
q_vec[1] = new_q;
return 1;
}
// Look if new_q belongs to q_vec
if (find_ivec_1(q_vec, nq, new_q) == -1) {
// Case it was not found, we add it
q_vec[nq + 1] = new_q;
nq = nq + 1;
}
// We sort vector
sort_inplace_ivec_1(q_vec, nq);
return nq;
}
int remove_mbs_q_elem(int *q_vec, int nq, int old_q)
{
int index;
// safety vector should already been allocated
if (q_vec == NULL)
{
mbs_msg("in mbs_set::remove_mbs_q_elem(): Vector not initialized, should be already initialized.\n");
return _MBS_ERR_LOW_FILES;
}
// nothing in the vector
if (!nq)
{
return nq;
}
// Look if old_q belongs to q_vec
index = find_ivec_1(q_vec, nq, old_q);
if (index != -1) {
// Case it was found, we remove it
q_vec[index] = q_vec[nq];
q_vec[nq] = 0;
nq = nq - 1;
}
// We sort vector
sort_inplace_ivec_1(q_vec, nq);
return nq;
}
void print_mbs_q_all(MbsData *mbs_data)
{
mbs_msg("qu -> ");
print_mbs_q_vec(mbs_data->qu, mbs_data->nqu);
mbs_msg("qv -> ");
print_mbs_q_vec(mbs_data->qv, mbs_data->nqv);
mbs_msg("qc -> ");
print_mbs_q_vec(mbs_data->qc, mbs_data->nqc);
mbs_msg("qdriven -> ");
print_mbs_q_vec(mbs_data->qdriven, mbs_data->nqdriven);
mbs_msg("qlocked -> ");
print_mbs_q_vec(mbs_data->qlocked, mbs_data->nqlocked);
}
void print_mbs_q_vec(int *q_vec, int nq)
{
int i;
if (nq)
{
mbs_msg("nb: %d - q: [%d]", nq, q_vec[0]);
for (i = 1; i <= nq; i++)
{
mbs_msg(" ; %d", q_vec[i]);
}
mbs_msg("\n");
}
else
{
mbs_msg("NULL\n");
}
}
/**
* @file mbs_set.h
*
*
* Creation date: 11/06/2015
* @author Olivier Lantsoght (based on the work of other from CEREM)
*
*
* (c) Universite catholique de Louvain
*/
#ifndef MBS_SET_h
#define MBS_SET_h
#include "mbs_data.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* Set the number of user constraints and update the related structures
*
* @param[in,out] mbs_data MbsData structure to modify
* @param[in] Nuserc number of user constraints
*
* This function must be called between mbs_load() and mbs_new_part() in 'main.c'
*/
void mbs_set_nb_userc(MbsData *mbs_data, int Nuserc);
/*! \brief set a variable to independent partition
*
* \param[in,out] mbs_data MbsData structure to modify
* \param[in] qu variable to set to independent
*
* This function must be called between mbs_load() and mbs_new_part() in 'main.c'
*/
void mbs_set_qu(MbsData *mbs_data, int qu);
/*! \brief set a variable to dependent partition
*
* \param[in,out] mbs_data MbsData structure to modify
* \param[in] qv variable to set to dependent
*
* This function must be called between mbs_load() and mbs_new_part() in 'main.c'
*/
void mbs_set_qv(MbsData *mbs_data, int qv);
/*! \brief set a variable to driven partition
*
* \param[in,out] mbs_data MbsData structure to modify
* \param[in] qdriven variable to set to driven
*
* This function must be called between mbs_load() and mbs_new_part() in 'main.c'
*/
void mbs_set_qdriven(MbsData *mbs_data, int qdriven);
/*! \brief set a variable to locked partition
*
* \param[in,out] mbs_data MbsData structure to modify
* \param[in] qlocked variable to set to locked
*
* This function must be called between mbs_load() and mbs_new_part() in 'main.c'
*/
void mbs_set_qlocked(MbsData *mbs_data, int qlocked);
/*! \brief set a variable to actuated partition
*
* \param[in,out] mbs_data MbsData structure to modify
* \param[in] qa variable to set to actuated
*
* This function must be called after mbs_load()
*/
void mbs_set_qa(MbsData *mbs_data, int qa);
/*! \brief unset a variable from the actuated partition
*