georegression.transform.twist.TwistCoordinate_F64 Maven / Gradle / Ivy

Show more of this group Show more artifacts with this name
Show all versions of georegression Show documentation

GeoRegression is a free Java based geometry library for scientific computing in fields such as robotics and computer vision with a focus on 2D/3D space.

There is a newer version: 0.27.1

Show newest version

/*
 * Copyright (C) 2020, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.transform.twist;

import georegression.struct.point.Vector3D_F64;

/**
 * Representation of a twist coordinate, which is se(3).  The exponential is SE(3) or rigid body.  A twist
 * can be interpreted as a mapping of points from their initial coordinate to another coordinate after the
 * rigid motion has been applied [1].
 *
 * 
 * p(t) = exp(hat{ξ}*t)*p(0)

 * p'(t) = w × (p(t) -q)

 * v = -w × q

 * where ||w|| = 1, q in Re³ is a point on the axis.  The point in a body undergoing a constant
 * screw motion which traces helices.
 * 
 *
 * 
 * [1] R. Murray, et. al. "A Mathematical Introduction to ROBOTIC MANIPULATION" 1994
 * 
 *
 * @author Peter Abeles
 */
public class TwistCoordinate_F64 {
	/**
	 * Angular component in so(3).   Does NOT need to be normalized to 1.
	 */
	public Vector3D_F64 w = new Vector3D_F64();
	/**
	 * Translational component. Re³
	 */
	public Vector3D_F64 v = new Vector3D_F64();

	public TwistCoordinate_F64() {
	}

	public TwistCoordinate_F64( double wx, double wy , double wz, double vx , double vy, double vz ) {
		set(wx,wy,wz, vx,vy,vz);
	}

	public TwistCoordinate_F64(Vector3D_F64 w, Vector3D_F64 v) {
		this.w.setTo(w);
		this.v.setTo(v);
	}

	public void set( double wx, double wy , double wz, double vx , double vy, double vz ) {
		this.w.setTo(wx,wy,wz);
		this.v.setTo(vx,vy,vz);
	}

	public void set( TwistCoordinate_F64 original ) {
		this.w.setTo( original.w );
		this.v.setTo( original.v );
	}

	public void print() {
		System.out.println(toString());
	}

	@Override
	public String toString() {
		return getClass().getSimpleName()+"{" +
				"w=" + w +
				", v=" + v +
				'}';
	}
}