void MotionSequenceController::setRootOffset(Vector3D& _offset_position, Vector3D& _offset_orientation, long _offset_start_frame)
{
apply_start_offset = true;
start_offset_frame = _offset_start_frame;
Vector3D orig_start_position(
motion_sequence->getValue(CHANNEL_ID(0,CT_TX),start_offset_frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_TY),start_offset_frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_TZ),start_offset_frame));
Vector3D orig_start_orientation(
motion_sequence->getValue(CHANNEL_ID(0,CT_RX),start_offset_frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_RY),start_offset_frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_RZ),start_offset_frame));
// compute root translation as position difference
start_offset_translation = _offset_position - orig_start_position;
Matrix4x4 so_translation_transform = Matrix4x4::translationXYZ(start_offset_translation);
// compute root rotation as orientation difference (subtracting Euler angles)
// this is not a valid difference, but it will work temporarily, since
// we're primarily dealing with a 2D rotation around y axis (yaw/heading)
start_offset_rotation = Vector3D(0.0f, _offset_orientation.yaw - orig_start_orientation.yaw, 0.0f);
start_offset_rotation_transform = Matrix4x4::rotationRPY(start_offset_rotation);
// combine the two root offset transforms
start_offset_combined_transform = so_translation_transform * start_offset_rotation_transform;
// initialize the root cache
if (frame_is_cached != NULL) delete [] frame_is_cached;
int num_frames = motion_sequence->numFrames();
frame_is_cached = new bool[num_frames];
memset(frame_is_cached, 0, num_frames);
root_cache.resize(num_frames,6);
}
static void do_conf()
{
char c = 0;
char line[64];
int args, id, num, gain, min, max;
struct chan *channel;
gets(line);
args = sscanf(line, "%c %u %u %i %i", &c, &num, &gain, &min, &max);
if (args > 0) {
id = CHANNEL_ID(c);
if (id >= CHANNELS)
NOK();
else if (args == 1) {
channel = conf_get(id);
printf("%c %u %u %i %i\n", c, channel->num, channel->gain, channel->min, channel->max);
}
else {
int tmp;
input_stop();
channel = conf_get(id);
tmp = channel->gain;
channel->num = limit(num, 0, AD_CHANNELS);
if (args >= 3) {
channel->gain = limit(gain, AD_GAIN_MIN, AD_GAIN_MAX);
if (tmp != channel->gain)
input_calibrate(id);
}
if (args == 4) {
max = limit(min, 0, AD_VMAX);
min = limit(-min, AD_VMIN, 0);
channel->min = min;
channel->max = max;
}
else if (args >= 5) {
channel->min = limit(min, AD_VMIN, AD_VMAX);
channel->max = limit(max, channel->min, AD_VMAX);
}
printf("ok %c %u %u %i %i\n", c, channel->num, channel->gain, channel->min, channel->max);
input_start();
}
}
else NOK();
}
static void do_info()
{
char c;
int i, tmp;
struct chan *channel;
input_stop();
if ((c = getchar()) == 'V') {
tmp = AD7793_voltmon();
printf("%u.%uV\n", tmp/1000000, tmp%1000000);
}
else if (c == 'T') {
tmp = AD7793_temperature();
printf("%u.%uC\n", tmp/10000, tmp%10000);
}
else {
if ((i = CHANNEL_ID(c)) < CHANNELS) // print only one channel
tmp = i+1;
else { // print all channels and state information
i = 0;
tmp = CHANNELS;
printf("%s\n", STATE_NAME(state_getState()));
}
while (i < tmp) {
channel = conf_get(i);
printf("%c %s ch%u %ux %i ... %i %iuV\n",
CHANNEL_NAME(i), ERROR_NAME(state_getError(i)),
channel->num, 1<<channel->gain,
channel->min, channel->max,
input_latest(i));
i++;
}
}
input_start();
}
void MotionGraphController::readInMotionSequences(MotionDataSpecification& motion_data_specs)
{
for (unsigned short i = 0; i<motion_data_specs.size(); i++)
{
string current_file = motion_data_specs.getBvhFilename(i);
cout << "MotionGraphController reading " << current_file << endl;
logout << "MotionGraphController reading " << current_file << endl;
char* BVH_filename = NULL;
string character_BVH2(current_file);
try
{
BVH_filename = data_manager.findFile(character_BVH2.c_str());
if (BVH_filename == NULL)
{
stringstream ss;
ss << "MotionGraphController::readInMotionSequences: Unable to find character BVH file <" << character_BVH2 << ">. Aborting load.";
logout << ss.str() << endl;
throw AppException(ss.str().c_str());
}
pair<Skeleton*, MotionSequence*> read_result;
try
{
read_result = data_manager.readBVH(BVH_filename);
}
catch (const DataManagementException& dme)
{
stringstream ss;
ss << "MotionGraphController::readInMotionSequences: Aborting due to DataManager expection: " << dme.msg;
logout << ss.str();
throw AppException(ss.str().c_str());
}
// throw away the skeleton
delete read_result.first;
MotionSequence * ms = read_result.second;
string seqID = motion_data_specs.getSeqID(i);
char* tmp = new char[strlen(seqID.c_str())+1];
strcpy(tmp, seqID.c_str());
ms->setId(tmp);
delete [] tmp;
//scale the character position in the motion sequence
ms->scaleChannel(CHANNEL_ID(0, CT_TX), character_size_scale);
ms->scaleChannel(CHANNEL_ID(0, CT_TY), character_size_scale);
ms->scaleChannel(CHANNEL_ID(0, CT_TZ), character_size_scale);
motion_sequence_map.insert(pair<string, MotionSequence*>(seqID, ms));
}
catch (BasicException& e)
{
stringstream ss;
ss << "MotionGraphController::readInMotionSequences: Aborting due to BasicException: " << e.msg;
logout << ss.str();
throw AppException(ss.str().c_str());
}
}
logout << "the number of motion sequences is : " << motion_sequence_map.size() << endl;
logout << "... MotionGraphController::readInMotionSequences finished" << endl;
}
void MotionGraphController::readInMotionSequences()
{
cout << "reading motion Sequences" << endl;
namespace fs = ::boost::filesystem;
fs::path p(BVH_MOTION_FILE_PATHMOTIONS);
if (!exists(p)) // does p actually exist?
cout << "doesn't exist" << endl;
fs::directory_iterator end_itr;
// cycle through the directory
for (fs::directory_iterator itr(p); itr != end_itr; ++itr)
{
// If it's not a directory, list it. If you want to list directories too, just remove this check.
if (is_regular_file(itr->path())) {
// assign current file name to current_file and echo it out to the console.
string current_file = itr->path().string();
current_file = itr->path().filename().string();
cout << current_file << endl;
DataManager dataman;
dataman.addFileSearchPath(BVH_MOTION_FILE_PATHMOTIONS);
char* BVH_filename = NULL;
string character_BVH2(current_file);
try
{
BVH_filename = dataman.findFile(character_BVH2.c_str());
if (BVH_filename == NULL)
{
logout << "AnimationControl::loadCharacters: Unable to find character BVH file <" << character_BVH2 << ">. Aborting load." << endl;
throw BasicException("ABORT");
}
pair<Skeleton*, MotionSequence*> read_result;
try
{
read_result = data_manager.readBVH(BVH_filename);
}
catch (const DataManagementException& dme)
{
logout << "AnimationControl::loadCharacters: Unable to load character data files. Aborting load." << endl;
logout << " Failure due to " << dme.msg << endl;
throw BasicException("ABORT");
}
Skeleton* skel = read_result.first;
MotionSequence * ms = read_result.second;
std::string x = current_file;
char *y = new char[x.length() + 1];
std::strcpy(y, x.c_str());
//set the ID aka filename
ms->setId(y);
delete[] y;
//scale each motion sequence once
ms->scaleChannel(CHANNEL_ID(0, CT_TX), character_size_scale);
ms->scaleChannel(CHANNEL_ID(0, CT_TY), character_size_scale);
ms->scaleChannel(CHANNEL_ID(0, CT_TZ), character_size_scale);
MotionSequenceContainer test;
test.MS = ms;
test.SeqID = current_file;
MsVector.push_back(test);
cout << "done loading: "<<current_file << MsVector.size() << endl;
}
catch (BasicException& e) { cout << e.msg << endl; }
}
}
cout << "the size of the vector is : " << MsVector.size() << endl;
}
float MotionSequenceController::getValue(CHANNEL_ID _channel, float _time)
{
if (motion_sequence == NULL)
throw AnimationException("MotionSequenceController has no attached MotionSequence");
if (!isValidChannel(_channel, _time))
{
string s = string("MotionSequenceController received request for invalid channel ")
+ " bone: " + toString(_channel.bone_id) + " dof: " + toString(_channel.channel_type);
throw AnimationException(s.c_str());
}
float duration = motion_sequence->getDuration();
long cycles = long(_time / duration);
sequence_time = _time - duration*cycles;
if (sequence_time > duration) sequence_time = 0.0f;
int frame = int(motion_sequence->numFrames()*sequence_time/duration);
float value = motion_sequence->getValue(_channel, frame);
if (apply_start_offset)
{
// modify root based on offsets.
if (_channel.bone_id == 0)
{
if (!frame_is_cached[frame])
{
Vector3D frame_position(
motion_sequence->getValue(CHANNEL_ID(0,CT_TX), frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_TY), frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_TZ), frame));
Vector3D frame_orientation(
motion_sequence->getValue(CHANNEL_ID(0,CT_RX), frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_RY), frame),
motion_sequence->getValue(CHANNEL_ID(0,CT_RZ), frame));
frame_position = start_offset_combined_transform * frame_position;
Matrix4x4 orig_orientation_transform =
Matrix4x4::rotationRPY(frame_orientation.roll, frame_orientation.pitch, frame_orientation.yaw);
Matrix4x4 new_orientation_transform = start_offset_rotation_transform*orig_orientation_transform;
float p, y, r;
// invalid rotation concatenation, Euler angles don't add properly
p = frame_orientation.pitch + start_offset_rotation.pitch;
y = frame_orientation.yaw + start_offset_rotation.yaw;
r = frame_orientation.roll + start_offset_rotation.roll;
//logout << "Frame " << frame << endl;
//logout << "\tframe orientation: " << frame_orientation << endl;
//logout << "\toffset rotation: " << start_offset_rotation << endl;
frame_orientation = Vector3D(p,y,r);
//logout << "\tnew orientation: " << frame_orientation << endl;
root_cache.set(frame,0,frame_position.x);
root_cache.set(frame,1,frame_position.y);
root_cache.set(frame,2,frame_position.z);
root_cache.set(frame,3,frame_orientation.pitch);
root_cache.set(frame,4,frame_orientation.yaw);
root_cache.set(frame,5,frame_orientation.roll);
frame_is_cached[frame] = true;
}
switch(_channel.channel_type)
{
case CT_TX:
value = root_cache.get(frame,0); break;
case CT_TY:
value = root_cache.get(frame,1); break;
case CT_TZ:
value = root_cache.get(frame,2); break;
case CT_RX:
value = root_cache.get(frame,3); break;
case CT_RY:
value = root_cache.get(frame,4); break;
case CT_RZ:
value = root_cache.get(frame,5); break;
default:
value = 0.0f; break;
}
}
}
return value;
}
