Panda3D
 All Classes Functions Variables Enumerations
movingPartMatrix.cxx
00001 // Filename: movingPartMatrix.cxx
00002 // Created by:  drose (23Feb99)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 
00016 #include "movingPartMatrix.h"
00017 #include "animChannelMatrixDynamic.h"
00018 #include "animChannelMatrixFixed.h"
00019 #include "compose_matrix.h"
00020 #include "datagram.h"
00021 #include "datagramIterator.h"
00022 #include "bamReader.h"
00023 #include "bamWriter.h"
00024 
00025 // Tell GCC that we'll take care of the instantiation explicitly here.
00026 #ifdef __GNUC__
00027 #pragma implementation
00028 #endif
00029 
00030 TypeHandle MovingPartMatrix::_type_handle;
00031 
00032 ////////////////////////////////////////////////////////////////////
00033 //     Function: MovingPartMatrix::Destructor
00034 //       Access: Public, Virtual
00035 //  Description: 
00036 ////////////////////////////////////////////////////////////////////
00037 MovingPartMatrix::
00038 ~MovingPartMatrix() {
00039 }
00040 
00041 
00042 ////////////////////////////////////////////////////////////////////
00043 //     Function: MovingPartMatrix::make_default_channel
00044 //       Access: Public, Virtual
00045 //  Description: Creates and returns a new AnimChannel that is not
00046 //               part of any hierarchy, but that returns the default
00047 //               value associated with this part.
00048 ////////////////////////////////////////////////////////////////////
00049 AnimChannelBase *MovingPartMatrix::
00050 make_default_channel() const {
00051   LVecBase3 pos, hpr, scale, shear;
00052   decompose_matrix(_default_value, pos, hpr, scale, shear);
00053   return new AnimChannelMatrixFixed(get_name(), pos, hpr, scale);
00054 }
00055 
00056 ////////////////////////////////////////////////////////////////////
00057 //     Function: MovingPartMatrix::get_blend_value
00058 //       Access: Public
00059 //  Description: Attempts to blend the various matrix values
00060 //               indicated, and sets the _value member to the
00061 //               resulting matrix.
00062 ////////////////////////////////////////////////////////////////////
00063 void MovingPartMatrix::
00064 get_blend_value(const PartBundle *root) {
00065   // If a forced channel is set on this particular joint, we always
00066   // return that value instead of performing the blend.  Furthermore,
00067   // the frame number is always 0 for the forced channel.
00068   if (_forced_channel != (AnimChannelBase *)NULL) {
00069     ChannelType *channel = DCAST(ChannelType, _forced_channel);
00070     channel->get_value(0, _value);
00071     return;
00072   }
00073 
00074   PartBundle::CDReader cdata(root->_cycler);
00075 
00076   if (cdata->_blend.empty()) {
00077     // No channel is bound; supply the default value.
00078     if (restore_initial_pose) {
00079       _value = _default_value;
00080     }
00081 
00082   } else if (_effective_control != (AnimControl *)NULL && 
00083              !cdata->_frame_blend_flag) {
00084     // A single value, the normal case.
00085     ChannelType *channel = DCAST(ChannelType, _effective_channel);
00086     channel->get_value(_effective_control->get_frame(), _value);
00087 
00088   } else {
00089     // A blend of two or more values, either between multiple
00090     // different animations, or between consecutive frames of the same
00091     // animation (or both).
00092     switch (cdata->_blend_type) {
00093     case PartBundle::BT_linear:
00094       {
00095         // An ordinary, linear blend.
00096         LMatrix4 net_value = LMatrix4::zeros_mat();
00097         PN_stdfloat net_effect = 0.0f;
00098         
00099         PartBundle::ChannelBlend::const_iterator cbi;
00100         for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
00101           AnimControl *control = (*cbi).first;
00102           PN_stdfloat effect = (*cbi).second;
00103           nassertv(effect != 0.0f);
00104           
00105           int channel_index = control->get_channel_index();
00106           nassertv(channel_index >= 0 && channel_index < (int)_channels.size());
00107           ChannelType *channel = DCAST(ChannelType, _channels[channel_index]);
00108           if (channel != (ChannelType *)NULL) {
00109             ValueType v;
00110             channel->get_value(control->get_frame(), v);
00111 
00112             if (!cdata->_frame_blend_flag) {
00113               // Hold the current frame until the next one is ready.
00114               net_value += v * effect;
00115             } else {
00116               // Blend between successive frames.
00117               PN_stdfloat frac = (PN_stdfloat)control->get_frac();
00118               net_value += v * (effect * (1.0f - frac));
00119 
00120               channel->get_value(control->get_next_frame(), v);
00121               net_value += v * (effect * frac);
00122             }
00123             net_effect += effect;
00124           }
00125         }
00126         
00127         if (net_effect == 0.0f) {
00128           if (restore_initial_pose) {
00129             _value = _default_value;
00130           }
00131         } else {
00132           _value = net_value / net_effect;
00133         }
00134       }
00135       break;
00136 
00137     case PartBundle::BT_normalized_linear:
00138       {
00139         // A normalized linear blend.  This means we do a linear blend
00140         // without scales or shears, normalize the scale and shear
00141         // components of the resulting matrix to eliminate
00142         // artificially-introduced scales, and then reapply the
00143         // scales and shears.
00144         
00145         LMatrix4 net_value = LMatrix4::zeros_mat();
00146         LVecBase3 scale(0.0f, 0.0f, 0.0f);
00147         LVecBase3 shear(0.0f, 0.0f, 0.0f);
00148         PN_stdfloat net_effect = 0.0f;
00149         
00150         PartBundle::ChannelBlend::const_iterator cbi;
00151         for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
00152           AnimControl *control = (*cbi).first;
00153           PN_stdfloat effect = (*cbi).second;
00154           nassertv(effect != 0.0f);
00155           
00156           ChannelType *channel = NULL;
00157           int channel_index = control->get_channel_index();
00158           if (channel_index >= 0 && channel_index < (int)_channels.size()) {
00159             channel = DCAST(ChannelType, _channels[channel_index]);
00160           }
00161           if (channel != (ChannelType *)NULL) {
00162             int frame = control->get_frame();
00163             ValueType v;
00164             LVecBase3 iscale, ishear;
00165             channel->get_value_no_scale_shear(frame, v);
00166             channel->get_scale(frame, iscale);
00167             channel->get_shear(frame, ishear);
00168             
00169             if (!cdata->_frame_blend_flag) {
00170               // Hold the current frame until the next one is ready.
00171               net_value += v * effect;
00172               scale += iscale * effect;
00173               shear += ishear * effect;
00174             } else {
00175               // Blend between successive frames.
00176               PN_stdfloat frac = (PN_stdfloat)control->get_frac();
00177               PN_stdfloat e0 = effect * (1.0f - frac);
00178               net_value += v * e0;
00179               scale += iscale * e0;
00180               shear += ishear * e0;
00181 
00182               int next_frame = control->get_next_frame();
00183               channel->get_value_no_scale_shear(next_frame, v);
00184               channel->get_scale(next_frame, iscale);
00185               channel->get_shear(next_frame, ishear);
00186               PN_stdfloat e1 = effect * frac;
00187               net_value += v * e1;
00188               scale += iscale * e1;
00189               shear += ishear * e1;
00190             }
00191             net_effect += effect;
00192           }
00193         }
00194         
00195         if (net_effect == 0.0f) {
00196           if (restore_initial_pose) {
00197             _value = _default_value;
00198           }
00199 
00200         } else {
00201           net_value /= net_effect;
00202           scale /= net_effect;
00203           shear /= net_effect;
00204           
00205           // Now rebuild the matrix with the correct scale values.
00206           
00207           LVector3 false_scale, false_shear, hpr, translate;
00208           decompose_matrix(net_value, false_scale, false_shear, hpr, translate);
00209           compose_matrix(_value, scale, shear, hpr, translate);
00210         }
00211       }
00212       break;
00213 
00214     case PartBundle::BT_componentwise:
00215       {
00216         // Componentwise linear, including componentwise H, P, and R.
00217         LVecBase3 scale(0.0f, 0.0f, 0.0f);
00218         LVecBase3 hpr(0.0f, 0.0f, 0.0f);
00219         LVecBase3 pos(0.0f, 0.0f, 0.0f);
00220         LVecBase3 shear(0.0f, 0.0f, 0.0f);
00221         PN_stdfloat net_effect = 0.0f;
00222         
00223         PartBundle::ChannelBlend::const_iterator cbi;
00224         for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
00225           AnimControl *control = (*cbi).first;
00226           PN_stdfloat effect = (*cbi).second;
00227           nassertv(effect != 0.0f);
00228           
00229           ChannelType *channel = NULL;
00230           int channel_index = control->get_channel_index();
00231           if (channel_index >= 0 && channel_index < (int)_channels.size()) {
00232             channel = DCAST(ChannelType, _channels[channel_index]);
00233           }
00234           if (channel != (ChannelType *)NULL) {
00235             int frame = control->get_frame();
00236             LVecBase3 iscale, ihpr, ipos, ishear;
00237             channel->get_scale(frame, iscale);
00238             channel->get_hpr(frame, ihpr);
00239             channel->get_pos(frame, ipos);
00240             channel->get_shear(frame, ishear);
00241             
00242             if (!cdata->_frame_blend_flag) {
00243               // Hold the current frame until the next one is ready.
00244               scale += iscale * effect;
00245               hpr += ihpr * effect;
00246               pos += ipos * effect;
00247               shear += ishear * effect;
00248             } else {
00249               // Blend between successive frames.
00250               PN_stdfloat frac = (PN_stdfloat)control->get_frac();
00251               PN_stdfloat e0 = effect * (1.0f - frac);
00252 
00253               scale += iscale * e0;
00254               hpr += ihpr * e0;
00255               pos += ipos * e0;
00256               shear += ishear * e0;
00257 
00258               int next_frame = control->get_next_frame();
00259               channel->get_scale(next_frame, iscale);
00260               channel->get_hpr(next_frame, ihpr);
00261               channel->get_pos(next_frame, ipos);
00262               channel->get_shear(next_frame, ishear);
00263               PN_stdfloat e1 = effect * frac;
00264 
00265               scale += iscale * e1;
00266               hpr += ihpr * e1;
00267               pos += ipos * e1;
00268               shear += ishear * e1;
00269             }
00270             net_effect += effect;
00271           }
00272         }
00273         
00274         if (net_effect == 0.0f) {
00275           if (restore_initial_pose) {
00276             _value = _default_value;
00277           }
00278 
00279         } else {
00280           scale /= net_effect;
00281           hpr /= net_effect;
00282           pos /= net_effect;
00283           shear /= net_effect;
00284           
00285           compose_matrix(_value, scale, shear, hpr, pos);
00286         }
00287       }
00288       break;
00289 
00290     case PartBundle::BT_componentwise_quat:
00291       {
00292         // Componentwise linear, except for rotation, which is a
00293         // quaternion.
00294         LVecBase3 scale(0.0f, 0.0f, 0.0f);
00295         LQuaternion quat(0.0f, 0.0f, 0.0f, 0.0f);
00296         LVecBase3 pos(0.0f, 0.0f, 0.0f);
00297         LVecBase3 shear(0.0f, 0.0f, 0.0f);
00298         PN_stdfloat net_effect = 0.0f;
00299         
00300         PartBundle::ChannelBlend::const_iterator cbi;
00301         for (cbi = cdata->_blend.begin(); cbi != cdata->_blend.end(); ++cbi) {
00302           AnimControl *control = (*cbi).first;
00303           PN_stdfloat effect = (*cbi).second;
00304           nassertv(effect != 0.0f);
00305           
00306           ChannelType *channel = NULL;
00307           int channel_index = control->get_channel_index();
00308           if (channel_index >= 0 && channel_index < (int)_channels.size()) {
00309             channel = DCAST(ChannelType, _channels[channel_index]);
00310           }
00311           if (channel != (ChannelType *)NULL) {
00312             int frame = control->get_frame();
00313             LVecBase3 iscale, ipos, ishear;
00314             LQuaternion iquat;
00315             channel->get_scale(frame, iscale);
00316             channel->get_quat(frame, iquat);
00317             channel->get_pos(frame, ipos);
00318             channel->get_shear(frame, ishear);
00319             
00320             if (!cdata->_frame_blend_flag) {
00321               // Hold the current frame until the next one is ready.
00322               scale += iscale * effect;
00323               quat += iquat * effect;
00324               pos += ipos * effect;
00325               shear += ishear * effect;
00326 
00327             } else {
00328               // Blend between successive frames.
00329               PN_stdfloat frac = (PN_stdfloat)control->get_frac();
00330               PN_stdfloat e0 = effect * (1.0f - frac);
00331 
00332               scale += iscale * e0;
00333               quat += iquat * e0;
00334               pos += ipos * e0;
00335               shear += ishear * e0;
00336 
00337               int next_frame = control->get_next_frame();
00338               channel->get_scale(next_frame, iscale);
00339               channel->get_quat(next_frame, iquat);
00340               channel->get_pos(next_frame, ipos);
00341               channel->get_shear(next_frame, ishear);
00342               PN_stdfloat e1 = effect * frac;
00343 
00344               scale += iscale * e1;
00345               quat += iquat * e1;
00346               pos += ipos * e1;
00347               shear += ishear * e1;
00348             }
00349             net_effect += effect;
00350           }
00351         }
00352         
00353         if (net_effect == 0.0f) {
00354           if (restore_initial_pose) {
00355             _value = _default_value;
00356           }
00357 
00358         } else {
00359           scale /= net_effect;
00360           quat /= net_effect;
00361           pos /= net_effect;
00362           shear /= net_effect;
00363           
00364           // There should be no need to normalize the quaternion,
00365           // assuming all of the input quaternions were already
00366           // normalized.
00367           
00368           _value = LMatrix4::scale_shear_mat(scale, shear) * quat;
00369           _value.set_row(3, pos);
00370         }
00371       }
00372       break;
00373     }
00374   }
00375 }
00376 
00377 ////////////////////////////////////////////////////////////////////
00378 //     Function: MovingPartMatrix::apply_freeze_matrix
00379 //       Access: Public, Virtual
00380 //  Description: Freezes this particular joint so that it will always
00381 //               hold the specified transform.  Returns true if this
00382 //               is a joint that can be so frozen, false otherwise.
00383 //               This is called internally by
00384 //               PartBundle::freeze_joint().
00385 ////////////////////////////////////////////////////////////////////
00386 bool MovingPartMatrix::
00387 apply_freeze_matrix(const LVecBase3 &pos, const LVecBase3 &hpr, const LVecBase3 &scale) {
00388   _forced_channel = new AnimChannelMatrixFixed(get_name(), pos, hpr, scale);
00389   return true;
00390 }
00391 
00392 ////////////////////////////////////////////////////////////////////
00393 //     Function: MovingPartMatrix::apply_control
00394 //       Access: Public, Virtual
00395 //  Description: Specifies a node to influence this particular joint
00396 //               so that it will always hold the node's transform.
00397 //               Returns true if this is a joint that can be so
00398 //               controlled, false otherwise.  This is called
00399 //               internally by PartBundle::control_joint().
00400 ////////////////////////////////////////////////////////////////////
00401 bool MovingPartMatrix::
00402 apply_control(PandaNode *node) {
00403   AnimChannelMatrixDynamic *chan = new AnimChannelMatrixDynamic(get_name());
00404   chan->set_value_node(node);
00405   _forced_channel = chan;
00406   return true;
00407 }
00408 
00409 ////////////////////////////////////////////////////////////////////
00410 //     Function: MovingPartMatrix::make_MovingPartMatrix
00411 //       Access: Protected
00412 //  Description: Factory method to generate a MovingPartMatrix object
00413 ////////////////////////////////////////////////////////////////////
00414 TypedWritable* MovingPartMatrix::
00415 make_MovingPartMatrix(const FactoryParams &params) {
00416   MovingPartMatrix *me = new MovingPartMatrix;
00417   DatagramIterator scan;
00418   BamReader *manager;
00419 
00420   parse_params(params, scan, manager);
00421   me->fillin(scan, manager);
00422   return me;
00423 }
00424 
00425 ////////////////////////////////////////////////////////////////////
00426 //     Function: MovingPartMatrix::register_with_factory
00427 //       Access: Public, Static
00428 //  Description: Factory method to generate a MovingPartMatrix object
00429 ////////////////////////////////////////////////////////////////////
00430 void MovingPartMatrix::
00431 register_with_read_factory() {
00432   BamReader::get_factory()->register_factory(get_class_type(), make_MovingPartMatrix);
00433 }
 All Classes Functions Variables Enumerations