Examples with AxisAngle maspack.matrix.AxisAngle used on opensource projects

Example 36 with AxisAngle

use of maspack.matrix.AxisAngle in project artisynth_core by artisynth.

the class RotAxisFrameMaterial method computeF.

public void computeF(Wrench wr, RigidTransform3d X21, Twist vel21, RigidTransform3d initialX21) {
    Vector3d p = X21.p;
    wr.f.x = myK.x * p.x;
    wr.f.y = myK.y * p.y;
    wr.f.z = myK.z * p.z;
    AxisAngle axisAng = new AxisAngle();
    X21.R.getAxisAngle(axisAng);
    wr.m.scale(myRotK * axisAng.angle, axisAng.axis);
    Vector3d v = vel21.v;
    Vector3d w = vel21.w;
    wr.f.x += myD.x * v.x;
    wr.f.y += myD.y * v.y;
    wr.f.z += myD.z * v.z;
    wr.m.scaledAdd(myRotD, w);
}

Example 37 with AxisAngle

use of maspack.matrix.AxisAngle in project artisynth_core by artisynth.

the class FemMuscleArm method addFemMuscle.

public void addFemMuscle(Point3d upper, Point3d lower) throws IOException {
    upperArm = model.rigidBodies().get("upper");
    RigidBody lowerArm = model.rigidBodies().get("lower");
    if (upperArm == null || lowerArm == null) {
        return;
    }
    muscle = new PointToPointMuscle("muscle", 1.0, 0.85, "muscle", true);
    model.addModel(muscle);
    RigidTransform3d X = new RigidTransform3d();
    // Point3d upper = new Point3d(2.4, 0, 20), lower = new Point3d(3, 0, -7);
    FemNode3d first = muscle.getFirstNode(), last = muscle.getLastNode();
    double scale = (upper.distance(lower) / first.getPosition().distance(last.getPosition()));
    System.out.println(scale);
    AffineTransform3d scaling = new AffineTransform3d();
    scaling.setIdentity();
    scaling.applyScaling(scale, scale, scale);
    // muscle.transformGeometry(scaling);
    Vector3d translate = new Vector3d();
    translate.sub(upper, first.getPosition());
    muscle.transformGeometry(new RigidTransform3d(translate, new AxisAngle()));
    Vector3d offfem = new Vector3d(), offtarget = new Vector3d();
    offfem.sub(last.getPosition(), first.getPosition());
    offtarget.sub(lower, upper);
    double angle = offfem.angle(offtarget);
    offfem.cross(offtarget);
    muscle.transformGeometry(new RigidTransform3d(new Vector3d(-upper.x, -upper.y, -upper.z), new AxisAngle()));
    muscle.transformGeometry(new RigidTransform3d(upper, new AxisAngle(offfem, angle)));
    muscle.getFirstNode().setDynamic(false);
    // ant.getFirstNode().setPosition(new Point3d(2.381, 0.000, 20));
    // model.AttachPoint(ant.getFirstNode(),upperArm,new Point3d(-size.x,0,-(size.z/2.0)/1.2));
    model.attachPoint(muscle.getLastNode(), lowerArm);
    muscle.setMaxForce(300000);
    muscle.setExcitation(0.0);
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setFaceColor(Color.RED);
    rp.setLineStyle(Renderer.LineStyle.LINE);
    rp.setPointStyle(Renderer.PointStyle.POINT);
    rp.setShading(Renderer.Shading.SMOOTH);
    rp.setLineColor(Color.WHITE);
    muscle.setRenderProps(rp);
    muscle.setSurfaceRendering(SurfaceRender.Shaded);
}

Example 38 with AxisAngle

use of maspack.matrix.AxisAngle in project artisynth_core by artisynth.

the class RootModel method setDefaultValues.

@Override
public void setDefaultValues() {
    super.setDefaultValues();
    myDefaultViewOrientation = new AxisAngle(DEFAULT_VIEW_ORIENTATION);
}