// Override featherstonePassOne since don't need to do everything...
public void featherstonePassOne(Vector3d w_h, Vector3d v_h, Matrix3d Rh_0) {
owner.update();
owner.jointTransform3D.get(Ri_0);
// this.jointDependentSetAndGetRotation(Ri_0);
Ri_0.transpose();
Rh_i = null;
Ri_h = null;
d_i = null;
u_iXd_i = null;
r_i = null;
// r_h = null;
// Now do the joint dependent stuff...
this.jointDependentFeatherstonePassOne();
// Now update the points attached to the joint:
R0_i.set(Ri_0);
R0_i.transpose();
// +++JEP OPTIMIZE
if (groundContactPointGroups != null) {
Collection<GroundContactPointGroup> groups = groundContactPointGroups.values();
for (GroundContactPointGroup groundContactPointGroup : groups) {
ArrayList<GroundContactPoint> groundContactPoints =
groundContactPointGroup.getGroundContactPointsInContact();
for (int i = 0; i < groundContactPoints.size(); i++) {
GroundContactPoint point = groundContactPoints.get(i);
point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
}
}
}
if (kinematicPoints != null) {
for (int i = 0; i < kinematicPoints.size(); i++) {
KinematicPoint point = kinematicPoints.get(i);
point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
}
}
// Recurse over the children:
for (int i = 0; i < owner.childrenJoints.size(); i++) {
Joint child = (Joint) owner.childrenJoints.get(i);
child.physics.featherstonePassOne(w_i, v_i, Ri_0);
}
}
protected void jointDependentFeatherstonePassOne() {
// No torques:
Q_i = 0.0;
// Are Holding on to the velocities as part of the state:
// But we need to rotate them into joint coordinates!!
w_i.set(
owner.qd_wx.getDoubleValue(), owner.qd_wy.getDoubleValue(), owner.qd_wz.getDoubleValue());
v_i.set(owner.qd_x.getDoubleValue(), owner.qd_y.getDoubleValue(), owner.qd_z.getDoubleValue());
// Ri_0.transform(w_i); // w and wd not in world coords. Only x,y,z are
Ri_0.transform(v_i);
// ///////////////////////////////
// +++JEP 10/17/01. Transform v_i to be at com, not the joint location...
wXr1.cross(w_i, owner.link.comOffset);
v_i.add(wXr1);
// ///////////////////////////////
}
