00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "baseIntegrator.h"
00016 #include "physicalNode.h"
00017 #include "forceNode.h"
00018 #include "nodePath.h"
00019
00020
00021
00022
00023
00024
00025 BaseIntegrator::
00026 BaseIntegrator() {
00027 }
00028
00029
00030
00031
00032
00033
00034 BaseIntegrator::
00035 ~BaseIntegrator() {
00036 }
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 void BaseIntegrator::
00047 precompute_linear_matrices(Physical *physical,
00048 const LinearForceVector &forces) {
00049 nassertv(physical);
00050
00051 PhysicalNode *physical_node = physical->get_physical_node();
00052 nassertv(physical_node);
00053
00054
00055 int global_force_vec_size = forces.size();
00056
00057
00058 int local_force_vec_size = physical->get_linear_forces().size();
00059
00060 ForceNode *force_node;
00061
00062
00063 _precomputed_linear_matrices.clear();
00064 _precomputed_linear_matrices.reserve(
00065 global_force_vec_size + local_force_vec_size);
00066
00067 NodePath physical_np(physical->get_physical_node_path());
00068 NodePath parent_physical_np = physical_np.get_parent();
00069
00070
00071 LinearForceVector::const_iterator fi;
00072 for (fi = forces.begin(); fi != forces.end(); ++fi) {
00073
00074 force_node = (*fi)->get_force_node();
00075 nassertv(force_node != (ForceNode *) NULL);
00076
00077 NodePath force_np = (*fi)->get_force_node_path();
00078 _precomputed_linear_matrices.push_back(
00079 force_np.get_transform(parent_physical_np)->get_mat());
00080 }
00081
00082
00083 const LinearForceVector &force_vector = physical->get_linear_forces();
00084 for (fi = force_vector.begin(); fi != force_vector.end(); ++fi) {
00085 force_node = (*fi)->get_force_node();
00086 nassertv(force_node != (ForceNode *) NULL);
00087
00088 NodePath force_np = (*fi)->get_force_node_path();
00089 _precomputed_linear_matrices.push_back(
00090 force_np.get_transform(parent_physical_np)->get_mat());
00091 }
00092 }
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 void BaseIntegrator::
00103 precompute_angular_matrices(Physical *physical,
00104 const AngularForceVector &forces) {
00105 nassertv(physical);
00106
00107 PhysicalNode *physical_node = physical->get_physical_node();
00108 nassertv(physical_node != NULL);
00109
00110
00111 int global_force_vec_size = forces.size();
00112
00113
00114 int local_force_vec_size = physical->get_angular_forces().size();
00115
00116 ForceNode *force_node;
00117
00118
00119 _precomputed_angular_matrices.clear();
00120 _precomputed_angular_matrices.reserve(
00121 global_force_vec_size + local_force_vec_size);
00122
00123 NodePath physical_np(physical->get_physical_node_path());
00124 NodePath parent_physical_np = physical_np.get_parent();
00125
00126
00127 AngularForceVector::const_iterator fi;
00128 for (fi = forces.begin(); fi != forces.end(); ++fi) {
00129 force_node = (*fi)->get_force_node();
00130 nassertv(force_node != (ForceNode *) NULL);
00131
00132 NodePath force_np = (*fi)->get_force_node_path();
00133 _precomputed_angular_matrices.push_back(
00134 force_np.get_transform(parent_physical_np)->get_mat());
00135 }
00136
00137
00138 const AngularForceVector &force_vector = physical->get_angular_forces();
00139 for (fi = force_vector.begin(); fi != force_vector.end(); ++fi) {
00140 force_node = (*fi)->get_force_node();
00141 nassertv(force_node != (ForceNode *) NULL);
00142
00143 NodePath force_np = (*fi)->get_force_node_path();
00144 _precomputed_angular_matrices.push_back(
00145 force_np.get_transform(parent_physical_np)->get_mat());
00146 }
00147 }
00148
00149
00150
00151
00152
00153
00154
00155 void BaseIntegrator::
00156 output(ostream &out) const {
00157 #ifndef NDEBUG //[
00158 out<<"BaseIntegrator (id "<<this<<")";
00159 #endif //] NDEBUG
00160 }
00161
00162
00163
00164
00165
00166
00167
00168 void BaseIntegrator::
00169 write_precomputed_linear_matrices(ostream &out, unsigned int indent) const {
00170 #ifndef NDEBUG //[
00171 out.width(indent);
00172 out<<""<<"_precomputed_linear_matrices\n";
00173 for (MatrixVector::const_iterator i=_precomputed_linear_matrices.begin();
00174 i != _precomputed_linear_matrices.end();
00175 ++i) {
00176 out.width(indent+2); out<<""; (*i).output(out); out<<"\n";
00177 }
00178 #endif //] NDEBUG
00179 }
00180
00181
00182
00183
00184
00185
00186
00187 void BaseIntegrator::
00188 write_precomputed_angular_matrices(ostream &out, unsigned int indent) const {
00189 #ifndef NDEBUG //[
00190 out.width(indent);
00191 out<<""<<"_precomputed_angular_matrices\n";
00192 for (MatrixVector::const_iterator i=_precomputed_angular_matrices.begin();
00193 i != _precomputed_angular_matrices.end();
00194 ++i) {
00195 out.width(indent+2); out<<""; (*i).output(out); out<<"\n";
00196 }
00197 #endif //] NDEBUG
00198 }
00199
00200
00201
00202
00203
00204
00205
00206 void BaseIntegrator::
00207 write(ostream &out, unsigned int indent) const {
00208 #ifndef NDEBUG //[
00209 out.width(indent); out<<""; out<<"BaseIntegrator:\n";
00210 write_precomputed_linear_matrices(out, indent+2);
00211 write_precomputed_angular_matrices(out, indent+2);
00212 #endif //] NDEBUG
00213 }